当前位置:   article > 正文

Intel Realsense D455深度相机的标定及使用(二)——对内置IMU和双目相机进行标定_realsense 相机深度校准

realsense 相机深度校准

        标定前需先安装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)

位置1

         放置正确后终端会出现: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-dev

Ceres安装:

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 -j4

4、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>

    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <!--TOPIC名称和上面一致-->
        <param name="imu_topic" type="string" value= "/camera/imu"/>
        <!--imu_name 无所谓-->
        <param name="imu_name" type="string" value= "d455"/>
        <!--标定结果存放路径-->
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <!--数据录制时间-min-->
        <param name="max_time_min" type="int" value= "120"/>
        <!--采样频率,即是IMU频率,采样频率可以使用rostopic hz /camera/imu查看,设置为200,为后面的rosbag play播放频率-->
        <param name="max_cluster" type="int" value= "200"/>
    </node>
    
</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(懒得对比原文件了直接写个新的,后面联合标定也用这个,命名随意)。

  1. <launch>
  2. <arg name="serial_no" default=""/>
  3. <arg name="usb_port_id" default=""/>
  4. <arg name="device_type" default=""/>
  5. <arg name="json_file_path" default=""/>
  6. <arg name="camera" default="camera"/>
  7. <arg name="tf_prefix" default="$(arg camera)"/>
  8. <arg name="external_manager" default="false"/>
  9. <arg name="manager" default="realsense2_camera_manager"/>
  10. <arg name="output" default="screen"/>
  11. <arg name="respawn" default="false"/>
  12. <arg name="fisheye_width" default="-1"/>
  13. <arg name="fisheye_height" default="-1"/>
  14. <arg name="enable_fisheye" default="false"/>
  15. <arg name="depth_width" default="640"/>
  16. <arg name="depth_height" default="480"/>
  17. <arg name="enable_depth" default="true"/>
  18. <arg name="confidence_width" default="-1"/>
  19. <arg name="confidence_height" default="-1"/>
  20. <arg name="enable_confidence" default="true"/>
  21. <arg name="confidence_fps" default="-1"/>
  22. <arg name="infra_width" default="640"/>
  23. <arg name="infra_height" default="480"/>
  24. <arg name="enable_infra" default="false"/>
  25. <arg name="enable_infra1" default="true"/>
  26. <arg name="enable_infra2" default="true"/>
  27. <arg name="infra_rgb" default="false"/>
  28. <arg name="color_width" default="640"/>
  29. <arg name="color_height" default="480"/>
  30. <arg name="enable_color" default="true"/>
  31. <arg name="fisheye_fps" default="-1"/>
  32. <arg name="depth_fps" default="30"/>
  33. <arg name="infra_fps" default="30"/>
  34. <arg name="color_fps" default="30"/>
  35. <arg name="gyro_fps" default="200"/>
  36. <arg name="accel_fps" default="250"/>
  37. <arg name="enable_gyro" default="true"/>
  38. <arg name="enable_accel" default="true"/>
  39. <arg name="enable_pointcloud" default="false"/>
  40. <arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
  41. <arg name="pointcloud_texture_index" default="0"/>
  42. <arg name="allow_no_texture_points" default="false"/>
  43. <arg name="ordered_pc" default="false"/>
  44. <arg name="enable_sync" default="true"/>
  45. <arg name="align_depth" default="false"/>
  46. <arg name="publish_tf" default="true"/>
  47. <arg name="tf_publish_rate" default="0"/>
  48. <arg name="filters" default=""/>
  49. <arg name="clip_distance" default="-2"/>
  50. <arg name="linear_accel_cov" default="0.01"/>
  51. <arg name="initial_reset" default="false"/>
  52. <arg name="reconnect_timeout" default="6.0"/>
  53. <arg name="wait_for_device_timeout" default="-1.0"/>
  54. <arg name="unite_imu_method" default="linear_interpolation"/>
  55. <arg name="topic_odom_in" default="odom_in"/>
  56. <arg name="calib_odom_file" default=""/>
  57. <arg name="publish_odom_tf" default="true"/>
  58. <arg name="stereo_module/exposure/1" default="7500"/>
  59. <arg name="stereo_module/gain/1" default="16"/>
  60. <arg name="stereo_module/exposure/2" default="1"/>
  61. <arg name="stereo_module/gain/2" default="16"/>
  62. <group ns="$(arg camera)">
  63. <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
  64. <arg name="tf_prefix" value="$(arg tf_prefix)"/>
  65. <arg name="external_manager" value="$(arg external_manager)"/>
  66. <arg name="manager" value="$(arg manager)"/>
  67. <arg name="output" value="$(arg output)"/>
  68. <arg name="respawn" value="$(arg respawn)"/>
  69. <arg name="serial_no" value="$(arg serial_no)"/>
  70. <arg name="usb_port_id" value="$(arg usb_port_id)"/>
  71. <arg name="device_type" value="$(arg device_type)"/>
  72. <arg name="json_file_path" value="$(arg json_file_path)"/>
  73. <arg name="enable_pointcloud" value="$(arg enable_pointcloud)"/>
  74. <arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/>
  75. <arg name="pointcloud_texture_index" value="$(arg pointcloud_texture_index)"/>
  76. <arg name="enable_sync" value="$(arg enable_sync)"/>
  77. <arg name="align_depth" value="$(arg align_depth)"/>
  78. <arg name="fisheye_width" value="$(arg fisheye_width)"/>
  79. <arg name="fisheye_height" value="$(arg fisheye_height)"/>
  80. <arg name="enable_fisheye" value="$(arg enable_fisheye)"/>
  81. <arg name="depth_width" value="$(arg depth_width)"/>
  82. <arg name="depth_height" value="$(arg depth_height)"/>
  83. <arg name="enable_depth" value="$(arg enable_depth)"/>
  84. <arg name="confidence_width" value="$(arg confidence_width)"/>
  85. <arg name="confidence_height" value="$(arg confidence_height)"/>
  86. <arg name="enable_confidence" value="$(arg enable_confidence)"/>
  87. <arg name="confidence_fps" value="$(arg confidence_fps)"/>
  88. <arg name="color_width" value="$(arg color_width)"/>
  89. <arg name="color_height" value="$(arg color_height)"/>
  90. <arg name="enable_color" value="$(arg enable_color)"/>
  91. <arg name="infra_width" value="$(arg infra_width)"/>
  92. <arg name="infra_height" value="$(arg infra_height)"/>
  93. <arg name="enable_infra" value="$(arg enable_infra)"/>
  94. <arg name="enable_infra1" value="$(arg enable_infra1)"/>
  95. <arg name="enable_infra2" value="$(arg enable_infra2)"/>
  96. <arg name="infra_rgb" value="$(arg infra_rgb)"/>
  97. <arg name="fisheye_fps" value="$(arg fisheye_fps)"/>
  98. <arg name="depth_fps" value="$(arg depth_fps)"/>
  99. <arg name="infra_fps" value="$(arg infra_fps)"/>
  100. <arg name="color_fps" value="$(arg color_fps)"/>
  101. <arg name="gyro_fps" value="$(arg gyro_fps)"/>
  102. <arg name="accel_fps" value="$(arg accel_fps)"/>
  103. <arg name="enable_gyro" value="$(arg enable_gyro)"/>
  104. <arg name="enable_accel" value="$(arg enable_accel)"/>
  105. <arg name="publish_tf" value="$(arg publish_tf)"/>
  106. <arg name="tf_publish_rate" value="$(arg tf_publish_rate)"/>
  107. <arg name="filters" value="$(arg filters)"/>
  108. <arg name="clip_distance" value="$(arg clip_distance)"/>
  109. <arg name="linear_accel_cov" value="$(arg linear_accel_cov)"/>
  110. <arg name="initial_reset" value="$(arg initial_reset)"/>
  111. <arg name="reconnect_timeout" value="$(arg reconnect_timeout)"/>
  112. <arg name="wait_for_device_timeout" value="$(arg wait_for_device_timeout)"/>
  113. <arg name="unite_imu_method" value="$(arg unite_imu_method)"/>
  114. <arg name="topic_odom_in" value="$(arg topic_odom_in)"/>
  115. <arg name="calib_odom_file" value="$(arg calib_odom_file)"/>
  116. <arg name="publish_odom_tf" value="$(arg publish_odom_tf)"/>
  117. <arg name="stereo_module/exposure/1" value="$(arg stereo_module/exposure/1)"/>
  118. <arg name="stereo_module/gain/1" value="$(arg stereo_module/gain/1)"/>
  119. <arg name="stereo_module/exposure/2" value="$(arg stereo_module/exposure/2)"/>
  120. <arg name="stereo_module/gain/2" value="$(arg stereo_module/gain/2)"/>
  121. <arg name="allow_no_texture_points" value="$(arg allow_no_texture_points)"/>
  122. <arg name="ordered_pc" value="$(arg ordered_pc)"/>
  123. </include>
  124. </group>
  125. </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和相机标定的结果,参照下面的格式编写:

  1. cam0:
  2. camera_model: pinhole
  3. distortion_coeffs: [0.006252205171105866, -0.0040039163191851815, 0.0028253923865729047, 0.0014894484127909796]
  4. distortion_model: radtan
  5. intrinsics: [387.3061040278026, 388.70426392594715, 321.2185553432099, 243.710879951254]
  6. resolution: [640, 480]
  7. rostopic: /infra_left
  8. cam1:
  9. T_cn_cnm1:
  10. - [0.9999988621914934, -0.0012557567512840304, 0.0008358771987739831, -0.09586169575038382]
  11. - [0.0012560162153501813, 0.9999991631746745, -0.0003099567850981058, -0.0002414148329795646]
  12. - [-0.0008354872689652846, 0.000311006307742342, 0.9999996026179666, -0.0001722652642823169]
  13. - [0.0, 0.0, 0.0, 1.0]
  14. camera_model: pinhole
  15. distortion_coeffs: [0.006593825120537277, -0.008671743399946441, 0.0037681282760781776, 0.0014848895024164256]
  16. distortion_model: radtan
  17. intrinsics: [388.18404676436523, 389.27254650876796, 321.4035775541331, 243.8629884469389]
  18. resolution: [640, 480]
  19. 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相机录制的数据包。

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/Cpp五条/article/detail/250743
推荐阅读
相关标签
  

闽ICP备14008679号