使用kalibr进行Camera-IMU联合标定

本文以Intel RealSense D435i为例，介绍使用kalibr进行Camera-IMU联合标定的方法。

整个过程比较复杂，需要花费的时间比较多，进行的准备工作也比较多。具体分为三步，即首先使用imu_utils标定IMU，然后再通过kalibr标定相机，最后结合前面两步标定的结果用kalibr进行Camera-IMU联合标定。

准备工作

软件方面：

• Linux系统，例如Ubuntu 18.04
• ROS环境，例如ROS Melodic
• RealSense SDK 与 realsense-ros-wrapper
• kalibr
• code_utils 与 imu_utils
• 其他相关依赖

硬件方面：

• Intel Realsense D435i相机
• 标定板一个，例如打印好的AprilTag或棋盘格

注：

使用kalibr生成标定板的命令格式如下，运行后可以得到一个pdf文件。需要使用原始尺寸打印出来，并且这里的参数要记好，后面需要用。

kalibr_create_target_pdf --type apriltag --nx [NUM_COLS] --ny [NUM_ROWS] --tsize [TAG_WIDTH_M] --tspace [TAG_SPACING_PERCENT]

在开始下面的工作之前，你需要先做好这个部分的准备工作。标定板可以用纸张打印后平贴到墙壁上，相关软件、依赖参考官网的教程即可顺利完成，这里不再赘述。

使用imu_utils标定imu

1.运行D435i相机

你可以通过下面的方式指定参数来运行：

roslaunch realsense2_camera rs_camera.launch \
    unite_imu_method:="linear_interpolation" \
    enable_gyro:=true \
     enable_accel:=true

当然，也可以修改rs_camera.launch这个文件里面的内容，把上面对应的部分修改好即可。然后直接运行：

roslaunch realsense2_camera rs_camera.launch

2.录制IMU数据包

在相机运行起来后，imu数据会在/camera/imu话题下发布，录制这个话题的信息即可，建议录制比较长的时间。

rosbag record -O imu_calibration /camera/imu 

3.使用imu_utils进行标定

首先需要新建一个imu_utils的launch文件，内容如下所示：

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <!--IMU的话题-->
        <param name="imu_topic" type="string" value= "/camera/imu"/>
        <!--这个和最后保存的文件名有关，最后的文件名为D435i_imu_param.yaml-->
        <param name="imu_name" type="string" value= "D435i"/>
        <!--标定结果存放的文件目录-->
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <!--修改标定时间，单位是分钟，需要根据数据包的时长来调整-->
        <param name="max_time_min" type="int" value= "10"/>
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

我们把新建的launch文件命名为d435i_imu_calibration.launch ，然后通过如下的命令就可以运行了：

roslaunch imu_utils d435i_imu_calibration.launch 

运行之后imu_utils会等待imu数据的发布，因此我们还需要播放之前录制的数据包。找到之前录制的数据包，运行下面的命令：

rosbag play -r 200 imu_calibration.bag

-r 200是以200倍的速度播放这个数据包。

这一部分的工作都完成之后，我们会得到D435i相机IMU的标定文件D435i_imu_param.yaml。

使用kalibr标定Camera

1.确认标定板的参数

这里我使用kalibr生成的标定板，生成的命令如下所示：

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

将上面命令中对应的参数保存到april_s.yaml文件中，之后我们还需要用到。文件内容如下所示：

target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.03           #size of apriltag, edge to edge [m]
tagSpacing: 0.3          #ratio of space between tags to tagSize

2.修改D435i的launch文件

由于需要标定的是D435i的两个红外摄像头，因此我们要在launch文件中开启D435i的红外摄像头，同时关掉D435i的激光，防止产生干扰。对应的launch文件保存为rs_d435i.launch，具体内容如下所示，可直接使用：

<launch>
  <arg name="emitter_enable"   	  default="0"/>

  <arg name="serial_no"           default=""/>
  <arg name="usb_port_id"         default=""/>
  <arg name="device_type"         default=""/>
  <arg name="json_file_path"      default=""/>
  <arg name="camera"              default="camera"/>
  <arg name="tf_prefix"           default="$(arg camera)"/>
  <arg name="external_manager"    default="false"/>
  <arg name="manager"             default="realsense2_camera_manager"/>
  <arg name="output"              default="screen"/>
  <arg name="respawn"              default="false"/>

  <arg name="fisheye_width"       default="-1"/>
  <arg name="fisheye_height"      default="-1"/>
  <arg name="enable_fisheye"      default="false"/>

  <arg name="depth_width"         default="640"/>
  <arg name="depth_height"        default="480"/>
  <arg name="enable_depth"        default="true"/>

  <arg name="confidence_width"    default="-1"/>
  <arg name="confidence_height"   default="-1"/>
  <arg name="enable_confidence"   default="true"/>
  <arg name="confidence_fps"      default="-1"/>

  <arg name="infra_width"         default="640"/>
  <arg name="infra_height"        default="480"/>
  <arg name="enable_infra"        default="true"/>
  <arg name="enable_infra1"       default="true"/>
  <arg name="enable_infra2"       default="true"/>
  <arg name="infra_rgb"           default="false"/>

  <arg name="color_width"         default="640"/>
  <arg name="color_height"        default="480"/>
  <arg name="enable_color"        default="true"/>

  <arg name="fisheye_fps"         default="30"/>
  <arg name="depth_fps"           default="30"/>
  <arg name="infra_fps"           default="30"/>
  <arg name="color_fps"           default="30"/>
  <arg name="gyro_fps"            default="200"/>
  <arg name="accel_fps"           default="250"/>
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>

  <arg name="enable_pointcloud"         default="false"/>
  <arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
  <arg name="pointcloud_texture_index"  default="0"/>
  <arg name="allow_no_texture_points"   default="false"/>
  <arg name="ordered_pc"                default="false"/>

  <arg name="enable_sync"               default="true"/>
  <arg name="align_depth"               default="true"/>

  <arg name="publish_tf"                default="true"/>
  <arg name="tf_publish_rate"           default="0"/>

  <arg name="filters"                   default=""/>
  <arg name="clip_distance"             default="-2"/>
  <arg name="linear_accel_cov"          default="0.01"/>
  <arg name="initial_reset"             default="false"/>
  <arg name="unite_imu_method"          default="linear_interpolation"/>
  <arg name="topic_odom_in"             default="odom_in"/>
  <arg name="calib_odom_file"           default=""/>
  <arg name="publish_odom_tf"           default="true"/>

  <arg name="stereo_module/exposure/1"  default="7500"/>
  <arg name="stereo_module/gain/1"      default="16"/>
  <arg name="stereo_module/exposure/2"  default="1"/>
  <arg name="stereo_module/gain/2"      default="16"/>
  
  

  <group ns="$(arg camera)">

    <!-- rosparam set stereo_module/emitter_enabled false -->
    <rosparam>
      stereo_module/emitter_enabled: 0
    </rosparam>

    <rosparam if="$(arg emitter_enable)">
      stereo_module/emitter_enabled: 1
    </rosparam>


    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
      <arg name="external_manager"         value="$(arg external_manager)"/>
      <arg name="manager"                  value="$(arg manager)"/>
      <arg name="output"                   value="$(arg output)"/>
      <arg name="respawn"                  value="$(arg respawn)"/>
      <arg name="serial_no"                value="$(arg serial_no)"/>
      <arg name="usb_port_id"              value="$(arg usb_port_id)"/>
      <arg name="device_type"              value="$(arg device_type)"/>
      <arg name="json_file_path"           value="$(arg json_file_path)"/>

      <arg name="enable_pointcloud"        value="$(arg enable_pointcloud)"/>
      <arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/>
      <arg name="pointcloud_texture_index"  value="$(arg pointcloud_texture_index)"/>
      <arg name="enable_sync"              value="$(arg enable_sync)"/>
      <arg name="align_depth"              value="$(arg align_depth)"/>

      <arg name="fisheye_width"            value="$(arg fisheye_width)"/>
      <arg name="fisheye_height"           value="$(arg fisheye_height)"/>
      <arg name="enable_fisheye"           value="$(arg enable_fisheye)"/>

      <arg name="depth_width"              value="$(arg depth_width)"/>
      <arg name="depth_height"             value="$(arg depth_height)"/>
      <arg name="enable_depth"             value="$(arg enable_depth)"/>

      <arg name="confidence_width"         value="$(arg confidence_width)"/>
      <arg name="confidence_height"        value="$(arg confidence_height)"/>
      <arg name="enable_confidence"        value="$(arg enable_confidence)"/>
      <arg name="confidence_fps"           value="$(arg confidence_fps)"/>

      <arg name="color_width"              value="$(arg color_width)"/>
      <arg name="color_height"             value="$(arg color_height)"/>
      <arg name="enable_color"             value="$(arg enable_color)"/>

      <arg name="infra_width"              value="$(arg infra_width)"/>
      <arg name="infra_height"             value="$(arg infra_height)"/>
      <arg name="enable_infra"             value="$(arg enable_infra)"/>
      <arg name="enable_infra1"            value="$(arg enable_infra1)"/>
      <arg name="enable_infra2"            value="$(arg enable_infra2)"/>
      <arg name="infra_rgb"                value="$(arg infra_rgb)"/>

      <arg name="fisheye_fps"              value="$(arg fisheye_fps)"/>
      <arg name="depth_fps"                value="$(arg depth_fps)"/>
      <arg name="infra_fps"                value="$(arg infra_fps)"/>
      <arg name="color_fps"                value="$(arg color_fps)"/>
      <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
      <arg name="accel_fps"                value="$(arg accel_fps)"/>
      <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
      <arg name="enable_accel"             value="$(arg enable_accel)"/>

      <arg name="publish_tf"               value="$(arg publish_tf)"/>
      <arg name="tf_publish_rate"          value="$(arg tf_publish_rate)"/>

      <arg name="filters"                  value="$(arg filters)"/>
      <arg name="clip_distance"            value="$(arg clip_distance)"/>
      <arg name="linear_accel_cov"         value="$(arg linear_accel_cov)"/>
      <arg name="initial_reset"            value="$(arg initial_reset)"/>
      <arg name="unite_imu_method"         value="$(arg unite_imu_method)"/>
      <arg name="topic_odom_in"            value="$(arg topic_odom_in)"/>
      <arg name="calib_odom_file"          value="$(arg calib_odom_file)"/>
      <arg name="publish_odom_tf"          value="$(arg publish_odom_tf)"/>
      <arg name="stereo_module/exposure/1" value="$(arg stereo_module/exposure/1)"/>
      <arg name="stereo_module/gain/1"     value="$(arg stereo_module/gain/1)"/>
      <arg name="stereo_module/exposure/2" value="$(arg stereo_module/exposure/2)"/>
      <arg name="stereo_module/gain/2"     value="$(arg stereo_module/gain/2)"/>

      <arg name="allow_no_texture_points"  value="$(arg allow_no_texture_points)"/>
      <arg name="ordered_pc"               value="$(arg ordered_pc)"/>
      
    </include>
  </group>
</launch>

3.录制Camera数据包

a) 启动RealSense的Camera节点

终端运行roslaunch realsense2_camera rs_d435i.launch，即运行我们上面编辑的launch文件，启动RealSense的Camera节点。

b) 可视化相机影像

方便起见，我们需要可视化相机影像，可以利用rqt_image_view工具来查看影像流，终端运行rosrun rqt_image_view rqt_image_view或者rqt_image_view即可。

我们需要标定的是D435i的两个红外摄像头，不过两个摄像头距离比较近，看到的东西差不多。所以开一个rqt_image_view，查看/camera/infra1/image_rect_raw或者/camera/infra2/image_rect_raw话题下的影像即可。

效果如下图所示：

c) 影像Topic重新发布

然后需要做的是，将传感器的Topic发送速率“转换”一下，因为在Kalibr的官方文档中，对于相机标定，推荐的速率是每秒4帧左右(FPS=4)，而D435i默认是30帧每秒，所以需要转换一下，不然标定时间会非常漫长。

转换使用ROS自带的topic_tools即可。新打开两个终端，分别在其中输入rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra1_image_rect_raw和rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra2_image_rect_raw即可。

这会将/camera/infra1/image_rect_raw和/camera/infra2/image_rect_raw的数据重新以4.0的频率在/infra1_image_rect_raw和/infra2_image_rect_raw下发布。

d) 录制Camera数据包

确认前几步都做好后，我们将标定板固定在某处，让相机对着标定板运动。

运动的方式为：先绕相机的各自三个轴，每个轴旋转三次，然后再沿着相机的三个轴，每个轴平移三次，基本就可以了，运动期间要保证相机基本能一直看到标定板的全部信息。

不过因为这是标定Camera，所以也可以将打印出来的标定板拿好，然后在相机前面来回运动，但因为笔者是用纸张打印的标定板，所以没采用这种方式。

通过如下命令录制camera数据包：

rosbag record -O camera_calibration /infra1_image_rect_raw /infra2_image_rect_raw

4.使用Kalibr进行标定

通过如下的命令运行kalibr进行标定：

kalibr_calibrate_cameras --target ./yaml/april_s.yaml --bag ./rosbag/camera_calibration.bag --bag-from-to 30 90 --models pinhole-radtan pinhole-radtan  --topics /infra1_image_rect_raw /infra2_image_rect_raw

--target：标定板参数信息存放的路径，即我们上面的april_s.yaml的路径。

--bag：录制的Camera数据包的路径。

--bag-from-to：起始和终止时间，单位是秒。为了避免开始录制和结束录制时的抖动，这里取了第30秒到第90秒之间的信息。

models：相机模型，一般为针孔模型，即pinhole-radtan。注意后面有几个话题前面就要写几次模型。

--topics：影像流发布的话题。

等待一段比较长的时间后会得到标定结果。

使用kalibr进行Camera-IMU联合标定

有了前几步的铺垫和积累，这里需要进行的工作并不是很多。

1.录制Camera-IMU数据包

a) 启动RealSense的Camera节点

终端运行roslaunch realsense2_camera rs_d435i.launch，即运行我们之前编辑的launch文件，启动RealSense的Camera节点。

b) 可视化相机影像

和标定Camera时一样，利用rqt_image_view工具来查看影像流，终端运行rosrun rqt_image_view rqt_image_view或者rqt_image_view即可。

我们需要标定的是D435i的两个红外摄像头，开一个rqt_image_view，查看/camera/infra1/image_rect_raw或者/camera/infra2/image_rect_raw话题下的影像即可。

c) 影像Topic重新发布

这里也与前面类似，但是对于相机-IMU标定，官方文档给出的建议帧率是：Camera 20FPS，IMU 200FPS。

转换依然使用ROS自带的topic_tools即可，新打开三个终端，分别在其中输入：

rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra1_image_rect_raw

rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra2_image_rect_raw

rosrun topic_tools throttle messages /camera/imu 200.0 /imu

d) 录制Camera数据包

确认前几步都做好后，我们将标定板固定在某处，让相机对着标定板运动，运动的方式是和之前一样的“三转、三移”。这里需要注意的是，一定要让相机对着标定板运动，因为我们需要标定imu，如果相机不动，imu的加速度就一直为0，无法进行标定。

通过如下命令录制camera数据包：

rosbag record -O camera_imu_calibration /infra1_image_rect_raw /infra2_image_rect_raw /imu

2修改IMU标定文件

我们之前使用imu_utils工具标定了IMU，但是得到标定文件是不能直接用的，还需要修改成kalibr指定的格式。

原来的格式如下所示：

type: IMU
name: D435i
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 1.5309108696273642e-03
      gyr_w: 2.0703289176602875e-05
   x-axis:
      gyr_n: 1.4720250697508204e-03
      gyr_w: 3.0849898119446267e-05
   y-axis:
      gyr_n: 1.7709433080656571e-03
      gyr_w: 2.0734874137051272e-05
   z-axis:
      gyr_n: 1.3497642310656153e-03
      gyr_w: 1.0525095273311089e-05
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 2.0588163388565360e-02
      acc_w: 7.8368470789407288e-04
   x-axis:
      acc_n: 1.7835189767520132e-02
      acc_w: 6.6321457518985427e-04
   y-axis:
      acc_n: 2.0230802690153690e-02
      acc_w: 6.7027913345637114e-04
   z-axis:
      acc_n: 2.3698497708022258e-02
      acc_w: 1.0175604150359932e-03

我们需要的是acc_n、acc_w、gyr_n和gyr_w这四个数据，将其对应的填到文件中即可，文件格式如下所示：

#Accelerometers
accelerometer_noise_density: 2.0588163388565360e-02   #Noise density (continuous-time)
accelerometer_random_walk:   7.8368470789407288e-04   #Bias random walk

#Gyroscopes
gyroscope_noise_density:     1.5309108696273642e-03   #Noise density (continuous-time)
gyroscope_random_walk:       2.0703289176602875e-05   #Bias random walk

rostopic:                		   /imu       #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

3.进行Camera-IMU联合标定

通过如下的命令运行kalibr进行标定：

kalibr_calibrate_imu_camera --target ./yaml/april_s.yaml  --cam ./result/camera/camchain-.rosbagcamera_calibration.yaml --imu ./result/imu/D435i_imu_param.yaml --bag rosbag/camera_imu_calibration.bag  --bag-from-to 20 130

--target：标定板参数信息存放的路径，即我们上面的april_s.yaml的路径。

--cam：Camera的标定文件路径。

--imu：IMU的标定文件路径。

--bag：录制的Camera-IMU数据包的路径。

--bag-from-to：起始和终止时间，单位是秒。为了避免开始录制和结束录制时的抖动，这里取了第30秒到第90秒之间的信息。

等待一段比较长的时间后会得到标定结果。

最终结果

运行结束后会弹出result report，并且kalibr会自动帮你保存文件。

我们最后会得到下图所示的四个文件，其中的yaml文件就是我们最后标定得到的结果。

这两个文件看起来是下面这种样子的：