D435i学习

D435i的imu坐标系定义。

环境变量

source ~/vins-fusion/devel/setup.bash

开启D435i相机

roslaunch realsense2_camera rs_camera.launch

关闭结构光

rosrun rqt_reconfigure rqt_reconfigure

关闭结构光可以减小对相机标定的影响，ORB_SLAM3同样建议关闭结构光。

 打开VINS-Fusion

连接相机参数

rosrun vins vins_node ~/vins-fusion/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml

开启回环检测

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

启动vins配置的rviz

roslaunch vins vins_rviz.launch 

启动之前要在/vins-fusion/src/VINS-Fusion/config/realsense_d435i/下，三个配置文件的left、right、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/gao/vins-fusion-output/"

cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
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.

body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 0.99999113, -0.00128726,
       0.00400931, 0.01895286,
       0.0012582, 0.99997298,
       0.00724281, -0.00225361,
       -0.00401852, -0.0072377,
       0.99996573, -0.02370379 , 0, 0, 0, 1 ]

body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 0.99999334, -0.00117404,
       0.00345644, -0.04943306,
       0.00115146, 0.99997804,
       0.00652691, -0.00220166,
       -0.00346403, -0.00652288,
       0.99997273, -0.02352069, 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: 0                # 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.011797914771221316          # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 0.0015758842300519476         # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 0.00019286365473644888         # accelerometer bias random work noise standard deviation.  #0.002
gyr_w: 1.0237193512142503e-05       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.79883982        # gravity magnitude

#unsynchronization parameters
estimate_td: 1                      # online estimate time offset between camera and imu
td: 0.00                             # 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/gao/vins-fusion-output/" # save and load path
save_image: 1                   # save image in pose graph for visualization prupose; you can close this function by setting 0 

 其中，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.（estimate_extrinsic: 1 # 0 有一个准确的外在参数。我们将相信以下imu^R_cam、imu^T_cam，不要改变它。# 1 有一个关于外在参数的初始猜测。我们将围绕你的初始猜测进行优化。）的值必须为1，

启动ORB_SLAM3

rosrun ORB_SLAM3 要启动的节点 path_to_ORBvoc.txt path_to_yaml 是否开启rectify(矫正) [是否开启equalize(平衡)]

 启动Stereo_Inertial节点：

rosrun ORB_SLAM3 Stereo_Inertial /home/gao/orb_slam3/src/ORB_SLAM3/Vocabulary/ORBvoc.txt /home/gao/orb_slam3/src/ORB_SLAM3/Examples_old/Stereo-Inertial/RealSense_D435i.yaml false

启动RGBD节点（话题名称对不上，所以需要更改话题名称）： 

rosrun ORB_SLAM3 RGBD /home/gao/orb_slam3/src/ORB_SLAM3/Vocabulary/ORBvoc.txt /home/gao/orb_slam3/src/ORB_SLAM3/Examples_old/RGB-D/RealSense_D455.yaml

同时：

rosrun topic_tools throttle messages /camera/color/image_raw 10.0 /camera/rgb/image_raw

rosrun topic_tools throttle messages /camera/depth/image_rect_raw 50 /camera/depth_registered/image_raw

修改配置

ORBextractor.nFeatures: 500 # Tested with 1250

修改后里程计就不会很飘，这里500就是最小值，再小ORB就会无法运行。

启动不同的节点后在相应的文件夹下修改相应的yaml文件，其中Tlr是cam0和cam1之间的转换举证（三行四列），Tbc是imu到left cam的转换矩阵（四行四列），同时记得修改相机内参、imu四个噪声。

启动ORB_SLAM3后，要将相机平移一个“8”，在两三圈之后，等待start VIBA 1、

end VIBA 1、start VIBA 2、end VIBA 2全部结束了之后，便可以开始建图。

跑完之后会自动将数据保存在orb_slam3-output

出现错误：**ERROR in the config file, the format is not correct**，terminate called after throwing an instance of 'int'。因为Camera.bf不能是int类型格式。Camera.bf: 0.0时，无法看到相机运行的轨迹，该数字调的越大轨迹越明显。

大概是ORB_SLAM3的d435i的配置文件版本问题，用新的版本总是会飘，于是尝试了一下老版本一下就好了，大概如下：

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
File.version: "1.0"

Camera.type: "PinHole"

# Left Camera calibration and distortion parameters (OpenCV)
Camera1.fx: 384.08180419333854
Camera1.fy: 385.1699570781023
Camera1.cx: 318.40812735005045
Camera1.cy: 235.15674726419329

# Kannala-Brandt distortion parameters
Camera1.k1: 0.0036963710377220997
Camera1.k2: -0.003936964392896428
Camera1.p1: -0.0006491587676550943
Camera1.p2: -0.0016100846933681853

# Right Camera calibration and distortion parameters (OpenCV)
Camera2.fx: 382.90626074548965
Camera2.fy: 384.27441003714694
Camera2.cx: 318.47243335920604
Camera2.cy: 235.3624696152331

# Kannala-Brandt distortion parameters
Camera2.k1: 0.006361877993096527
Camera2.k2: -0.005807113901972516
Camera2.p1: 4.695700559961851e-06
Camera2.p2: -0.0010316795152097214
    
# Transformation matrix from right camera to left camera(应该是左相机到右相机)
Stereo.T_c1_c2: !!opencv-matrix
  rows: 4
  cols: 4
  dt: f
  data: [0.99999984, 0.00010922, -0.00055367, -0.06839879,
         -0.00010962, 0.99999974, -0.00071548, 0.00003707,
         0.00055359, 0.00071554, 0.99999959, 0.00017422,
          0.0, 0.0, 0.0, 1.0]


# Camera resolution
Camera.width: 640
Camera.height: 480

# Camera frames per second 
Camera.fps: 30

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

# Close/Far threshold. Baseline times.
Stereo.ThDepth: 40.0

#--------------------------------------------------------------------------------------------
# IMU Parameters
#--------------------------------------------------------------------------------------------

# Transformation from camera 0 to body-frame (imu)(imu到左相机)
IMU.T_b_c1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: f
   data: [0.99999113, 0.0012582, -0.00401852, -0.01904511, 
           -0.00128726, 0.99997298, -0.0072377, 0.00210639,
           0.00400931, 0.00724281, 0.99996573, 0.02364332,
           0.0, 0.0, 0.0, 1.0]

# Do not insert KFs when recently lost
IMU.InsertKFsWhenLost: 0

# IMU noise (Use those from VINS-mono)
IMU.NoiseGyro: 0.0015758842300519476 # 3 # 2.44e-4 #1e-3 # rad/s^0.5
IMU.NoiseAcc: 0.011797914771221316 #2 # 1.47e-3 #1e-2 # m/s^1.5
IMU.GyroWalk: 1.0237193512142503e-05 # rad/s^1.5
IMU.AccWalk: 0.00019286365473644888 # m/s^2.5
IMU.Frequency: 200.0

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 500 # Tested with 1250

# ORB Extractor: Scale factor between levels in the scale pyramid 	
ORBextractor.scaleFactor: 1.2

# ORB Extractor: Number of levels in the scale pyramid	
ORBextractor.nLevels: 8

# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast			
ORBextractor.iniThFAST: 15
ORBextractor.minThFAST: 7

#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1.0
Viewer.GraphLineWidth: 0.9
Viewer.PointSize: 2.0
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3.0
Viewer.ViewpointX: 0.0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -3.5
Viewer.ViewpointF: 500.0
Viewer.imageViewScale: 2.0

evo轨迹分析

将.txt格式更改为.tum格式文件（注意：evo只能分析八列数据且中间用逗号分开，每一行后面没有多余的字符）

evo_traj euroc 你想更改的文件 --save_as_tum

evo_traj euroc ORB-SLAM3-FrameTrajectory.txt --save_as_tum

画出轨迹

evo_traj euroc ORB-SLAM3-FrameTrajectory.tum --plot

轨迹对齐（采用Sim(3) Umeyama对齐，同时处理平移旋转和尺度） ：

evo_ape tum ORB-SLAM3-FrameTrajectory.tum ORB-SLAM3-KeyFrameTrajectory.tum -r full -as --plot --plot_mode xyz