realsense d435i标定imu与camera

realsense d435i标定imu与camera

1.标定目的
realsense d435i包含两个红外相机、红外发射器、RGB相机和IMU四个模块，显然四个传感器的空间位置是不同的，我们在处理图像和IMU数据时需要将这些数据都放在统一的坐标系上去。比如我们用d435i运行vins，我们处理的图像和IMU数据都需要放在同一个坐标系下，因此我们需要标定IMU相对RGB相机的空间位置（包括旋转和位移）。另外，相机固有参数比如焦距、畸变参数等以及IMU的零偏和scale系数等都需要提前知道。前者我们称为外参，后者称为内参，在运行程序前我们需要标定它们，不论程序是否有自标定功能，毕竟好的初始标定值对于自标定来说也是有利的。

2.标定准备

1. 安装realsense-sdk2.0，包括d435i的驱动等，直到可以运行realsense-viewer，可以看到图像和深度图。

2. 安装realsense-ros，前提要安装ros-kinetic，这个包可以直接读取d435i的数据流，并发布各个topic，后面标定操作直接订阅相关的topic。

3. 安装imu_utils，前提要安装code_utils，这个用于标定IMU的噪音密度和随机游走系数。
   在github上下载,并编译,地址:https://github.com/gaowenliang/imu_utils
   问题:code_utils-master/src/sumpixel_test.cpp:2:24: fatal error: backward.hpp:No such file or directory
   解决:在code_utils下面找到sumpixel_test.cpp，修改#include "backward.hpp"为 #include “code_utils/backward.hpp”，再编译。

4. 安装Kalibr。这个软件包可以同时标定多个相机的外参和内参（提供不同的相机的模型），另外可以标定相机和IMU的外参。

3.imu标定
在imu_utils/launch/目录下新建 d435i_imu.launch


#imu topic的名字


#标定的时长min



运行相机
roslaunch realsense2_camera rs_camera.launch

录制imu数据包
rosbag record -O imu_calibration /camera/imu
录制2小时按CTRL+C完成

运行校准程序
roslaunch imu_utils d435i_imu.launch
回放数据包(20倍回放)
rosbag play -r 20 imu_calibration.bag
经过这些标定会生成一个yaml文件和很多txt文件，主要是yaml文件，给出了加速度计和陀螺仪三轴的noise_density和random_walk，同时计算出了平均值，后面IMU+摄像头联合标定的时候需要这些均值。
#根据标定结果修改① imu.yaml，后面会用到，其文件内容为
#Accelerometers
accelerometer_noise_density: 3.6510867135455143e-02 #Noise density (continuous-time)
accelerometer_random_walk: 8.7794225985338419e-04 #Bias random walk
#Gyroscopes
gyroscope_noise_density: 1.1110598103547959e-02 #Noise density (continuous-time)
gyroscope_random_walk: 2.1198529500305598e-04 #Bias random walk
rostopic: /camera/imu #the IMU ROS topic
update_rate: 200.0 #Hz (for discretization of the val

4.相机标定

设置棋盘格参数，保存成yaml文件，（位置随意，在后面引用绝对路径即可，例如kalibr_calibrate_cameras --target ~/d435i_calibration/checkboard.yaml）②，保存好后面会用到。
target_type: ‘checkerboard’ #gridtype
targetCols: 8 #number of internal chessboard corners 注意是内角点数目
targetRows: 11 #number of internal chessboard corners 注意是内角点数目
rowSpacingMeters: 0.03 #size of one chessboard square [m]
colSpacingMeters: 0.03 #size of one chessboard square [m]

录制camera标定数据：rosbag

为了方便查看，先打开可视化窗口：

终端1：roslaunch realsense2_camera rs_camera.launch //打开相机节点
终端2：rviz/，选camera_link,add topic->camera->color->image_raw 打开可视化窗口，固定相机，移动标定板（效果应稍微好一些）,或者rqt也可。

将图像频率降低为4HZ，这里可以用throttle方法，不会出错，并发布新的topic，不会修改原topic：

rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color
利用throttle工具降低录制的RGB图像频率，降至4HZ，新发布的topic名字是/color

rosbag record -O camd435i /color
其中 -O表示可以保存成camera_calibration.bag(设置bag名称)，/color 为录制的话题名称。
（ctrl-c结束录制，bag文件会自动保存到录制时命令行所在的目录）

大部分时间应保证目标（标定板）占视野一半以上，尽可能多角度和多位置（上下左右等）甚至到摄像头捕捉图像的边缘，这样移动目标1min左右即可。

kalibr标定单个camera
kalibr_calibrate_cameras --target ~/d435i_calibration/checkboard.yaml --bag ~/camera_calibration.bag --bag-from-to 5 50 --models pinhole-radtan --topics /color --show-extraction
其中–target ~/bagfiles/checkboard.yaml为定位棋盘格yaml参数文件路径；
–bag ~/bagfiles/camd435i.bag为设置录制bag文件路径；
–bag-from-to 5 50表示用5~50s（根据录制数据的质量决定时间的取舍）之间的数据进行计算；
–models pinhole-radtan为相机模型；
–topics /color，bag录制的话题为topics。

完成后会生成③camchain.yaml，保存好后面会用

错误：No module named ceres
解决：https://github.com/ceres-solver/ceres-solver
错误:ImportError: No module named igraph
解决:pip install igraph --user

5.imu和相机共同标定
调用kalibr的算法计算IMU和camera外参
需要准备的文件：
1.imu.yaml:包含imu参数；
2.camchain.yaml: 包含相机的内参、畸变参数的文件，如果是双目的话，还包含两个相机的位置转换矩阵；
3.checkboard.yaml:标定目标板的参；
4…bag:包含有图片信息和IMU数据的ROS包。
我们之前已经获取了①②③，三个文件，只差bag包了。

抓取bag包
官方推荐 camera 20hz，imu 200hz。
rosrun topic_tools throttle messages /camera/color/image_raw 20.0 /color
rosrun topic_tools throttle messages /camera/imu 200.0 /imu

固定标定目标，确保摄像头能够提取特征前提下充分调整d435i的姿势和位置，录制数据包。
rosbag record -O imu_camera_calibration /color /imu

先面对标定目标，然后俯仰、偏航和横滚三个角度分别面向目标运动，然后是前后左右和上下运动，充分运动起来，推荐时间在2min以上。
完成后得到 ④imu_camera_calibration

运行标定
kalibr_calibrate_imu_camera --target ~/d435i_calibration/checkboard.yaml --cam ~/d435i_calibration/camchain.yaml --imu ~/d435i_calibration/imu.yaml --bag ~/d435i_calibration/imu_camera_calibration.bag --bag-from-to 5 45 --show-extraction

注意： --bag-from-to 10 100 选择10-100s之间的数据
-show-extraction 展示特征提取情况

问题:ImportError: No module named scipy.optimize
解决 pip install scipy –user

标定结果
T_ic: (cam0 to imu0):
[[ 0.99960066 0.01259205 -0.02529763 -0.0054386 ]
[-0.01271351 0.99990839 -0.00464588 0.01025557]
[ 0.02523681 0.00496565 0.99966917 0.00876445]
[ 0. 0. 0. 1. ]]

timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
-0.0451287115686

标定评价
一般般。