一、IMU内参标定
1.1 安装code_utils
mkdir -p imu_ws/src
cd imu_ws/src
git clone https://github.com/gaowenliang/code_utils
cd ..
catkin_make
问题1:
fatal error: backward.hpp: 没有那个文件或目录
解决措施:
修改/home/jetson/imu_ws/src/code_utils/src/sumpixel_test.cpp,将:
#include "backward.hpp"
修改为:
#include "code_utils/backward.hpp"
问题2:
error: ‘CV_LOAD_IMAGE_UNCHANGED’ was not declared in this scope
解决措施:
OpenCV4.x版本以上,有些函数的名称修改了,具体如下:
CV_LOAD_IMAGE_UNCHANGED 改为 cv::IMREAD_UNCHANGED
CV_LOAD_IMAGE_GRAYSCALE 改为 cv::IMREAD_GRAYSCALE
CV_LOAD_IMAGE_COLOR 改为 cv::IMREAD_COLOR
CV_LOAD_IMAGE_ANYDEPTH 改为 cv::IMREAD_ANYDEPTH
CV_MINMAX改成NORM_MINMAX
1.2 安装imu_utils
cd src
git clone https://github.com/gaowenliang/imu_utils
cd ..
catkin_make #编译imu_utils
问题1:
error: aggregate ‘std::ofstream out_t’ has incomplete type and cannot be defined
解决措施:
在对应文件中添加如下头文件:
#include <fstream>
1.3 内参标定
首先新建launch文件,命名为d455_imu_calibration.launch,并将rs_camera.launch中的参数完全移植过来,作出如下修改:
<arg name="gyro_fps" default="200"/>
<arg name="accel_fps" default="200"/>
<arg name="enable_gyro" default="true"/>
<arg name="enable_accel" default="true"/>
<arg name="unite_imu_method" default="linear_interpolation"/>
文件整体内容为:
<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="848"/>
<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="848"/>
<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="848"/>
<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>
运行roslaunch realsense2_camera d455_imu_calibration.launch后可以看到/camera/imu的话题。并使用
rosbag record -O imu_calibration /camera/imu
对话题进行录制。录制时长应该在2h以上,期间应该保持相机静止不动。
在/home/jetson/imu_ws/src/imu_utils/launch目录下创建d455_imu.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= "50"/>
<!--采样频率,即是IMU频率,采样频率可以使用rostopic hz /camera/imu查看,设置为200,为后面的rosbag play播放频率-->
<param name="max_cluster" type="int" value= "200"/>
</node>
</launch>
在imu_ws工作空间下使用
roslaunch imu_utils d455_imu.launch
并播放录制好的rosbag
cd 存放imu_calibration.bag的路径
rosbag play -r 200 imu_calibration.bag
即可得到标定结果。标定完成后在/home/jetson/imu_ws/src/imu_utils/data下查看,主要看d455_imu_param.yaml文件,文件内容如下(结果仅供参考):
%YAML:1.0
---
type: IMU
name: d455
Gyr:
unit: " rad/s"
avg-axis:
gyr_n: 2.9607246176650919e-03
gyr_w: 3.1833671764598549e-05
x-axis:
gyr_n: 3.2107485622757540e-03
gyr_w: 3.1031741697968581e-05
y-axis:
gyr_n: 4.0558334112516994e-03
gyr_w: 4.4750594899819731e-05
z-axis:
gyr_n: 1.6155918794678217e-03
gyr_w: 1.9718678696007337e-05
Acc:
unit: " m/s^2"
avg-axis:
acc_n: 1.2016768615517539e-02
acc_w: 2.9698273035189197e-04
x-axis:
acc_n: 1.2783173823264504e-02
acc_w: 3.1888476690015229e-04
y-axis:
acc_n: 1.1879408974696542e-02
acc_w: 3.2179191741915538e-04
z-axis:
acc_n: 1.1387723048591571e-02
acc_w: 2.5027150673636830e-04
二、D455双目相机外参标定
2.1 kalibr标定功能包安装
参考如下的博客
Ubuntu20.04安装kalibr_ubuntu20.04 kalibr_可即的博客-CSDN博客
编译可以按照步骤来,但是下载的话直接去官网选择kalibr-master安装并解压,要不然后续会出现很多奇怪的问题。
编译完成后运行指令测试
rosrun kalibr kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.022 --tspace 0.3
本应该在工作空间下生成pdf,但是现在没有任何反应并出现如下报错:
Traceback (most recent call last):
File "/home/jetson/kalibr_ws/devel/lib/kalibr/kalibr_create_target_pdf", line 15, in <module>
exec(compile(fh.read(), python_script, 'exec'), context)
File "/home/jetson/kalibr_ws/src/kalibr/aslam_offline_calibration/kalibr/python/kalibr_create_target_pdf", line 5, in <module>
from pyx import *
ModuleNotFoundError: No module named 'pyx'
解决措施:
pyx是python中绘制图形所需要的依赖项,通过如下命令:
pip install Cython
pip install pyx
即可解决。
解决问题1后,会产生报错2,报错2如下:
FileNotFoundError: [Errno 2] No such file or directory: 'tex'
解决措施:这是报错无法生成pdf文件,需要通过如下命令进行安装
参考:
kalibr的编译bug_kalibr_create_target_pdf:未找到命令_天际的鸟的博客-CSDN博客
sudo pip install pyx
sudo apt-get install python3-pyx
2.2 标定过程
Intel Realsense D455深度相机的标定及使用(二)——对内置IMU和双目相机进行标定_realsense标定_AndyVictory的博客-CSDN博客
编写Aprilgrid.yaml,我的路径是/home/jetson/d455_params/Aprilgrid.yaml
内容如下:这里是给出标定板的参数,提供绝对参考
target_type: 'aprilgrid' #gridtype
tagCols: 6 #number of apriltags 列数
tagRows: 6 #number of apriltags 行数
tagSize: 0.0352 #size of apriltag, edge to edge [m] 大格子尺寸,单位为米
tagSpacing: 0.3 #ratio of space between tags to tagSize 小格子与大格子尺寸的比值
我自己的情况,标定程序的命令为:注意,参考博客里用的图像大小是640x480,我这里改成了848x480,方便后续vins-fusion的参数匹配。其实影响不是很大。
rosrun kalibr kalibr_calibrate_cameras --target /home/jetson/d455_params/Aprilgrid.yaml --bag /home/jetson/kalibr_ws/camera_calibration_4Hz.bag --models pinhole-radtan pinhole-radtan pinhole-radtan --topics /infra_left /infra_right /color --bag-from-to 3 209 --show-extraction --approx-sync 0.1
运行时出现如下报错:
File "/home/jetson/kalibr_ws/src/kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras", line 4, in <module>
import cv_bridge
SystemError: initialization of cv_bridge_boost raised unreported exception
这是因为对应python文件导入库顺序的问题:[ROS2报错问题]SystemError: initialization of cv_bridge_boost raised unreported exception_Black__Jacket的博客-CSDN博客
解决措施如下:
在该文件中写入import cv2,import cv2一定要在import cv_bridge之前。
如果还有别的类似的库的问题,可以参考以下博客:
使用Kalibr问题汇总:ModuleNotFoundError: No module named ‘wx‘_kkmd66的博客-CSDN博客
在标定过程中,可能会提示:
Failed to load module "canberra-gtk-module"
只需要安装环境即可。这里忽略这个报错好像也无所谓,我是害怕影响标定精度。
sudo apt-get install libcanberra-gtk-module
问题3,
解决措施:
更改rosrun启动命令中的最后的参数--appros-sync为0.1即可
ZED2相机标定--双目、IMU、联合标定_zed2标定_可即的博客-CSDN博客
问题4:
RuntimeError: [CameraChainParameters Reader]: Could not write configuration to camchain-/home/jetson/camera_calibration_4Hz.yaml
这个问题产生于之前git kalibr的时候没有选择kalibr-master。因此只需要从官网上下载kalibr-master,直接提取到工作空间的src下编译即可。
2.3 Notice
cam0是左目相机,cam1是右目相机,cam2是rgb的单目彩色相机,标定完成后会在kalibr_ws下生成三个文件,如下图所示。标定结果在camera_calibration_4Hz-camchain.yaml中。
三、IMU和相机标定
3.1 参数文件
参考2.2的链接博客,编写chain.yaml文件:
Intel Realsense D455深度相机的标定及使用(二)——对内置IMU和双目相机进行标定_realsense标定_AndyVictory的博客-CSDN博客
基于第二章的标定结果,我的chain.yaml文件编写如下,并放在/home/jetson/d455_params这一路径下:
cam0:
camera_model: pinhole
distortion_coeffs: [0.0046385247795052405, -0.004370670319697626, -0.0006128880407912631, -0.0007154563512153851]
distortion_model: radtan
intrinsics: [391.5332352221955, 392.02975317193886, 323.0825200042921, 240.07762864000276]
resolution: [640, 480]
rostopic: /infra_left
cam1:
T_cn_cnm1:
- [0.9999990823569973, -0.00044451149430390486, -0.0012797244591537737, -0.09523434445323715]
- [0.0004412066799845757, 0.9999965704178216, -0.0025815672094267996, -0.0004369319578970591]
- [0.001280867606531486, 0.0025810002174897996, 0.9999958488994084, -0.004377818916151]
- [0.0, 0.0, 0.0, 1.0]
camera_model: pinhole
distortion_coeffs: [0.004157651324150097, -0.0033820573067231694, 4.5574416505251825e-05, -0.00022604395241497698]
distortion_model: radtan
intrinsics: [387.0238679583512, 387.753774950445, 324.1489569790256, 241.78685526070177]
resolution: [640, 480]
rostopic: /infra_right
编写imu.yaml文件,根据imu内参的标定结果可以编写如下imu.yaml文件:
#Accelerometers
accelerometer_noise_density: 2.0477290485501922e-02 #Noise density (continuous-time)
accelerometer_random_walk: 4.2308969579290693e-04 #Bias random walk
#Gyroscopes
gyroscope_noise_density: 2.2488785808195085e-03 #Noise density (continuous-time)
gyroscope_random_walk: 1.5385085422768701e-05 #Bias random walk
rostopic: /imu #the IMU ROS topic
update_rate: 200.0 #Hz (for discretization of the values above)
我所参考的内参标定结果是:
Gyr:
unit: " rad/s"
avg-axis:
gyroscope_noise_density: 2.9607246176650919e-03
gyroscope_random_walk: 3.1833671764598549e-05
Acc:
unit: " m/s^2"
avg-axis:
accelerometer_noise_density: 1.1387723048591571e-02
accelerometer_random_walk: 2.5027150673636830e-04
3.2 录制rosbag包
参考之前的步骤,录制步骤基本相同,
roscore
rviz
roslaunch realsense2_camera rs_imu_stereo_calibration_640.launch
rosrun rqt_reconfigure rqt_reconfigure ##关闭结构光
修改相机和IMU的发布频率:
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
录制:
rosbag record -O imu_stereo_640.bag /infra_left /infra_right /color /imu
3.3 联合标定
在kalibr_ws工作空间下先source,再运行如下程序:
一定一定注意修改成自己的路径
rosrun kalibr kalibr_calibrate_imu_camera --bag /home/jetson/d455_params/imu_stereo_640.bag --cam /home/jetson/d455_params/chain.yaml --imu /home/jetson/d455_params/imu.yaml --target /home/jetson/d455_params/Aprilgrid.yaml --bag-from-to 5 258 --show-extraction
结果同样会生成在kalibr_ws工作空间下,可以直接查看。
标定结果如下:
和相机相同,标定的结果最终在imu_stereo_640-camchain-imucam.yaml文件中。