相机如何标定（Intel Realsense D455深度相机的标定及使用（二）——对内置IMU和双目相机进行标定）
标定前需先安装librealsense SDK2.0以及realsense-ros ，可参考教程：Intel Realsense D455深度相机的标定及使用（一）——安装librealsense SDK2.0以及realsense-ros
三 、IMU标定
1 、重力加速度自检
插入相机并静置 ， 终端输入realsense-viewer ，开启realsense-viewer左侧的Motion Module模块 ，将鼠标放在加速度计（Accel stream）上 ，观察g_norm读数是否在9.8附近 。
2 、 利用官方的rs_imu_calibration.py工具进行IMU自校准
g_norm读数正常的 ，请跳过此步骤 ，直接进入步骤3 ；g_norm读数不正常的 ，利用官方的rs_imu_calibration.py工具进行IMU自校准 ，详情参考官方网站：
https://dev.intelrealsense.com/docs/imu-calibration-tool-for-intel-realsense-depth-camera
我将官方文档中Ubuntu系统下的大致步骤列出如下 ，英文不好的可以做个参照（window请看源文档） 。
（a）下载rs_imu_calibration.py脚本
https://github.com/IntelRealSense/librealsense
通过上面的链接下载 ，下载后在tools/rs_imu_calibration文件夹里可以找到脚本文件rs_imu_calibration.py 。
（b）构建Python环境及其他依赖包
检查Python版本 ，通过命令：python --version
这边使用的是Python2 ，如果2和3都安装的 ，运行如下命令进行手动切换 。
sudo update-alternatives --config python
显示如下 ，键入python3的序号（这里是2） ，回车 。
有 2 个候选项可用于替换 python (提供 /usr/bin/python) 。
选择 路径 优先级 状态
------------------------------------------------------------
* 0 /usr/bin/python3 150 自动模式
1 /usr/bin/python2 100 手动模式
2 /usr/bin/python3 150 手动模式要维持当前值[*]请按<回车键> ，或者键入选择的编号：
注意：此方法不会修改Python的优先级 ，也就是之后还会切换回Python3 。如果想更换Python2为默认环境 ，需要sudo update-alternatives --install /usr/bin/python python /usr/bin/python2 150加上sudo update-alternatives --install /usr/bin/python python /usr/bin/python3 100命令来修改优先级 。两种方法二选一！！！
有些依赖或者软件之前安装过 ，以防万一 ，建议还是install一下 。
sudo apt-get install python-pip
sudo pip install numpy
sudo pip install pyrealsense2
（c）运行脚本
插上相机 ，进入rs_imu_calibration.py文件所在文件夹（我的在~/Packages/librealsense-master/tools/rs-imu-calibration下） ，运行
python rs-imu-calibration.py
出现如下界面 ，并不停地有读数进来 。
（d）从6个位置捕获IMU数据
将相机以下图所示位置摆放（螺纹孔朝下 ，懒得自己照相了 ，图用的是官方文件中的D435i）
放置正确后终端会出现：Status.collect_data[...................]] ，期间不要移动相机 ，直到省略号满格出现Direction data collected 。若不小心移动 ，会出现Status.collect_dataWARNING: MOVING的警告 ，此时摆正位置不要移动直到出现Direction data collected
出现Direction data collected后翻转相机到下一个位置 。依次在位置2~6重复操作：
位置2
位置3
位置4
位置5
位置6
（f）更新数据
6个位置都捕获完成后 ，会提示
意思是是否保存原数据 ，按回车不保存原数据 。然后会计算出重力加速度值：
计算完后会提示：Would you like to write the results to the camera? (Y/N) 。键入Y写入到设备
3 、下载编译用于IMU标定的code_utils和imu_utils
（1）Ceres安装
依赖安装：
sudo apt-get install cmake
sudo apt-get install libgoogle-glog-dev
sudo apt-get install libatlas-base-dev
sudo apt-get install libeigen3-dev
sudo apt-get install libsuitesparse-devCeres安装：
git clone https://github.com/ceres-solver/ceres-solver.git
cd ceres-solver
mkdir build
cd build
cmake ..
make -j4
sudo make install（2）下载编译code_utils
mkdir -p ~/Calibration/IMU_ws/src ##可自定义
cd ~/Calibration/IMU_ws/src
git clone https://github.com/gaowenliang/code_utils.git
cd ..
catkin_make -j4报错解决1：error：libdw.h没有那个文件或目录
解决措施：sudo apt-get install libdw-dev
报错解决2：error：backward.hpp没有那个文件或目录
解决措施：将sumpixel_test.cpp中# include "backward.hpp"
改为：#include "code_utils/backward.hpp"
报错解决3：error：‘integer_sequence’ is not a member of ‘std’
解决措施： 将CMakeLists.txt中的 set(CMAKE_CXX_FLAGS "-std=c++11") 改成 set(CMAKE_CXX_STANDARD 14)
（3）下载编译imu_utils
cd ~/Calibration/IMU_ws/src ##和code_utils的工作空间保持一致
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make -j44 、IMU标定
（1）修改launch文件并启动相机
在~/Packages/RealsenseRos_ws/src/realsense-ros/realsense2_camera/launch文件夹下（路径仅供参考 ，具体参照之前realsense-ros的工作空间） ，复制rs_camera.launch到同一个目录下 ，并重命名为rs_imu_calibration.launch(命名随意) 。并将新文件中的<arg name="unite_imu_method" default=""/>修改为
<arg name="unite_imu_method" default="linear_interpolation"/>
这样做的目的是将accel和gyro的数据合并得到imu话题 ，不这样做发布的topic没有合并的camera/imu话题 。
用新launch文件启动相机
roslaunch realsense2_camera rs_imu_calibration.launch
使用rostopic list查看是否有/camera/imu话题发布 。
（2）录制imu数据包
在保存bag包的路径打开终端 ，静置相机 ，终端输入：
rosbag record -O imu_calibration /camera/imu
放置时间建议大于2h（官方建议） 。
（3）编写启动文件
cd ~/Calibration/IMU_ws/src/imu_utils/launch ##路径仅供参考
gedit d455_imu_calibration.launch
写入
<launch>
</launch>
（4）imu标定
在刚才imu_utils的工作空间下打开终端（我的是~/Calibration/IMU_ws） ，运行
source ./devel/setup.bash
roslaunch imu_utils d455_imu_calibration.launch
回放数据包,打开新的终端
cd 存放imu_calibration.bag的路径
rosbag play -r 200 imu_calibration.bag标定完成后在~/Calibration/IMU_ws/src/imu_utils/data下查看 ，主要看d455_imu_param.yaml文件 ，文件内容如下（结果仅供参考）：
可将标定结果移动到自己创建的文件夹里 ，后续的标定结果也放在这里（我的路径是~/Calibration/D455/imu_calibration） 。
四 、多相机标定
1 、kalibr安装编译
kalibr的安装编译与版本环境有很大关系 ，不同版本的安装差异很大 ，建议参照官网教程：
https://github.com/ethz-asl/kalibr/wiki/installation
2 、标定板制作或淘宝购买
在kalibr的工作空间下打开终端：
source ./devel/setup.bash
#生成标定板PDF文件（这里是以A4纸的规格生成的）
rosrun kalibr kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.022 --tspace 0.3
在工作空间下会找到PDF文件 ，使用A4纸100%缩放打印出来 ，量一下大格子的尺寸是否是0.022m（2.2cm） ，贴在墙上 。
参照官方格式编写yaml文件保存到自己放置自己标定结果的文件夹（~/Calibration/D455）target_type: aprilgrid #gridtype
tagCols: 6 #number of apriltags 列数
tagRows: 6 #number of apriltags 行数
tagSize: 0.022 #size of apriltag, edge to edge [m] 大格子尺寸 ，单位为米
tagSpacing: 0.3 #ratio of space between tags to tagSize 小格子与大格子尺寸的比值#example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]
3 、修改相机启动文件
在~/Packages/RealsenseRos_ws/src/realsense-ros/realsense2_camera/launch文件夹下（路径仅供参考 ，具体参照之前realsense-ros的工作空间） ，创建文件rs_imu_stereo_calibration_640.launch(懒得对比原文件了直接写个新的 ，后面联合标定也用这个 ，命名随意) 。
<launch> <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="false"/> <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="-1"/> <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="false"/> <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="reconnect_timeout" default="6.0"/> <arg name="wait_for_device_timeout" default="-1.0"/> <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)"> <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="reconnect_timeout" value="$(arg reconnect_timeout)"/> <arg name="wait_for_device_timeout" value="$(arg wait_for_device_timeout)"/> <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>4 、启动相机并关闭结构光
roslaunch realsense2_camera rs_imu_stereo_calibration_640.launch
rosrun rqt_reconfigure rqt_reconfigure将图中红圈部分改为OFF
5 、打开rviz并修改相机帧数
新终端运行roscore 和 rviz ，点击add添加双目对应的topic ，/camera/color/image_raw 、/camera/infra1/image_rect_raw 、/camera/infra2/image_rect_raw
按照官方建议修改相机帧数为4Hz ，并命名为新话题（此方法会使得发布频率略小于4Hz ，报告影响不大） 。
rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_left
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_right查看实际频率
rostopic hz /color /infra_left /infra_right
6 、录制bag包
在bag保存路径打开终端
rosbag record -O camera_calibration_4Hz /infra_left /infra_right /color
录制时的要求：通过rviz界面观察 ，手持相机对准标定板 。偏航角左右摆动3次 ，俯仰角摆动3次 ，滚转角摆动3次 ，上下移动3次 ，左右移动3次 ，前后移动3次 ，然后自由移动一段时间 ，摆动幅度要大一点 ，让视角变化大一点 ，但是移动要缓慢一点 ，同时要保证标定板在3个相机视野内部 ，整个标定时间要在90s以上更好 。
7 、使用kalibr标定
cd ~/Calibration/Kalibr_ws #路径仅供参考
source ./devel/setup.bash关于第三句命令 ， 需要根据自己的情况修改 。修改说明如下
首先是rosrun kalibr kalibr_calibrate_cameras ，
第二段--target ~/Calibration/D455/Aprilgrid.yaml（改为自己标定板yaml文件的路径） ，
第三段--bag ~/Dataset/Calibration_D455/640_480/camera_calibration_4Hz.bag ，
第四五六段--models pinhole-radtan pinhole-radtan pinhole-radtan这句话不用改 ，表示三个摄像头的相机模型和畸变模型（详细解释参考Supported models · ethz-asl/kalibr Wiki · GitHub）
第七段--topics /infra_left /infra_right /color话题名称 ，按我之前步骤来的不用改 ，
第八段----bag-from-to 5 169 表示处理bag中5-169秒的数据 ，根据自己录制的bag来确定（通过rosbag info+bag名称 来查看） ，
第九段--show-extraction表示显示检测特征点的过程
第十段----approx-sync 0.04是三个相机时间同步的容许误差（可以不加这段试试 ，我的会标定失败）
综合起来 ，对于我自己的情况 ，完整命令如下：
rosrun kalibr kalibr_calibrate_cameras --target /home/andyvictory/Calibration/D455/Aprilgrid.yaml --bag ~/Dataset/Calibration_D455/640_480/camera_calibration_4Hz.bag --models pinhole-radtan pinhole-radtan pinhole-radtan --topics /infra_left /infra_right /color --bag-from-to 5 169 --show-extraction --approx-sync 0.04
标定过程可能有点漫长................
如果出现报错：cannot import name NavigationToolbar2Wx
将PlotCollection.py中的NavigationToolbar2Wx换成NavigationToolbar2WxAgg
最终得到的三个结果文件保存在了kalibr的工作空间下 ，将文件移动到自己保存结果的文件夹里 。看一下camera_calibration_4Hz-report-cam.pdf中的三个相机重投影误差reprojection error ，至少也得在1px以内吧 ，下面是我标定的结果 。
五 、IMU+双目相机联合标定
1 、编写chain.yaml文件
编写chain.yaml并保存（能找到就行 ，我还是放在了~/Calibration/D455/640_480下） ，根据自己之前IMU和相机标定的结果 ，参照下面的格式编写：
cam0: camera_model: pinhole distortion_coeffs: [0.006252205171105866, -0.0040039163191851815, 0.0028253923865729047, 0.0014894484127909796] distortion_model: radtan intrinsics: [387.3061040278026, 388.70426392594715, 321.2185553432099, 243.710879951254] resolution: [640, 480] rostopic: /infra_left cam1: T_cn_cnm1: - [0.9999988621914934, -0.0012557567512840304, 0.0008358771987739831, -0.09586169575038382] - [0.0012560162153501813, 0.9999991631746745, -0.0003099567850981058, -0.0002414148329795646] - [-0.0008354872689652846, 0.000311006307742342, 0.9999996026179666, -0.0001722652642823169] - [0.0, 0.0, 0.0, 1.0] camera_model: pinhole distortion_coeffs: [0.006593825120537277, -0.008671743399946441, 0.0037681282760781776, 0.0014848895024164256] distortion_model: radtan intrinsics: [388.18404676436523, 389.27254650876796, 321.4035775541331, 243.8629884469389] resolution: [640, 480] rostopic: /infra_right其中 ，T_cn_cnm1:表示的是左目相机到右目相机的旋转和平移 。
2 、录制bag包
参照之前录制相机标定bag的步骤：
roscore
rviz
roslaunch realsense2_camera rs_imu_stereo_calibration_640.launch
rosrun rqt_reconfigure rqt_reconfigure ##关闭结构光修改相机和IMU发布的频率（这里录制的画面包括了/color相机 ，我这边是为了以后使用 ，要是不需要 ，只用于标定的话可以不录制color的话题即把下面第三句删掉）：
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 20.0 /infra_left
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 20.0 /infra_right
rosrun topic_tools throttle messages /camera/color/image_raw 20.0 /color
rosrun topic_tools throttle messages /camera/imu 200.0 /imu在bag的保存路径打开终端并录制（只录制两个相机画面的这里不需要/color）：
rosbag record -O imu_stereo_640.bag /infra_left /infra_right /color /imu
3 、联合标定
cd ~/Calibration/Kalibr_ws #路径仅供参考
source ./devel/setup.bash运行命令（参照之前相机标定过程修改）
rosrun kalibr kalibr_calibrate_imu_camera --bag ~/Dataset/Calibration_D455/640_480/imu_stereo_640.bag --cam ~/Calibration/D455/640_480/second/chain.yaml --imu ~/Calibration/D455/IMU/imu.yaml --target ~/Calibration/D455/Aprilgrid.yaml --bag-from-to 5 136 --show-extraction
标定结果的四个文件还是放在kalibr的工作空间下 ，打开imu_stereo_640-results-imucam.txt查看 ，重投影误差Reprojection error ，两个相机的mean都起码都得在0.5以下 。
至此 ，相机和IMU各自的标定和联合标定的工作都已完成 ，下一步就是根据标定结果跑自己的算法了 。下一篇将以VINS-Fusion算法运行自己用Realsense D455相机录制的数据包 。
创心域SEO版权声明：以上内容作者已申请原创保护，未经允许不得转载，侵权必究！授权事宜、对本内容有异议或投诉，敬请联系网站管理员，我们将尽快回复您，谢谢合作！