Kalibr installation tutorial
I was confused about installing Kalibr, but there is no even one hint in README.md. I just put them in the catkin_ws, in which so many ROS packages are also there. Unsuccessfully, it can't be compiled one by one package by the command catkin_make -DCATKIN_WHITELIST_PACKAGE="PACKAGE_NAME". It means a good choice is to build another ROS workspace in case of rebuilding others in the same workspace.
Resiquite:
sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev ros-kinetic-vision-opencv ros-kinetic-image-transport-plugins ros-kinetic-cmake-modules python-software-properties software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev
sudo pip install python-igraph --upgrade
Warning: If having done catkin_make at first then must run the following command.
catkin clean -bdy
cd ~
mkdir -p kalibr_ws/src
cd ~/kalibr_ws
source /opt/ros/kinetic/setup.bash
catkin init
catkin config --extend /opt/ros/kinetic
catkin config --merge-devel # Necessary for catkin_tools >= 0.4. catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
cd ~/kalibr_ws/src
git clone https://github.com/ethz-asl/kalibr.git
cd ..
catkin build -DCMAKE_BUILD_TYPE=Release -j4
Output seems like this:
Finished <<< kalibr [ 16.1 seconds ]
[build] Summary: All 37 packages succeeded!
[build] Ignored: None.
[build] Warnings: 21 packages succeeded with warnings.
[build] Abandoned: None.
[build] Failed: None.
[build] Runtime: 14 minutes and 53.4 seconds total.
[build] Note: Workspace packages have changed, please re-source setup files to use them.
source ~/kalibr_ws/devel/setup.bash
Update:
Traceback (most recent call last):
File "../python/kalibr_calibrate_cameras", line 6, in
import sm
ImportError: No module named sm
**Solution: **
sudo pip install sm
then rebuild kalibr.
Multiple camera calibration
roslaunch realsense2_camera rs_camera.launch
rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color
rosbag record -O rs_cam_hz4 /color
Which distortiong model should be choose for Realsense D435i? From all I know, a factory calibration setup of D435i looks like: (You can /usr/local/bin/rs-sensor-control, type 0, 1, 2, 91 etc to see)
Principal Point : 322.424, 237.813
Focal Length : 617.521, 617.576
Distortion Model : Brown Conrady
Distortion Coefficients : [0,0,0,0,0]
And according to the dorodnic, of course a equidistant distortion model could be used. (But r1 & r2 are needed in realsense comfig in vins. So the best distortion model must be radial-tangential (radtan))
Yes, these are supposed to be zero for the D400. We consider adding coefficient estimation to the RGB calibration to reduce the distortion (by about 1 pixel at extremes), but at the moment projection without coefficients is the most accurate you can do (we are not calibrating and then ignoring the coefficients, we estimate fx, fy, ppx and ppy without them)
cd ~/kalibr_ws/src/kalibr/aslam_offline_calibration/kalibr/data
../python/kalibr_calibrate_cameras --target april_6x6_50x50cm.yaml --bag rs_cam_hz4.bag --models pinhole-equi --topics /color
Note that in the bag file there are up to 800 images, but it only 39. Maybe that's enough for calibration?
Output:
Calibration complete.
[ WARN] [1556719991.003758]: Removed 26 outlier corners.
Processed 826 images with 39 images used
Camera-system parameters:
cam0 (/color):
type:
distortion: [ 0.3044413 2.04741574 -11.06112629 18.6743852 ] +- [ 0.0320288 0.46759766 2.76374537 5.41971393]
projection: [ 604.9671891 602.10506316 325.8395051 238.35406753] +- [ 10.62286295 10.41921913 1.68531874 1.43868064]
reprojection error: [-0.000000, -0.000000] +- [0.153693, 0.138547]
Results written to file: camchain-rs_cam_hz4.yaml
Detailed results written to file: results-cam-rs_cam_hz4.txt
Result:
camchain-rs_cam_hz4.yaml
cam0:
cam_overlaps: []
camera_model: pinhole
distortion_coeffs: [0.3044413037380324, 2.0474157424478348, -11.061126286843251,
18.67438520203368]
distortion_model: equidistant
intrinsics: [604.9671890973748, 602.1050631617551, 325.83950509989114, 238.35406753467785]
resolution: [640, 480]
rostopic: /color
Compared to the default settings, assumes our result is accurate. The reprojection error seems like good too.
IMU calibration
imu_utils from HKUST
Protecting from error:
CMake Warning at /opt/ros/kinetic/share/catkin/cmake/catkinConfig.cmake:76 (find_package):
Could not find a package configuration file provided by "code_utils" with
any of the following names:
code_utilsConfig.cmake
code_utils-config.cmake
Add the installation prefix of "code_utils" to CMAKE_PREFIX_PATH or set
"code_utils_DIR" to a directory containing one of the above files. If
"code_utils" provides a separate development package or SDK, be sure it has
been installed.
Put code_utils in the workspace, catkin_make first.
Then do the same for imu_utils.
Result (BMI055 is the IMU D435i is using):
BMI055_imu_param.yaml
%YAML:1.0
---
type: IMU
name: BMI055
Gyr:
unit: " rad/s"
avg-axis:
gyr_n: 6.0673370376614875e-03
gyr_w: 3.6211951458325785e-05
x-axis:
gyr_n: 5.4501442406047970e-03
gyr_w: 4.0723401163659986e-05
y-axis:
gyr_n: 5.9380128602687073e-03
gyr_w: 2.9388325769986972e-05
z-axis:
gyr_n: 6.8138540121109601e-03
gyr_w: 3.8524127441330383e-05
Acc:
unit: " m/s^2"
avg-axis:
acc_n: 3.3621979208052800e-02
acc_w: 9.8256589971851467e-04
x-axis:
acc_n: 3.6095477320173631e-02
acc_w: 9.6831827726998488e-04
y-axis:
acc_n: 3.4696437020780901e-02
acc_w: 1.3092042863834673e-03
z-axis:
acc_n: 3.0074023283203882e-02
acc_w: 6.7017513550209160e-04
camera/IMU calibration
roscd realsense2_camera/
roslaunch realsense2_camera rs_camera.launch
rostopic hz /camera/imu
rostopic hz /camera/color/image_raw
rosrun topic_tools throttle messages /camera/color/image_raw 20.0 /color
rosrun topic_tools throttle messages /camera/imu 200.0 /imu
Some problem:
In the rs_camera.launch, but when I check the frequency: IMU is 150 Hz and the camera is 15FPS. It can't be slow down to the frequency needed.
The best frequency is 200 Hz and 30 Hz. Of course, others are still good.
rosbag record -O rs_cam15hz_imu150hz.bag /color /imu
camchain-rs_cam_hz4.yaml
cam0:
cam_overlaps: []
camera_model: pinhole
distortion_coeffs: [0.3044413037380324, 2.0474157424478348, -11.061126286843251,
18.67438520203368]
distortion_model: equidistant
intrinsics: [604.9671890973748, 602.1050631617551, 325.83950509989114, 238.35406753467785]
resolution: [640, 480]
rostopic: /color
imu.yaml
rostopic: /imu
update_rate: 150.0 #Hz
accelerometer_noise_density: 3.3621979208052800e-02 #continous
accelerometer_random_walk: 9.8256589971851467e-04
gyroscope_noise_density: 6.0673370376614875e-03 #continous
gyroscope_random_walk: 3.6211951458325785e-05
roscd kalibr
cd data
cp ~/catkin_ws/src/realsense/realsense2_camera/rs_cam15hz_imu150hz.bag .
../python/kalibr_calibrate_imu_camera --target april_6x6_50x50cm.yaml --cam camchain-rs_cam_hz4.yaml --imu imu-BMI055.yaml --bag rs_cam15hz_imu150hz.bag
Note that when something is wrong with the input data in bagfile, just record another one bagfile.
Initializing
Optimization problem initialized with 101968 design variables and 1079428 error terms
The Jacobian matrix is 2310198 x 458841
[0.0]: J: 1.35165e+06
Exception in thread block: [aslam::Exception] /home/william/kalibr_ws/src/kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.55677e+09 <= 1.55677e+09 < 1.55677e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception] /home/william/kalibr_ws/src/kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.55677e+09 <= 1.55677e+09 < 1.55677e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception] /home/william/kalibr_ws/src/kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.55677e+09 <= 1.55677e+09 < 1.55677e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
[ERROR] [1556773048.921808]: Optimization failed!
Traceback (most recent call last):
File "../python/kalibr_calibrate_imu_camera", line 236, in
main()
File "../python/kalibr_calibrate_imu_camera", line 206, in main
iCal.optimize(maxIterations=parsed.max_iter, recoverCov=parsed.recover_cov)
File "/home/william/kalibr_ws/src/kalibr/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py", line 179, in optimize
raise RuntimeError("Optimization failed!")
RuntimeError: Optimization failed!
Result looks like this:
After Optimization (Results)
==================
Normalized Residuals
----------------------------
Reprojection error (cam0): mean 0.169417479013, median 0.154212672023, std: 0.0973946838993
Gyroscope error (imu0): mean 0.18574054756, median 0.159830346682, std: 0.115913332564
Accelerometer error (imu0): mean 0.169497068217, median 0.145829709726, std: 0.10939033445
Residuals
----------------------------
Reprojection error (cam0) [px]: mean 0.169417479013, median 0.154212672023, std: 0.0973946838993
Gyroscope error (imu0) [rad/s]: mean 0.013802268496, median 0.0118768970357, std: 0.00861345010194
Accelerometer error (imu0) [m/s^2]: mean 0.0697960902289, median 0.0600502633182, std: 0.0450451310679
Transformation T_cam0_imu0 (imu0 to cam0, T_ci):
[[ 0.01542341 -0.99976267 0.01538561 0.00713584]
[ 0.03147917 -0.01489429 -0.99939343 -0.03487332]
[ 0.9993854 0.01589838 0.03124198 -0.05266484]
[ 0. 0. 0. 1. ]]
cam0 to imu0 time: [s] (t_imu = t_cam + shift)
0.0334634768386
IMU0:
----------------------------
Model: calibrated
Update rate: 150.0
Accelerometer:
Noise density: 0.0336219792081
Noise density (discrete): 0.411783466011
Random walk: 0.000982565899719
Gyroscope:
Noise density: 0.00606733703766
Noise density (discrete): 0.0743093991988
Random walk: 3.62119514583e-05
T_i_b
[[ 1. 0. 0. 0.]
[ 0. 1. 0. 0.]
[ 0. 0. 1. 0.]
[ 0. 0. 0. 1.]]
time offset with respect to IMU0: 0.0 [s]
Saving camera chain calibration to file: camchain-imucam-rs_cam15hz_imu150hz.yaml
Saving imu calibration to file: imu-rs_cam15hz_imu150hz.yaml
Detailed results written to file: results-imucam-rs_cam15hz_imu150hz.txt
Generating result report...
/home/william/kalibr_ws/src/kalibr/Schweizer-Messer/sm_python/python/sm/PlotCollection.py:57: wxPyDeprecationWarning: Using deprecated class PySimpleApp.
app = wx.PySimpleApp()
Report written to report-imucam-rs_cam15hz_imu150hz.pdf
