Ubuntu 18.04 ———（Intel RealSense D435i）标定后结果用于VINS-Fusion

最新推荐文章于 2024-08-20 11:14:52 发布

@曾记否

最新推荐文章于 2024-08-20 11:14:52 发布

阅读量4.7k

点赞数 8

分类专栏：双目相机文章标签：计算机视觉自动驾驶人工智能

本文链接：https://blog.csdn.net/qq_38364548/article/details/124955452

版权

双目相机专栏收录该内容

6 篇文章 0 订阅

订阅专栏

Ubuntu 18.04 ———（Intel RealSense D435i）标定后结果用于VINS-Fusion

一、相机内外参
二、修改配置文件
三、运行VINS-FUSION
参考文献

一、相机内外参

1、什么是相机内外参？

以小孔成像的相机模型为例，相机模型的坐标系共有四种：世界坐标系 $O_w)$ 、相机坐标系 $O_c)$ 、图像坐标系 $(x^{'}, y^{'})$ 和像素坐标系 $(u, v)$ 。

在这里插入图片描述

世界坐标系到像素坐标系直接的坐标变换关系：

在这里插入图片描述

我们将世界坐标系到相机坐标系的坐标变换矩阵称为外参矩阵，将相机坐标系到像素坐标系之间的坐标变换矩阵称为内参矩阵。其中dx和dy为像元的宽度，和fx和fy为等效焦距一般情况下，二者相等。由于在以标定板中心点为世界坐标系原点时，标定的每张图片对应的外参矩阵都不同，因此只需要标定相机内参。
同时为了更加准确得描述成像系统，还引入了畸变模型，即像素坐标系中的图像并非在理想的位置(u*,v*) ，需要在实际位置(u,v)的基础上补偿非线性畸变，即:

在这里插入图片描述

非线性畸变的表达式如下所示，公式中的k为径向畸变系数p为离心畸变系数。

在这里插入图片描述

2、如何获得相机标定前的内参？

可以看我以前的博客：https://blog.csdn.net/qq_38364548/article/details/124784454—— 三、查看相机内参

roslaunch realsense2_camera rs_camera.launch
rostopic echo /camera/depth/camera_info

3、如何把标定参数用于VINS_Fusion?

相机系统校准（camchain.yaml）
此文件存储相机内参和外参的校准，以及IMU相对于相机的空间和时间校准参数。

每个摄像机都有以下参数：

camera_model
相机投影类型（针孔/全向）
包含给定投影类型的固有参数的内联
向量。元素如下： 针孔：
[fu fv pu pv]
omni： [xi fu fv pu pv]
ds： [xi alpha fu fv pu pv]
eucm： [alpha beta fu fv pu pv]
有关详细信息，请参阅支持的模型
distortion_model
镜头失真类型（radtan / 等距）
distortion_coeffs失真模型
的
参数向量，请参阅支持的模型以获取更多信息
T_cn_cnm1
相机的外在变换，始终相对于链
中的最后一个相机（例如 cam1：T_cn_cnm1 = T_c1_c0，将 cam0 转换为 cam1 坐标）
T_cam_imu
IMU 外参：从 IMU 到相机坐标的转换 （T_c_i）
timeshift_cam_imu相机和 IMU 时间戳之间的
时移（以秒为单位）（t_imu = t_cam + 移位）
相机图像流的玫瑰
主题
分辨率
相机分辨率 [宽度、高度]

对应的英文如下：

Each camera has the following parameters:

camera_model
camera projection type (pinhole / omni)
intrinsics
vector containing the intrinsic parameters for the given projection type. elements are as follows:
pinhole: [fu fv pu pv]
omni: [xi fu fv pu pv]
ds: [xi alpha fu fv pu pv]
eucm: [alpha beta fu fv pu pv]
see Supported models for more information
distortion_model
lens distortion type (radtan / equidistant)
distortion_coeffs
parameter vector for the distortion model
see Supported models for more information
T_cn_cnm1
camera extrinsic transformation, always with respect to the last camera in the chain
(e.g. cam1: T_cn_cnm1 = T_c1_c0, takes cam0 to cam1 coordinates)
T_cam_imu
IMU extrinsics: transformation from IMU to camera coordinates (T_c_i)
timeshift_cam_imu
timeshift between camera and IMU timestamps in seconds (t_imu = t_cam + shift)
rostopic
topic of the camera's image stream
resolution
camera resolution [width,height]

（1）、相机标定结果

camchain-imucam-imu_stereo.yaml文件

cam0:
  T_cam_imu:   //IMU 外参：从 IMU 到相机坐标的转换 （T_c_i）
  - [0.9999397258788518, -0.004683976226100742, -0.009930003828888262, 0.00740103501939206]
  - [ 0.004719594270289592, 0.9999825025383232, 0.0035665175056139383, -0.006330740676123676]
  - [ 0.00991312459582054, -0.0036131681260804997, 0.9999443359391772, -0.01893316608480435]
  - [ 0.0, 0.0, 0.0, 1.0]
  camera_model: pinhole    //相机模型:针孔
  distortion_coeffs:   //畸变参数
 [ 0.3504343204199696, 0.09942954345832032, 0.06984316113464994, 0.06546342606600335]
  distortion_model: equidistant   //畸变模型：等距
  intrinsics:      //内参
  [ 422.64066392524313, 424.02986182898763, 433.8337283244407, 239.28229368758497]
  resolution:   //相机分辨率
  [ 848, 480]
  rostopic: /infra_left    //话题
  timeshift_cam_imu: 0.005100358375965326   //相机和 IMU 时间戳之间的时移（以秒为单位）（t_imu = t_cam + 移位）
cam1:
  T_cam_imu://IMU 外参：从 IMU 到相机坐标的转换 （T_c_i）
  - [ 0.9999670292694932, -0.004747919302927823, -0.006587688231643368, -0.04257359730949143]
  - [ 0.0047659964968495335, 0.9999849130497962, 0.002731107867444307, -0.006302019926150486]
  - [ 0.006574621763756843, -0.002762414719857174, 0.9999745713834822, -0.019074706862001883]
  - [ 0.0, 0.0, 0.0, 1.0]
  T_cn_cnm1:    //相机的外在变换，始终相对于链中的最后一个相机（例如 cam1：T_cn_cnm1 = T_c1_c0，将 cam0 转换为 cam1 坐标）
  - [ 0.999994412046126, -5.189266997265358e-05, 0.0033426312493865817, -0.04991163289867289]
  - [ 5.4683759833798546e-05, 0.9999996499658775, -0.0008349118544065274, 1.250629277077362e-05]
  - [ -0.003342586753546276, 0.0008350899766020524, 0.999994064851648, -0.00011162780864553323]
  - [ 0.0, 0.0, 0.0, 1.0]
  camera_model: pinhole
  distortion_coeffs: [ 0.34116697848102345, 0.14997256247725585, -0.035613030583661255, 0.12752761977350466]
  distortion_model: equidistant
  intrinsics: [ 422.6846630997528, 424.22648250200166, 432.1283922008165, 239.61231361314714]
  resolution: [ 848, 480]
  rostopic: /infra_right
  timeshift_cam_imu: 0.005063952809659112

imu-imu_stereo.yaml文件

imu0:
  T_i_b:
  - [ 1.0, 0.0, 0.0, 0.0]
  - [ 0.0, 1.0, 0.0, 0.0]
  - [ 0.0, 0.0, 1.0, 0.0]
  - [ 0.0, 0.0, 0.0, 1.0]
  accelerometer_noise_density: 0.020546933436785896   //Noise density (continuous-time)
  accelerometer_random_walk: 0.0010213093603518382    //Bias random walk
  gyroscope_noise_density: 0.0019035698029406516
  gyroscope_random_walk: 1.9530927686562762e-05
  model: calibrated
  rostopic: /imu
  time_offset: 0.0
  update_rate: 200.0

（2）、根据标定结果修改相机内外参

相机外参

（1）相机外参填写到realsense_stereo_imu_config.yaml中，我的结果如下其中cam0：T_cam_imu表示的是imu到相机的变换矩阵，我们填写到body_T_cam0的矩阵应该是相机到imu的矩阵，所以对T_cam_imu矩阵取逆，因为旋转矩阵是正交矩阵，它的逆矩阵就等于它的转置矩阵，平移向量的逆，三轴取反就可以了，cam1的同理
在这里插入图片描述 MATLAB求逆：

%%%%%%%%%%%%%%%MATLAB%%%%%%%%%%%%%%%%%%%
%% cam0:
 %% T_cam_imu:
Y =

         0.999939725878852      -0.00468397622610074      -0.00993000382888826       0.00740103501939206
       0.00471959427028959         0.999982502538323       0.00356651750561394      -0.00633074067612368
       0.00991312459582054       -0.0036131681260805         0.999944335939177       -0.0189331660848043
                         0                         0                         0                         1

>> inv(Y)

ans =

          0.99993972587885       0.00471959427028958       0.00991312459582052      -0.00718302356669691
      -0.00468397622610073         0.999982502538322      -0.00361316812608049       0.00629688746408727
      -0.00993000382888825       0.00356651750561393         0.999944335939176         0.019028183191421
                         0                         0                         0                         1
%% cam1:
 %% T_cam_imu:
Z =

         0.999967029269493      -0.00474791930292782      -0.00658768823164337       -0.0425735973094914
       0.00476599649684953         0.999984913049796       0.00273110786744431      -0.00630201992615049
       0.00657462176375684      -0.00276241471985717         0.999974571383482       -0.0190747068620019
                         0                         0                         0                         1

>> inv(Z)

ans =

         0.999967029269493       0.00476599649684953       0.00657462176375684        0.0427276380146512
      -0.00474791930292782         0.999984913049796      -0.00276241471985717       0.00604709659241631
      -0.00658768823164337       0.00273110786744431         0.999974571383482        0.0188109717288225
                         0                         0                         0                         1

（2）加速度计和陀螺仪的噪声和随机游走填入相应位置
在这里插入图片描述

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/output/fusion"

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

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0   # 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.
                        # 0 有一个精确的外部参数。 我们会信任下面的imu和r_cam，imu和t_cam，不要改变它。
                        # 1 对外部参数有初步的猜测。 我们将围绕您的初步猜测进行优化。

body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 0.99993972587885, 0.00471959427028958, 0.00991312459582052, -0.00718302356669691,
      -0.00468397622610073, 0.999982502538322, -0.00361316812608049, 0.00629688746408727,
      -0.00993000382888825, 0.00356651750561393, 0.999944335939176, 0.019028183191421,
      0, 0, 0, 1]

body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 0.999967029269493, 0.00476599649684953, 0.00657462176375684, 0.0427276380146512,
      -0.00474791930292782, 0.999984913049796, -0.00276241471985717, 0.00604709659241631,
      -0.00658768823164337, 0.00273110786744431, 0.999974571383482, 0.0188109717288225,
      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: 1           # 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: 0.020546933436785896          # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 0.0019035698029406516         # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 0.0010213093603518382         # accelerometer bias random work noise standard deviation.  #0.002
gyr_w: 1.9530927686562762e-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.005063952809659112            # 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/output/fusion/pose_graph/" # save and load path
save_image: 1                   # save image in pose graph for visualization prupose; you can close this function by setting 0

相机内参

相机配置文件left.yaml和right.yaml
根据camchain-imucam-homezjimu_stereo.yaml中的相机内参和畸变参数填写，结果如下：

（1）left.yaml

原文件：
在这里插入图片描述
修改后：

%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 848
image_height: 480
distortion_parameters:
   k1: 0.3504343204199696
   k2: 0.09942954345832032
   p1: 0.06984316113464994
   p2: 0.06546342606600335
projection_parameters:
   fx: 422.64066392524313
   fy: 424.02986182898763
   cx: 433.8337283244407
   cy: 239.28229368758497

（2）right.yaml

原文件：
在这里插入图片描述
修改后：

%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 848
image_height: 480
distortion_parameters:
   k1: 0.34116697848102345
   k2: 0.14997256247725585
   p1: -0.035613030583661255
   p2: 0.12752761977350466
projection_parameters:
   fx: 422.6846630997528
   fy: 424.22648250200166
   cx: 432.1283922008165
   cy: 239.61231361314714

二、修改配置文件

在这里插入图片描述

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

cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 848
image_height: 480 
//...
//修改显示image-track（默认 0 ，没有发布image-track）
show_track: 1  # publish tracking image as topic
//...
'pose_graph_save_path'
pose_graph_save_path: "/home/output/fusion/pose_graph/" # save and load path

在你自己安装的realsence文件下（本文在~/catkin_ws/src/realsense/realsense2_camera/launch）：rs_camera.launch复制并重命名为：rs_fusion_camera_stereo.launch可以直接参考我的ROS启动文件

打开双目，修改图片的width、height
打开imu的加速度计和陀螺仪并合并为一个topic

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

三、运行VINS-FUSION

打开一个终端，用roslaunch 打开realsense摄像头

source ~/catkin_ws/devel/setup.bash //将其添加到bash配置文件中，可省略这一步（我已添加）
roslaunch realsense2_camera rs_fusion_camera_stereo.launch

打开一个终端，用于打开rviz

source ~/catkin_ws/devel/setup.bash
roslaunch vins vins_rviz.launch

打开一个终端，跑起来

source ~/catkin_ws/devel/setup.bash
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/yourconfig_path/your_config_file.yaml 
#我的是rosrun vins vins_node src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml

三个终端运行成功的话，你的rviz会有数据，特征点和frame的pose

开回环
在src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml中修改：

save_image: 1

在终端启动：

rosrun loop_fusion loop_fusion_node ~/vins_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml

线太短不好操作，先这样吧！！
在这里插入图片描述

参考文献

https://github.com/IntelRealSense/librealsense/wiki/Projection-in-RealSense-SDK-2.0#intrinsic-camera-parameters

@曾记否

关注

8
点赞
踩
94

收藏

觉得还不错? 一键收藏
12
评论
复制链接

分享到 QQ

分享到新浪微博

扫一扫

专栏目录