Realsence D455标定并运行Vins-Fusion-Toy模板网

这篇具有很好参考价值的文章主要介绍了Realsence D455标定并运行Vins-Fusion。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

一、双目相机标定

1. 标定板准备

1.1 打印标定板

地址：https://github.com/ethz-asl/kalibr/wiki/downloads
Realsence D455标定并运行Vins-Fusion 下载，然后直接用A4纸就可以打印出来（有条件去淘宝买一个视觉标定板，标出来的误差会更小）

1.2 标定板信息

原始pdf的格子参数是：

Realsence D455标定并运行Vins-Fusion

调整后的格子参数是：

新建april_6x6_A4.yaml文件，内容展示如下：

target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.021           #size of apriltag, edge to edge [m]
tagSpacing: 0.3          # 小格子与大格子边长比例
codeOffset: 0            #code offset for the first tag in the aprilboard

千万要自己测量大格子边长，即tagSize

2. 左右目相机数据准备

在这之前需要安装realsence的sdk和ros，参考博客：Intel Realsense D455深度相机的标定及使用（一）——安装librealsense SDK2.0以及realsense-ros

2.1 修改rs_camera.launch内容

（1）打开双目

 <arg name="enable_infra1"       default="true"/>
 <arg name="enable_infra2"       default="true"/>

（2）打开imu的加速度计和陀螺仪

  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>

并合并为一个topic

<arg name="unite_imu_method"          default="linear_interpolation"/>

（3）传感器同步

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

当然也可以修改图像分辨率(我选择默认了)：

  <arg name="infra_width"         default="848"/>
  <arg name="infra_height"        default="480"/>

rs_camera.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="-1"/>
  <arg name="depth_height"        default="-1"/>
  <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="-1"/>
  <arg name="color_height"        default="-1"/>
  <arg name="enable_color"        default="true"/>

  <arg name="fisheye_fps"         default="-1"/>
  <arg name="depth_fps"           default="-1"/>
  <arg name="infra_fps"           default="30"/>
  <arg name="color_fps"           default="-1"/>
  <arg name="gyro_fps"            default="-1"/>
  <arg name="accel_fps"           default="-1"/>
  <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>

2.2 关闭结构光

默认开始IR结构光时，双目图像会有很多点，这些点可能对标定有影响，所以使用时需要关闭结构光。
终端1：

source /home/lessle6/Packages/RealsenseRos_ws/devel/setup.bash
roslaunch /home/lessle6/Packages/RealsenseRos_ws/src/realsense-ros/realsense2_camera/launch/rs_camera.launch

再打开一个终端2：

rosrun rqt_reconfigure rqt_reconfigure

打开后将camera->stereo_module中的emitter_enabled设置为off(0) ，展示如下：
Realsence D455标定并运行Vins-Fusion

2.3 可视化双目图像

新打开终端，运行rviz

rviz

修改Fiexd Fram 为camera_link
Realsence D455标定并运行Vins-Fusion

之后在里面add rgb和双目对应的topic，/camera/color/image_raw、/camera/infra1/image_rect_raw、/camera/infra2/image_rect_raw展示如下：
Realsence D455标定并运行Vins-Fusion 之后固定标定版，尝试移动标定板，同时要确保标定板一直在三个图像当中。录制过程参考https://www.youtube.com/watch?v=puNXsnrYWTY&app=desktop
需要科学上网观看
总结下来就是偏航角左右摆动3次，俯仰角摆动3次，滚转角摆动3次，上下移动3次，左右移动3次，前后移动3次，然后自由移动一段时间，摆动幅度要大一点，让视角变化大一点，但是移动要缓慢一点，同时要保证标定板在3个相机视野内部，整个标定时间要在90s以上更好。

2.4 修改相机播包帧数

修改相机帧数（官方推荐是4Hz减少冗余帧，加快计算速度）
Realsence D455标定并运行Vins-Fusion 不过有时候updatesize之后发现两个双目时间辍总是差点东西：
后来联标完成，看时间辍延迟不太对发现的。不过大多数都一致～

kalibr在处理标定数据的时候要求频率不能太高，一般为4Hz，我们可以使用如下命令来更改topic的频率，实际上是将原来的topic以新的频率转成新的topic，实际测试infra1才是对应左目相机。

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

2.5 录制数据

rosbag record -O multicameras_calibration /infra_left /infra_right /color

multicameras_calibration.bag为录制数据

3. kalibr 程序标定

这里需要安装kalibr

source /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/devel/setup.bash

cd  /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace

rosrun kalibr kalibr_calibrate_cameras \
--target /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/data/april_6x6_A4.yaml \
--bag  /home/lessle6/multicameras_calibration.bag \
--models pinhole-radtan pinhole-radtan pinhole-radtan \
--topics /infra_left /infra_right /color \
--bag-from-to 3 202 --show-extraction --approx-sync 0.04

参数解释：

–target /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/data/april_6x6_A4.yaml
是标定板的配置文件，注意如果选择棋格盘，注意targetCols和targetRows表示的是内侧角点的数量，不是格子数量。
–bag /home/lessle6/multicameras_calibration.bag \ 是录制的数据包
models pinhole-radtan pinhole-radtan pinhole-radtan表示三个摄像头的相机模型和畸变模型（解释参考https://github.com/ethz-asl/kalibr/wiki/supported-models，根据需要选取,其中VINS使用的畸变模型是radtan)
—topics /infra_left /infra_right /color表示三个摄像头对应的拍摄的数据话题
–bag-from-to 3 202表示处理bag中3-202秒的数据。
–show-extraction表示显示检测特征点的过程，这些参数可以相应的调整。
可以使用rosbag info 来参看录制的包的信息

报错1：
Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance.
应该是两个相机时间不同步导致的，需要调整参数：–approx-sync

参考链接：https://github.com/shanmo/Kalibr-camera-calibration/tree/master/stereo%20camera%20calibration/preliminary%20test

调整后：

rosrun kalibr kalibr_calibrate_cameras \
--target /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/data/april_6x6_A4.yaml \
--bag /home/lessle6/multicameras_calibration.bag \
--models pinhole-radtan pinhole-radtan pinhole-radtan \
--topics /infra_left /infra_right /color \
--bag-from-to 3 202 --show-extraction --approx-sync 0.1

报错2：
Initialization of focal length failed. You can enable manual input by setting ‘KALIBR_MANUAL_FOCAL_LENGTH_INIT’.
[ERROR] [1668944382.174500]: initialization of focal length for cam with topic /color failed

解决：
如果提示不能得到初始焦距的时候，可以设置：export KALIBR_MANUAL_FOCAL_LENGTH_INIT=1(终端输入)。然后运行程序，当程序运行失败的时候，它会提示要你手动输入一个焦距，Initialization of focal length failed. Provide manual initialization: 这时你手动输入比如 400,给比较大的值，也能收敛。

如果只标定单目：
录制

 rosbag record -O mono_calibration /infra_left

标定：


source /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/devel/setup.bash

cd  /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace

rosrun kalibr kalibr_calibrate_cameras \
--target /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/data/april_6x6_A4.yaml \
--bag  /home/lessle6/mono_calibration.bag \
--models pinhole-radtan \
--topics /infra_left \
--show-extraction

注意： \前要有空格

4. kalibr 标定结果

如下三个文件：
Realsence D455标定并运行Vins-Fusion

二、IMU标定

1. 标定工具准备

code_utils和imu_utils都是imu标定需要用到的，用于标定imu噪声密度以及随机游走系数。
imu_utils依赖code_utils，所以先编译code_utils。

1.1 code_utils

mkdir -p /home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws/src
cd /home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws/src
catkin_init_workspace
cd ..
catkin_make
source  /home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws/devel/setup.bash

注意“/home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws”是我的绝对ROS空间地址。

下面下载编译code_utils：

cd /home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws/src
git clone git@github.com:gaowenliang/code_utils.git
cd ..
catkin_make

问题1 catkin_make时出现libdw.h没有找到

sudo apt-get install libdw-dev

问题2 catkin_make时出现backward.hpp没有找到

解决方法：将sumpixel_test.cpp中# include "backward.hpp"改为：#include “code_utils/backward.hpp

1.2 imu_utils

cd  /home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws/src
git clone git@github.com:gaowenliang/imu_utils.git
cd ..
catkin_make

2. 录制数据准备

2.1 编辑IMU的话题

方式：

找到realsense-ros包，进入
/home/lessle6/Packages/RealsenseRos_ws/src/realsense-ros/realsense2_camera/launch（路径仅供参考），
复制一份rs_camera.launch，在rs_camera.launch里面修改：

<arg name="unite_imu_method"          default=""/>

修改为：

<arg name="unite_imu_method"          default="linear_interpolation"/>

原因：

将accel和gyro的数据合并得到imu话题，如果不这样做发布的topic中只要加速计和陀螺仪分开的topic，
没有合并的camera/imu topic。

最后运行：

source /home/lessle6/Packages/RealsenseRos_ws/devel/setup.bash
roslaunch /home/lessle6/Packages/RealsenseRos_ws/src/realsense-ros/realsense2_camera/launch/rs_camera.launch

这样就可以播放对应格式的IMU。

2.2 录制数据

rosbag record -O imu_calibration /camera/imu

Realsence D455标定并运行Vins-Fusion realsense静止放置，录制一个小时（自定）。
其中imu_calibration是bag包的名字，可以更改，录的包在当前终端目录下，/camera/imu是发布的IMU topic，可以通过以下命令查看。

rostopic list

3. 正式标定工作

3.1 imu_util 工具包launch文件编写

# 进入文件夹
cd /home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws/src/imu_utils/launch 

# 创建文件
touch d455_imu_calibration.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= "50"/>
        <!--采样频率，即是IMU频率，采样频率可以使用rostopic hz /camera/imu查看，设置为200，为后面的rosbag play播放频率-->
        <param name="max_cluster" type="int" value= "200"/>
    </node>
    
</launch>

3.2 imu_util标定程序运行

source /home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws/devel/setup.bash

roslaunch  /home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws/src/imu_utils/launch/d455_imu_calibration.launch

3.3 播放刚才录制的IMU数据包

cd 存放imu_calibration.bag的路径
rosbag play -r 200 imu_calibration.bag

3.4 标定结果

标定结束后在imu_catkin_ws/src/imu_utils/data中生成许多文件，其中d455_imu_param.yaml就是我们想要的结果，展示如下：

%YAML:1.0
---
type: IMU
name: d455
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 1.8351398172861977e-03
      gyr_w: 1.3154828587252936e-05
   x-axis:
      gyr_n: 1.8848941703928685e-03
      gyr_w: 1.7010312342829289e-05
   y-axis:
      gyr_n: 2.3497663951613673e-03
      gyr_w: 1.2699656085618862e-05
   z-axis:
      gyr_n: 1.2707588863043573e-03
      gyr_w: 9.7545173333106557e-06
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 1.7574789006499388e-02
      acc_w: 5.3103238396236881e-04
   x-axis:
      acc_n: 1.1838936933010856e-02
      acc_w: 4.1400494167342325e-04
   y-axis:
      acc_n: 1.9827984613668935e-02
      acc_w: 6.3400755296148608e-04
   z-axis:
      acc_n: 2.1057445472818376e-02
      acc_w: 5.4508465725219715e-04

三、IMU+双目相机标定

1. 准备yaml文件

格式参考Kalibr官方教程： https://github.com/ethz-asl/kalibr/wiki/yaml-formats

1.1 编写chain.yaml

参考一、双目相机标定标定结果multicameras_calibration-camchain.yaml，编写chain.yaml：

cam0:
  camera_model: pinhole
  distortion_coeffs: [0.007017020579074508, 0.013075992589794715, -0.0037765402744058983, -0.0005132729806830811]
  distortion_model: radtan
  intrinsics: [437.44398421645786, 437.72233141976125, 430.95314113824475, 231.60352693067642]
  resolution: [848, 480]
  rostopic: /infra_left
cam1:
  T_cn_cnm1:
  - [0.9999952197233123, 0.002108064461866742, -0.0022619891120047486, -0.09575399583929174]
  - [-0.0021145367376279966, 0.9999936666664111, -0.002862754171397105, 0.0005322182496990349]
  - [0.0022559399157413514, 0.002867523545717535, 0.9999933439997514, -0.0035089144049743597]
  - [0.0, 0.0, 0.0, 1.0]
  camera_model: pinhole
  distortion_coeffs: [0.018914793505132418, -0.0026985776594766744, -0.0025567868843695152, 0.00645205341789554]
  distortion_model: radtan
  intrinsics: [432.2163360247922, 431.7881804665646, 433.3636493084969, 232.14028787830168]
  resolution: [848, 480]
  rostopic: /infra_right

T_cn_cnm1:表示的是左目相机到右目相机的旋转和平移，具体的参数可以参考前面标定得到的结果

1.2 编写imu.yaml

参考二、IMU标定标定结果d455_imu_param.yaml，编写imu.yaml：

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

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

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

1.3 april_6x6_A4.yaml

第一节写过的：

target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.021           #size of apriltag, edge to edge [m]
tagSpacing: 0.3          # 小格子与大格子边长比例
codeOffset: 0            #code offset for the first tag in the aprilboard

1.4 rs_camera.launch

复制realsense-ros包中rs_camera.launch，更改内容为:
(1) imu和双目数据时间对齐

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

(2)合并加速计和陀螺仪的topic

2. 录制数据

2.1 关闭结构光

首先关闭IR：
默认开始IR结构光时，双目图像会有很多点，这些点可能对标定有影响，所以使用时需要关闭结构光。
终端1：

source /home/lessle6/Packages/RealsenseRos_ws/devel/setup.bash
roslaunch /home/lessle6/Packages/RealsenseRos_ws/src/realsense-ros/realsense2_camera/launch/rs_camera.launch

再打开一个终端2：

rosrun rqt_reconfigure rqt_reconfigure

打开后将camera->stereo_module中的emitter_enabled设置为off(0) 。

2.2 可视化双目图像

新打开终端，运行rviz

rviz

修改Fiexd Fram 为camera_link
Realsence D455标定并运行Vins-Fusion

之后在里面add rgb和双目对应的topic，/camera/color/image_raw、/camera/infra1/image_rect_raw、/camera/infra2/image_rect_raw展示如下：
Realsence D455标定并运行Vins-Fusion 录制过程参考https://www.youtube.com/watch?v=puNXsnrYWTY&app=desktop，同样注意双目图像在整个过程要包含整个标定板，同时运动不能太快，这样会造成图像过于模糊，在前后左右上下方向来回移动，录制大概90s以上。

2.3 修改相机播包帧数

调整imu和双目topic的发布频率以及以新的topic名发布它们，其中双目图像的发布频率改为20Hz（注意这里是20了，刚才双目标定是4hz），imu发布频率改为200Hz
Realsence D455标定并运行Vins-Fusion

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/imu 200.0 /imu

2.4 录制数据

rosbag record -O imu_stereo.bag /infra_left /infra_right /imu

imu_stereo.bag为录制数据

3. kalibr 程序标定

source /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/devel/setup.bash

cd  /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace

rosrun kalibr kalibr_calibrate_imu_camera \
--bag  /home/lessle6/imu_stereo.bag \
--cam /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/data/chain.yaml \
--imu /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/data/imu.yaml \
--target /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/data/april_6x6_A4.yaml \
--bag-from-to 2 146 \
--show-extraction

4. 标定结果

Realsence D455标定并运行Vins-Fusion

四、修改VINS的yaml文件

1. realsense_stereo_imu_config.yaml

%YAML:1.0

#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; 
imu: 1         
num_of_cam: 2  

imu_topic: "/camera/imu"
image0_topic: "/camera/infra1/image_rect_raw"
image1_topic: "/camera/infra2/image_rect_raw"
output_path: "/home/zj/output/"

cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 848
image_height: 480
   

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
#相机到imu的变换矩阵
body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 0.99981999,  0.00163688 , 0.0189025 , -0.00089318,
 -0.00146025 , 0.99995518 ,-0.00935466  ,0.00021664,
 -0.01891696,  0.00932537 , 0.99977757  ,0.00065183,
  0.        ,  0.      ,    0.     ,     1.         ]


body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [0.99977591, -0.0005314 ,  0.0211626,   0.0949139 ,
 0.00066889 , 0.99997871, -0.00649049, -0.00027429,
 -0.0211587 ,  0.00650319 , 0.99975498 , 0.0021304 ,
 0.   ,       0.       ,   0.      ,    1.    ]


#Multiple thread support
multiple_thread: 1

#feature traker paprameters
max_cnt: 150            # max feature number in feature tracking
min_dist: 30            # min distance between two features 
freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
F_threshold: 1.0        # ransac threshold (pixel)
show_track: 0           # publish tracking image as topic
flow_back: 1            # perform forward and backward optical flow to improve feature tracking accuracy

#optimization parameters
max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time
max_num_iterations: 8   # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 1.7574789006499388e-02        # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 1.8351398172861977e-03      # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 5.3103238396236881e-04      # accelerometer bias random work noise standard deviation.  #0.002
gyr_w: 1.3154828587252936e-05    # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.78921469         # gravity magnitude

#unsynchronization parameters
estimate_td: 1                      # online estimate time offset between camera and imu
td:0           # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

#loop closure parameters
load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/zj/output/pose_graph/" # save and load path
save_image: 0                   # save image in pose graph for visualization prupose; you can close this function by setting 0

2 left.yaml

%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 848
image_height: 480
distortion_parameters:
   k1: 0.007017020579074508
   k2: 0.013075992589794715
   p1: -0.0037765402744058983
   p2: -0.0005132729806830811
projection_parameters:
   fx: 437.44398421645786
   fy: 437.72233141976125
   cx: 430.95314113824475
   cy: 231.60352693067642

3 right.yaml

%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 848
image_height: 480
distortion_parameters:
   k1: 0.018914793505132418
   k2: -0.0026985776594766744
   p1: -0.0025567868843695152
   p2: 0.00645205341789554
projection_parameters:
   fx: 432.2163360247922
   fy: 431.7881804665646
   cx: 433.3636493084969
   cy: 232.14028787830168

五、运行VINS-FUSION

启动vins

source /home/lessle6/Project/Vins-Fusion-ws/devel/setup.bash

rosrun loop_fusion loop_fusion_node /home/lessle6/Project/Vins-Fusion-ws/src/VINS-Fusion/config/realsense-d455/realsense_stereo_imu_config.yaml
rosrun vins vins_node /home/lessle6/Project/Vins-Fusion-ws/src/VINS-Fusion/config/realsense-d455/realsense_stereo_imu_config.yaml

启动rviz

source /home/lessle6/Project/Vins-Fusion-ws/devel/setup.bash
cd  /home/lessle6/Project/Vins-Fusion-ws
roslaunch vins vins_rviz.launch

启动相机

source /home/lessle6/Packages/RealsenseRos_ws/devel/setup.bash
roslaunch /home/lessle6/Packages/RealsenseRos_ws/src/realsense-ros/realsense2_camera/launch/rs_camera.launch

参考

https://blog.csdn.net/xiaoxiaoyikesu/article/details/105646064

https://blog.csdn.net/qq_40186909/article/details/113104595?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166312286216782391821994%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=166312286216782391821994&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2_allfirst_rank_ecpm_v1~pc_rank_v39-2-113104595-null-null.142%5Ev47%5Epc_rank_34_2,201%5Ev3%5Econtrol_2&utm_term=realsense%20%E7%9A%84%20D%20%E7%9F%A9%E9%98%B5&spm=1018.2226.3001.4187文章来源地址https://www.toymoban.com/news/detail-476972.html

到了这里，关于Realsence D455标定并运行Vins-Fusion的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！

Realsence D455标定并运行Vins-Fusion

一、双目相机标定

1. 标定板准备

1.1 打印标定板

1.2 标定板信息

原始pdf的格子参数是：

调整后的格子参数是：

2. 左右目相机数据准备

2.1 修改rs_camera.launch内容

2.2 关闭结构光

2.3 可视化双目图像

2.4 修改相机播包帧数

2.5 录制数据

3. kalibr 程序标定

4. kalibr 标定结果

二、IMU标定

1. 标定工具准备

1.1 code_utils

问题1 catkin_make时出现libdw.h没有找到

问题2 catkin_make时出现backward.hpp没有找到

1.2 imu_utils

2. 录制数据准备

2.1 编辑IMU的话题

方式：

原因：

最后运行：

2.2 录制数据

3. 正式标定工作

3.1 imu_util 工具包launch文件编写

3.2 imu_util标定程序运行

3.3 播放刚才录制的IMU数据包

3.4 标定结果

三、IMU+双目相机标定

1. 准备yaml文件

1.1 编写chain.yaml

1.2 编写imu.yaml

1.3 april_6x6_A4.yaml

1.4 rs_camera.launch

2. 录制数据

2.1 关闭结构光

2.2 可视化双目图像

2.3 修改相机播包帧数

2.4 录制数据

3. kalibr 程序标定

4. 标定结果

四、修改VINS的yaml文件

1. realsense_stereo_imu_config.yaml

2 left.yaml

3 right.yaml

五、 运行VINS-FUSION

参考

相关文章

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

微信扫一扫打赏

支付宝扫一扫领取红包，优惠每天领

二维码1

二维码2

五、运行VINS-FUSION