主要参考。
https://blog.csdn.net/weixin_44350238/article/details/102672744
最左端的为 RGB摄像头，其次为 infra1, 红外发射器， infra2.
****************************************************************************************--
---环境准备****************************************************************************---
****************************************************************************************--
1. git clone 然后用 ROS 编译。
编译完毕后。 执行 python setup.py install 【kalibr/src/Schweizer-Messer/sm_python/】
编译完毕后。 执行 python setup.py install 【kalibr/src/Schweizer-Messer/numpy_eigen/】
### 也可以用 下载 CDE， 解压到 /opt/kalibr-cde/, 并加载到 $PATH 目录, 但目前我是用的远吗。
2. 安装 librealsense源码和 ROS包
3. 安装 code_utils + imu_utils 的 ROS包。
4. 记得执行的时候有个so是个空的， 把 devel 的同名的拷贝到系统目录中即可。
****************************************************************************************--
****先进行 IMU 校准********************************************************************---
****************************************************************************************--
1. 保证输出 IMU数据。
roscd realsense2_camera/launch;
mv rs_camera.launch d435i_imu_calibration.launch
修改 d435i_imu_calibration.launch
参数 unite_imu_method 改成 linear_interpolation,即把acc_gyro数据对象到 /camera/imu 发布。
2. 添加 imu_utils/launch/d435i.launch. 内容 参见附件
4. 执行imu数据录制。
把D435i放在一个位置静止不动。
roslaunch realsense2_camera d435i_imu_calibration.launch
rosbag record /camera/imu -O imu.bag --duration=61m #录制61分钟的imu数据, 稍微长一些。
5. 执行校准 - 几秒钟搞定
rosbag play -r 400 imu.bag
roslauch imu_utils d435i.launch
roscd imu_utils/data/； ls *d435i*; 得到IMU标定结果文件 d435i_imu_param.yaml
6. 制作 kalibr 可用的 IMU 文件。使用 Kalibr 下载的yaml文件， 对数据进行修改。
cd /work/bag; cp /work/tools/Kalibr/imu_adis16448.yaml ./d435i_imu.yaml
然后把d435i_imu_param.yaml文件内容带入到 d435i_imu.yaml。 内容为：
rostopic: /imu
update_rate: 200.0 #Hz
accelerometer_noise_density: 2.6200858019544634e-02
accelerometer_random_walk: 9.8972569971020634e-04
gyroscope_noise_density: 3.8738152631686493e-03
gyroscope_random_walk: 3.9809609780432112e-05
****************************************************************************************--
****在进行多个摄像头校准****************************************************************---
****************************************************************************************--
1. 创建标定模板 kalibr_create_target_pdf --type checkerboard --nx 6 --ny 7 --csx 0.030 --csy 0.030
重命名为 checkerboard_7x6_30x30cm.pdf
$ cat checkerboard_7x6_30x30cm.yaml
target_type: 'checkerboard' #gridtype
targetCols: 6 #number of internal chessboard corners
targetRows: 7 #number of internal chessboard corners
rowSpacingMeters: 0.03 #size of one chessboard square [m]
colSpacingMeters: 0.03 #size of one chessboard square [m]
2. 在一个大的屏幕上显示这个PDF文件， 注意要按照实际尺寸显示。
3. 建立 目标模板的配置文件。 $ cat checkerboard_7x6_30x30cm.yaml
target_type: 'checkerboard' #gridtype
targetCols: 6 #number of internal chessboard corners
targetRows: 7 #number of internal chessboard corners
rowSpacingMeters: 0.03 #size of one chessboard square [m]
colSpacingMeters: 0.03 #size of one chessboard square [m]
4.
roslaunch realsense2_camera rs_camera.launch
rosrun topic_tools throttle messages /camera/color/image_raw 20 /color
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 20 /infra2
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 20 /infra1
### rosrun camera_calibration cameracalibrator.py --size 6x7 --square 0.09 left:=/camera/infra1/image_rect_raw right:=/camera/infra2/image_rect_raw left_camera=/camera/infra1 right_camera=/camera/infra2 --no-service-check 可以配上这个，查看是否确实存在了足够多的校准位置。
rosbag record -O multi_cameras_calibration /infra1 /infra2 /color ###
kalibr_calibrate_cameras --target checkerboard_7x6_30x30cm.yaml --bag multi_cameras_calibration.bag --models pinhole-equi pinhole-equi pinhole-equi --topics /infra1 /infra2 /color --show-extraction --approx-sync 0.04 --时间约 3个小时
### 实际上官方 infra1/camera_info: fxy cxy = 384.3780517578125,
384.3780517578125,
318.5202941894531,
239.21954345703125
### infra1 - Kalibr 结果：
370.2196633831552,
370.50369104149075,
323.6389467404484,
237.18080401436492
### infra1 - VINS PIC Calibr 结果： fx: 386.03001052763881
387.41208996358472
320.37436596887255
239.78829921129437
### infra1 - ROS online Calibr 结果： 387.1209079053243,
388.5363515030237,
321.6271008133721,
239.61367391587535
### 结论：从接近程序上， VINS PIC 方式 结果精度更接近官方位置。 但前提是官方的是标准储存值，对于个体差异来说算不得说是标准值。
### 4.1 使用 VINS camera_calibrte 方法
### 准备拍照
###
mkdir infa1_pic; cd infa1_pic; roslaunch realsense-viewer
###
# 设置单个摄像头 infra1， 640x480， 关闭曝光
###
#对准PDF文件，远近和各个方向以及各个边角， 做多个照片保存。
###
#然后去除无效文件， 改成合规的文件名称。
###
rm *.raw *.csv *.log
###
let idx=1; for file in `ls *.png`; do mv $file infra1-$idx.png; let idx=$idx+1; done
###
#执行vins的校准
###
rosrun camera_models Calibrations -w 6 -h 7 -s 30 -i infa1_pic --camera-model pinhole
###
#结果文件改名
###
mv camera_camera_calib.yaml infra1_calib.yaml
###
#结果为
### %YAML:1.0
### ---
### model_type: PINHOLE
### camera_name: camera
### image_width: 640
### image_height: 480
### distortion_parameters:
### k1: 8.3854744668972643e-03
### k2: -8.7156989408758694e-03
### p1: 4.7121977368693515e-04
### p2: 8.7258032407440910e-04
### projection_parameters:
### fx: 3.8603001052763881e+02
### fy: 3.8741208996358472e+02
### cx: 3.2037436596887255e+02
### cy: 2.3978829921129437e+02
### 而官方的信息
### $ rostopic echo /camera/infra1/camera_info
### K: [384.3780517578125, 0.0, 318.5202941894531, 0.0, 384.3780517578125, 239.21954345703125, 0.0, 0.0, 1.0]
### P: [384.3780517578125, 0.0, 318.5202941894531, 0.0, 0.0, 384.3780517578125, 239.21954345703125, 0.0, 0.0, 0.0, 1.0, 0.0]
###
### 同理， 拍照 infra2， 得到 infra2_calib.yaml
### 最后修改成
### distortion_coeffs形变系数向量[k1, k2, p1, p2 外参] intrinsic=[fx, fy, cx, cy 内参]
****************************************************************************************--
****************************************************************************************--
****************************************************************************************--
### 4.3 使用 ROS camera_calibrtion python脚本 进行 摄像头校准方法。
roslaunch realsense2_camera rs_camera.launch
rosrun camera_calibration cameracalibrator.py --size 6x7 --square 0.09 left:=/camera/infra1/image_rect_raw right:=/camera/infra2/image_rect_raw left_camera=/camera/infra1 right_camera=/camera/infra2 --no-service-check
移植等待出现 按钮， 然后
Left:
('D = ', [0.005756208102302846, -0.0010481896800584056, 0.0004670441892117128, 0.0014779867831939061, 0.0])
('K = ', [387.1209079053243, 0.0, 321.6271008133721, 0.0, 388.5363515030237, 239.61367391587535, 0.0, 0.0, 1.0])
('R = ', [0.9998321277849453, 0.0032247369976330614, 0.01803655511245961, -0.0032057868144247438, 0.999994278822762, -0.0010794686861498386, -0.018039932924741063, 0.0010214661227932451, 0.9998367453874811])
('P = ', [397.0994706326997, 0.0, 311.42203521728516, 0.0, 0.0, 397.0994706326997, 240.57631874084473, 0.0, 0.0, 0.0, 1.0, 0.0])
Right:
('D = ', [0.00443395673886329, -0.0004303955419512914, 0.0007861244738098656, 0.000979372825030523, 0.0])
('K = ', [386.1137207490011, 0.0, 320.0622933460605, 0.0, 387.5007695649709, 240.6377842768414, 0.0, 0.0, 1.0])
('R = ', [0.99989079607022, 0.0032847885416025014, 0.014408542545961984, -0.0032999234817991797, 0.9999940281111515, 0.0010267653323981303, -0.014405083792748652, -0.0010742002934747926, 0.9998956643843664])
('P = ', [397.0994706326997, 0.0, 311.42203521728516, -58.160846301963616, 0.0, 397.0994706326997, 240.57631874084473, 0.0, 0.0, 0.0, 1.0, 0.0])
('self.T ', [-0.14644818039251067, -0.00048110384332218203, -0.002110335294871596])
('self.R ', [0.999993387784054, -9.023406376158736e-05, 0.003635415517434497, 9.784794829334061e-05, 0.9999978022880105, -0.0020942408954319584, -0.003635218555971738, 0.002094582765808488, 0.9999911989158142])
# oST version 5.0 parameters
[image]
width 640
height 480
[narrow_stereo/left]
camera matrix
387.120908 0.000000 321.627101
0.000000 388.536352 239.613674
0.000000 0.000000 1.000000
distortion
0.005756 -0.001048 0.000467 0.001478 0.000000
rectification
0.999832 0.003225 0.018037
-0.003206 0.999994 -0.001079
-0.018040 0.001021 0.999837
projection
397.099471 0.000000 311.422035 0.000000
0.000000 397.099471 240.576319 0.000000
0.000000 0.000000 1.000000 0.000000
# oST version 5.0 parameters
[image]
width 640
height 480
[narrow_stereo/right]
camera matrix
386.113721 0.000000 320.062293
0.000000 387.500770 240.637784
0.000000 0.000000 1.000000
distortion
0.004434 -0.000430 0.000786 0.000979 0.000000
rectification
0.999891 0.003285 0.014409
-0.003300 0.999994 0.001027
-0.014405 -0.001074 0.999896
projection
397.099471 0.000000 311.422035 -58.160846
0.000000 397.099471 240.576319 0.000000
0.000000 0.000000 1.000000 0.000000
****************************************************************************************--
****最后进行摄像头和IMU联合校准************************************************************
****************************************************************************************--
1.
roslaunch realsense2_camera d435i_stero_calibration.launch # 设置 enable_sync=true 设置 unite_imu_method = copy
2.
rosrun topic_tools throttle messages /camera/imu 200 /imu
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 20 /infra1
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 20 /infra2
3.
### rosrun camera_calibration cameracalibrator.py --size 6x7 --square 0.09 left:=/camera/infra1/image_rect_raw right:=/camera/infra2/image_rect_raw left_camera=/camera/infra1 right_camera=/camera/infra2 --no-service-check 可以配上这个，查看是否确实存在了足够多的校准位置。
rosbag record -O camera_imu_calib /imu /infra1 /infra2 ## rpy旋转， 并且远近变幻。 要想有比较好的标定结果，必须使得加速度和角速度得到充分激励，就是不能单纯的旋转，也不能单纯的平移，并且一直观测到标定版。
4.
kalibr_calibrate_imu_camera --target checkerboard_7x6_30x30cm.yaml --cam camchain-multi_cameras_calibration.yaml --imu d435i_imu.yaml --bag camera_imu_calib.bag --max-iter 15 --show-extraction ### 时间约 3个小时
****************************************************************************************--
****设置 VINS ************************************************************
****************************************************************************************--
body_T_cam0 means when you are sitting on body (imu) the pose (position and orientation) of camera0 (left camera).
So, body_T_cam0 is the pose of the cam0 as observed from body frame aka imu.
k1 k2 p1 p2 来自于 results-imucam-camera_imu_calib.txt
Distortion coefficients: [0.3330463486073408, 0.16935030764642128, -0.021284972894140688, 0.04875952715825176]
fx fy cx cy 来自于 camchain-imucam-camera_imu_calib.yaml
intrinsics: [370.2196633831552, 370.50369104149075, 323.6389467404484, 237.18080401436492]
acc_n/w + gyr_n/w 来自于 results-imucam-camera_imu_calib.txt 的 IMU configuration 部分
body_T_cam0、1 来自于 results-imucam-camera_imu_calib.txt 的 T_ci: (imu0 to cam0、1 部分)
