D455+VINS-Fusion+surfelmapping 稠密建图（一）

环境：Ubuntu18.04+ros-melodic

一、编译VINS-Fusion

 1.1 安装Ceres Solver

//安装依赖
sudo apt-get install libgoogle-glog-dev libgflags-dev
sudo apt-get install libatlas-base-dev
sudo apt-get install libeigen3-dev
sudo apt-get install libsuitesparse-dev

//安装cere solver
git clone https://ceres-solver.googlesource.com/ceres-solver
cd ceres-solver
mkdir build
cd build
cmake ..
make
sudo make install

1.2 安装VINS-Fusion 

mkdir -p catkin_ws/src
cd catkin_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
cd ..
catkin_make

二、运行数据集

roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 
rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 
rosbag play /你自己的路径/MH_01_easy.bag

效果如图， 如果想要点云不消失，把rviz左侧的HistoryPointCloud里的Decay Time改大一点就行

 三、D455相机实测

安装步骤跳过，和D435i相机一样，具体可以看参考这篇D435i安装，注意两个版本号对应上。

然后我们开始配置D455相机的参数文件，将config下的realsense_d435i文件夹复制粘贴重命名为realsense_d455，将里面的left.yaml、right.yaml进行修改，然后配置realsense_stereo_imu_config.yaml

根据标定结果，修改上述三个文件，标定过程可参考双目imu标定1，2，3

这里贴出我的配置文件：

%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/tzy/output/"

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

# Extrinsic parameter between IMU and Camera.
# IMU和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.  有一个准确的外在参数。我们将信任以下 imu^R_cam，imu^T_cam，不要更改它
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.初步猜测外在参数。我们将围绕您最初的猜测进行优化。

#cam0 to body
body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 0.99994541, 0.00421397, 0.00956181, -0.02966641,
      -0.00427543, 0.99997028, 0.00641563, 0.00135581,
      -0.00953449, -0.00645616, 0.9999337, 0.01513884,
      0, 0, 0, 1]
#cam1 to body
body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 0.99996667, 0.00423679, 0.00697879, 0.06545514,
      -0.00428232, 0.99996956, 0.00652227, 0.00101751,
      -0.00695095, -0.00655194, 0.99995438, 0.01459833,
      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)ransac 阈值（像素）
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.01229199829486073         # accelerometer measurement noise standard deviation. #0.2   0.04    0.1
gyr_n: 0.00282608342737        # gyroscope measurement noise standard deviation.     #0.05  0.004   0.01
acc_w: 0.000294331088336         # accelerometer bias random work noise standard deviation.  #0.002   0.001
gyr_w: 0.000105243922055      # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.73473346         # gravity magnitude

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

#loop closure parameters
#回环检测
load_previous_pose_graph: 1        # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/tzy/out" # save and load path
save_image: 1                   # save image in pose graph for visualization prupose; you can close this function by setting 0 

开始实际运行，打开vins-fusion自带显示rviz：

roslaunch realsense2_camera rs_camera_vins.launch

打开相机，stereo-imu.launch文件需要修改的地方可参考这里：（建议infra_fps为15）

roslaunch realsense2_camera stereo-imu.launch

关闭结构光：

rosrun rqt_reconfigure rqt_reconfigure

开启跟踪节点：

rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 

开启闭环

rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 

 实测效果如下：

下一篇开始使用surfelmapping 进行稠密建图