Realsense D435I标定

参考链接：kilibr官网 https://github.com/ethz-asl/kalibr/wiki/camera-imu-calibration ；

https://blog.csdn.net/HUST_lc/article/details/96144499 感谢学友指教

https://blog.csdn.net/weixin_40830684/article/details/88768225

https://blog.csdn.net/zhubaohua_bupt/article/details/80222321 标定结果等各参数讲解

https://blog.csdn.net/mxdsdo09/article/details/83514310 同上，标定结果等各参数讲解

https://blog.csdn.net/tercel_zhang/article/details/90637392 标定中各个坐标系理解，讲的非常清晰

https://blog.csdn.net/Aoulun/article/details/78768570 同上，内含畸变参数讲解

https://blog.csdn.net/zhuoyueljl/article/details/89509623 标定单目相机的一些注意事项参考

https://blog.csdn.net/qq_31119155/article/details/79908668

https://blog.csdn.net/u012177641/article/details/92824014

总体流程：

前期准备：在ubuntu16.04配置好realsense d435i相机环境，详细参见前面的博客：https://blog.csdn.net/weixin_40628128/article/details/90376767

https://blog.csdn.net/weixin_40628128/article/details/89891227

https://blog.csdn.net/weixin_40628128/article/details/89703000

标定工具安装与配置（kalibr）----> 深度相机(RGB,其中IR相机为双目计算，不需要标定)标定----> imu标定 ----> 深度相机+imu联合标定

一、kalibr安装：

直接按照官网进行：

https://github.com/ethz-asl/kalibr/wiki/installation

选用非CDE方式在ros kinetic环境下安装。期间遇到pip报错，解决办法：https://blog.csdn.net/lyll616/article/details/85090132

至此，安装好了kalibr。

在标定（如kalibr_calibrate_cameras）前，需进入kalibr_workspace文件目录下source一下：

cd ~/kalibr_workspace
source ~/kalibr_workspace/devel/setup.bash

二、realsense D435i 的camera标定

（1）realsense d435i可以直接读取的相机内参：

roslaunch realsense2_camera rs_camera.launch //打开相机节点
rostopic echo /camera/color/camera_info //查看相机内参

（2）kalibr标定camera

设置棋盘格参数，保存成yaml文件（位置随意，在后面引用绝对路径即可，例如kalibr_calibrate_cameras --target ~/bagfiles/checkboard.yaml）

checkboard的参数配置yaml文件在链接(https://github.com/ethz-asl/kalibr/wiki/downloads)下载，下载后修改以下参数（根据自己的实际标定板参数进行修改，我这里是购买的是GP340标定棋盘格9X12）：

target_type: 'checkerboard' #gridtype
targetCols: 8               #number of internal chessboard corners 注意是内角点数目
targetRows: 11               #number of internal chessboard corners 注意是内角点数目
rowSpacingMeters: 0.025      #size of one chessboard square [m]
colSpacingMeters: 0.025      #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  打开可视化窗口，固定相机，移动标定板（效果应稍微好一些）

ros录制和回放数据子集教程参考官网或链接https://blog.csdn.net/u010510350/article/details/72457758。

将图像频率降低为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表示可以保存成 camd435i.bag(设置bag名称)，/color 为录制的话题名称。录制方法参见（https://blog.csdn.net/zhuoyueljl/article/details/89509623）

（ctrl-c结束录制，bag文件会自动保存到录制时命令行所在的目录，我这里是~/bagfiles）

kalibr标定单个camera（之前先对kalibr_workspace source一下）

kalibr_calibrate_cameras --target ~/bagfiles/checkboard.yaml --bag ~/bagfiles/camd435i.bag --bag-from-to 5 50 --models pinhole-radtan --topics /color

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

果然也卡在了角点上。。。承蒙前辈手留余香～～https://github.com/ethz-asl/kalibr/issues/164，在后面添加--show-extraction

kalibr_calibrate_cameras --target ~/bagfiles/checkerboard.yaml --bag ~/bagfiles/camd435i.bag --bag-from-to 5 50 --models pinhole-radtan --topics /color --show-extraction

【此外，还可以对多个相机进行标定，如两个相机：

kalibr_calibrate_cameras --target src/kalibr/checkerboard.yaml --bag src/kalibr/bag/fisheye_2019-07-15-21-52-12.bag --models pinhole-radtan pinhole-radtan --topics /image_raw1_th /image_raw2_th

详情参见：https://blog.csdn.net/HUST_lc/article/details/96144499 】

三、IMU内参获取/标定

https://cloud.tencent.com/developer/article/1438368 参考理解，内有matlab标定源码链接及论文讲解。

这里需要对IMU原理及算法熟悉，这周时间预算不足，放在vio学习阶段学习。这里暂时用网友示例的数据做yaml，先看联合标定。

四、kalibr标定camera+IMU

参考 https://blog.csdn.net/mxdsdo09/article/details/83514310

联合标定的目的

我们进行camera-IMU标定的目的是为了得到IMU和相机坐标系的相对位姿矩阵T和相对时间延时t_shift(t_imu=t_cam + t_shift)。

需要的文件

1、.bag:包含有图片信息和IMU数据的ROS包
2、camchain.yaml: 包含相机的内参、畸变参数的文件，如果是双目的话，还包含两个相机的位置转换矩阵；
3、IMU.yaml: 包含IMU的噪声密度、随机游走；

4、checkboard.yaml:标定目标板的参数

所以在进行camera-IMU 标定前，我们分别要对camera内参(第二节)和IMU进行标定（第三节）得到相应的camd435i.yaml和IMU.yaml文件，已准备好相关文件，放到～/bagfile文件目录下。

输出结果

IMU和相机坐标系的相对位姿矩阵T和重投影误差（或者像素误差，Pixel Error（像素误差）指的是the standard deviation of the reprojection error (in Pixel) in both x and y directions respectivly（在x和y方向上以像素为单位的重投影误差的标准差。根据优化的准则我们知道重投影误差越小，就说相机标定的精度越高）。

标定方法（kalibr）

checkerboard.yaml：

target_type: 'checkerboard' #gridtype
targetCols: 8               #number of internal chessboard corners
targetRows: 11               #number of internal chessboard corners
rowSpacingMeters: 0.025      #size of one chessboard square [m]
colSpacingMeters: 0.025      #size of one chessboard square [m]

camchain.yaml:(用自己的相机内参参数替换里面的数值，参数见camd435i.yaml)

cam0:camera_model: pinholeintrinsics: [632.9640658678117, 638.2668942402212, 339.9807921782614, 243.68020465500277]distortion_model: equidistantdistortion_coeffs: [0.366041713382057, 1.1433178097591097, -3.008125411486294, -3.1186836086733227]T_cam_imu:- [0.01779318, 0.99967549,-0.01822936, 0.07008565]- [-0.9998017, 0.01795239, 0.00860714,-0.01771023]- [0.00893160, 0.01807260, 0.99979678, 0.00399246]- [0.0, 0.0, 0.0, 1.0]timeshift_cam_imu: -8.121e-05rostopic: /colorresolution: [640, 480]

IMU.yaml:

rostopic: /imu
update_rate: 200.0 #Hzaccelerometer_noise_density: 0.01 #continous
accelerometer_random_walk: 0.0002
gyroscope_noise_density: 0.005 #continous
gyroscope_random_walk: 4.0e-06

方法基本与camera标定等同，只不过需要设定IMU的频率（降低至200HZ）、同时录制IMU topic与color topic等。

标定动作参照官网的视频：https://github.com/ethz-asl/kalibr 主要需要激活留个自由度的运动（xyz旋转与平移）

https://blog.csdn.net/u012177641/article/details/92824014 主要参考如何将acce与gyro合成一个话题

realsense的ROS launch文件中有将acc和gyr两个整成一个imu话题的配置，设置一下就好，对应为：

<arg name="enable_sync"               default="true"/><arg name="unite_imu_method"         value="linear_interpolation"/>     【原来值为空：value= “ ”/】

其中，linear_interpolation可以换成其他的，具体是啥目前不太清楚。

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

rosrun topic_tools throttle messages /camera/color/image_raw 20.0 /color
rosrun topic_tools throttle messages /camera/gyro/image_info 200.0 /imu

利用throttle工具降低录制的RGB图像频率，降至20HZ，新发布的topic名字是/color,把IMU频率降低为200Hz，新发布的topic名字是/imul。

录制dynamic.bag数据（同时录制/imu与/color话题）：

rosbag record -O dynamic /color /imu

开始标定：

kalibr_calibrate_imu_camera --target ~/bagfiles/checkerboard.yaml --cam ~/bagfiles/camchain.yaml --imu ~/bagfiles/imu.yaml --bag ~/bagfiles/dynamic.bag --bag-from-to 5 45 --show-extraction

五、realsense D435i 的IMU configuration（官网方法）

该方法便于商用，但仅采用了六个位置静止放置，标定数据太少，标定误差会较大，不利于高精度测量和导航应用，可能并不适合SLAM。

参照官网进行标定（白皮书）：https://www.intel.com/content/www/us/en/support/articles/000032303/emerging-technologies/intel-realsense-technology.html

其中在配置软件环境时，sudo pip install numpy因网络问题总是中断，（不断尝试），sudo apt install python-numpy似乎也行。不放心，可以导入numpy试试是否已经安装成功：

未提示错误，表示numpy安装成功（感谢学友指点）。

tip:深度图与彩色图对齐：在d435i的launch文件（rs_camera.launch）里修改align_depth值为true即可。