bmi055 标定_Kalibr tutorials

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