RealSenseD345I —— imu + camera标定

目录

1、标定目的

2、标定准备

3、标定步骤

     1）IMU标定

    2）相机标定

   3）IMU+相机联合标定

4、标定评估

      1）多目标定评估

      2）IMU+CAM联合标定评估

---

 

---

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的噪音密度和随机游走系数。

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

---

3、标定步骤

      标定顺序：IMU标定 —> 相机标定  —> IMU+相机联合标定

     这么设定顺序是因为最后一部IMU和相机的联合标定需要IMU和相机的内参，现在开始展开说明。

     1）IMU标定

         step1：进入realsense-ros包，开启d435i，发布IMU话题

                      roslaunch realsense2_camera rs_imu_calibration.launch

                      rs_imu_calibration.launch是我在rs_camera.launch针对IMU校准做了修改，具体是：

                       

                        => 

                   

                    如果不改的话camera/imu没有这个数据流。

          step2:  录制imu数据包
                      rosbag record -O imu_calibration /camera/imu
           step3: 运行校准程序

                         roslaunch imu_utils d435i_imu_calibration.launch

           step4: 回放数据包

                        rosbag play -r 20 imu_calibration.bag

                       20倍回放速度真是快。

             当然也可以不用录制和回放直接在线标定，也即是1和3步即可。经过这些标定会生成一个yaml文件和很多txt文件，主要是yaml文件，给出了加速度计和陀螺仪三轴的noise_density和random_walk，同时计算出了平均值，后面IMU+摄像头联合标定的时候需要这些均值。

    2）相机标定

        下面是使用Kalibr标定相机的内参和多个相机相对位置关系即外参，需要准备kalibr提供的标定物，具体可以在kalibr-wiki目录中找到Calibration Target ，然后找到april grid下载，那里面提供了标定目标的图案和相应的参数。你也可以定做不同大小的标定目标。具体标定步骤：

      step 0:   使用kalibr自带的标定物生成脚本生成自己想要尺寸的aprilgrid，将其pdf文件放在电脑屏幕上按照真实尺寸显示，这样免去了打印的麻烦，而且因为目标是在绝对的水平面上，不会引入额外的标定误差

                      kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.08 --tspace 0.3

                      yaml文件需要按照设定的尺寸进行修改

       step 1：进入realsense-ros包，开启d435i，发布摄像头图像的话题

                      roslaunch realsense2_camera rs_camera.launch                 

注意：d453i是有红外发射器的，可以发射很多红外小斑点，如果打开你会在rviz看到很多光斑，可能不利于标定，所以标定时，我是关闭这个发射器的。

         step2：将标定目标AprilGrid置于相机前方合理距离范围内，然后缓慢移动标定目标，让所有摄像头均能看到标定物

                       一定不要太远，不然无法检测到标定目标的特征，在标定算法中需要检测是否有足够数量图片检测到标定特征，否则直接无法标定。移动标定物时候不要过快导致运动模糊，我们只需要获取不同位置和角度的图像，确保图像清晰和特征完整即可。另外要尽可能多角度和多位置（上下左右等）甚至到摄像头捕捉图像的边缘，这样移动目标1min左右即可。

        step3：降低图像话题的频率，录制图像数据包

                       kalibr在处理标定数据的时候要求图像的频率不可过高，一般为4hz。我们使用如下指令来限制图像频率：

                       rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color
                       rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_left
                       rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_right

                         然后进行录制：

                      rosbag record -O multicameras_calibration /infra_left /infra_right /color

        step4：调用kalibr的算法计算各个摄像头的内参和外参

                   kalibr_calibrate_cameras --target ../yaml/april_6x6_A4.yaml --bag ./bag/multicameras_calibration_25.bag --models pinhole-equi pinhole-equi pinhole-equi --topics /infra_left /infra_right /color --bag-from-to 10 100

                 命令说明：

                april_6x6_A4.yaml -- 标定物的参数，具体是标定目标的尺寸之类，因为我是缩小打印在A4上，所以要对参数进行修改；                 pinhole-equi  -- 选择的相机模型，kalibr提供了很多相机模型，可以自己选择；

                --bag-from-to 可以选择时间段，毕竟录制的时候不能保证整体都录制的很好。

                这个计算的结果会很久，最后会输出一个pdf和txt文件，有内外参数据。

   3）IMU+相机联合标定

              在标定IMU和camera的时候可以选择多个相机和IMU一起，也可以选择一个相机，必定前面已将多个相机进行了标定，如果需要可以只标定主相机（作为参照坐标系的相机）与IMU的相对位置。

       step1：准备IMU和camera配置文件，将之前标定的数据按照kalibr的yaml文件准备好

                前面摄像头标定的时候其实已经生成了相关的yaml文件，只要将imu参照kalibr提供的例子，将参数换成自己标定好的参数即可。

      step2：进入realsense-ros包，开启d435i，发布摄像头图像和IMU数据的话题

                这里我将

                         ，  

                        

               一个是保持IMU和图像信息同步，另一个要确保输出IMU数据。

        step3：固定标定目标，确保摄像头能够提取特征前提下充分调整d435i的姿势和位置，录制数据包

                 具体可以参照kalibr的视频，视频中是先面对标定目标，然后俯仰、偏航和横滚三个角度分别面向目标运动，然后是前后左右和上下运动，充分运动起来。因为kalibr推荐IMU 200Hz，图像20Hz，所以用topic_tools throttle限制频率。

                 注意：用topic_tools throttle限制频率后一定要看一下限制后的topic输出频率：rostopic hz /topic ，你会发现实际的频率与设定的频率并不一致，你可能需要设置不同的数值比如：rosrun topic_tools throttle messages /topic_1  25 /topic_2，如果topic_1是40hz，/topic_2可能不是25hz，而是20hz，具体原因不明。有知道的可以留言告知，谢谢！

                  注意：切记一定要保证目标在图像中清晰可见，同时要求整个视频时间尽量短，否则后续优化耗时很久。

        step4：调用kalibr的算法计算IMU和camera外参

               kalibr_calibrate_imu_camera --target ../yaml/april_6x6_A4.yaml --cam ../yaml/infraleft_d435i.yaml --imu ../yaml/imu_d435i.yaml --bag ./bag/imu_cameras_calibration.bag --bag-from-to 10 100 --max-iter 15  --show-extraction

           注意：--bag-from-to 10 100  选择10-100s之间的数据

                       --max-iter 15                设置优化迭代次数为15次，默认30

                       --show-extraction         展示特征提取情况

---

4、标定评估

      1）多目标定评估

         kalibr标定多目后会生成标定报告以及标定出的外参值，标定报告会直接给出相机坐标的空间位置示意图，结合标定结果和实际位置比较可以较为直观的判断结果是否可靠；另外就是看重投影误差，这个值越小越好，我的在0.15以下已经是很好的了。内参数的话也可以关注一下，我使用的d435i，标定后双面的内参基本一样，不过和自带/camera_info还是有所区别的，但是不是很大。

      2）IMU+CAM联合标定评估

          联合标定后kalibr同样给出标定报告，同上面多目一样主要关注位置和实际位置是否相符，其次就是残差项，具体我不知道如何评定残差是否合理，我的是：

Normalized Residuals
----------------------------
Reprojection error (cam0): mean 0.116388852487, median 0.100494263466, std: 0.115779726566
Reprojection error (cam1): mean 0.118837162459, median 0.102707273927, std: 0.118032053496
Gyroscope error (imu0): mean 0.566382365906, median 0.424566309149, std: 0.541174462645
Accelerometer error (imu0): mean 0.303879126638, median 0.257914882481, std: 0.20381924896
Residuals
----------------------------
Reprojection error (cam0) [px]: mean 0.116388852487, median 0.100494263466, std: 0.115779726566
Reprojection error (cam1) [px]: mean 0.118837162459, median 0.102707273927, std: 0.118032053496
Gyroscope error (imu0) [rad/s]: mean 0.0221873017669, median 0.016631839881, std: 0.0211998145317
Accelerometer error (imu0) [m/s^2]: mean 0.128924994664, median 0.109424017423, std: 0.0864731838454

         而kalibr提供的例程是：

Normalized Residuals
----------------------------
Reprojection error (cam0):     mean 0.142975755445, median 0.130686487357, std: 0.0817972217348
Reprojection error (cam1):     mean 0.139113383779, median 0.129061976876, std: 0.0768758604603
Gyroscope error (imu0):        mean 0.0947751091434, median 0.082773649866, std: 0.0609365033876
Accelerometer error (imu0):    mean 0.363001590633, median 0.285683155402, std: 0.341485432021

Residuals
----------------------------
Reprojection error (cam0) [px]:     mean 0.142975755445, median 0.130686487357, std: 0.0817972217348
Reprojection error (cam1) [px]:     mean 0.139113383779, median 0.129061976876, std: 0.0768758604603
Gyroscope error (imu0) [rad/s]:     mean 0.0067016122363, median 0.00585298091238, std: 0.00430886147672
Accelerometer error (imu0) [m/s^2]: mean 0.0513361772636, median 0.0404016992912, std: 0.0482933329317

      感觉可能是没有标的太好吧。