ubuntu1804使用Realsense D435i运行VINS-Fusion并建图

转载自：https://blog.csdn.net/qq_42800654/article/details/109393646

1 安装VINS

到github：VINS，按照说明安装依赖和编译vins。

ceres别装2.0版本，装1.4
开始装的2.0遇到错误error: ‘integer_sequence’ is not a member of ‘std’ struct SumImpl<std::integer_sequence<T, N, Ns…>>
教程查看：https://blog.csdn.net/qq_41586768/article/details/107541917

2 安装测试librealsense SDK 2.0

• 注册公匙

 - sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE

• 添加repositories（Ubuntu 18.04）

sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u

• 安装library

sudo apt-get install librealsense2-dkms
sudo apt-get install librealsense2-utils

• 安装developer和debug packages

sudo apt-get install librealsense2-dev
sudo apt-get install librealsense2-dbg

• 运行测试（需要usb3.0，否则没有imu输入）

realsense-viewer

3 安装测试realsense的ROS包

安装：

cd ~/catkin_ws/src
git clone https://github.com/intel-ros/realsense.git
cd ..
catkin_make
rospack profile
source devel/setup.sh

catkin_make可能报错缺少ddynamic_reconfigure，请到github下载代码直接放置到你的catkin_ws/src中。
注：安装以下（不装的话，我的电脑运行一会儿就出错，忘记记录错误了，大家可以先不装）

sudo apt-get install ros-melodic-rgbd-launch

启动：

roslaunch realsense2_camera rs_rgbd.launch
rosrun rviz rviz

rviz中添加话题：

左上角 Displays 中 Fixed Frame 选项中，下拉菜单选择 camera_link，Global Status由红色变绿。
Add -> 上方点击 By topic -> /depth_registered 下的 /points 下的/PointCloud2
Add -> 上方点击 By topic -> /color 下的 /image_raw 下的image
…

4 使用RealsenseD435i运行VINS-Fusion

1、修改Realsense文件 修改~/catkin_ws/src/realsense/realsense2_camera/launch/目录下的rs_camera.launch，重命名为rs_camera_vins.launch，将它保存在同目录下。修改内容主要有：

//打开imu
<arg name="enable_gyro"         default="true"/>
<arg name="enable_accel"        default="true"/>
//联合方式copy或linear_interpolation
<arg name="unite_imu_method"          default="linear_interpolation"/>
//时间戳同步
<arg name="enable_sync"               default="true"/>

还有一些无关紧要的地方，可以直接下载Realsense修改后的文件
2、修改VINS文件 在~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/…目录下

• 修改外参（可以不改，使用默认值，但漂移严重）
  在realsense_stereo_imu_config.yaml、left.yaml、right.yaml三个文件中，分别对应body_T_cam0、body_T_cam1两个外参和两个projection_parameters内参，D435i的参数可以从SDK查看：外参查看方法。可以直接下载我的D435i修改后的参数文件。
  其中外参：

body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 1, 0, 0, -0.00552,
           0, 1, 0, 0.0051,
           0, 0, 1, 0.01174,
           0, 0, 0, 1 ]
           
body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 1, 0, 0, 0.0446571,
           0, 1, 0, 0.0051,
           0, 0, 1, 0.01174,
           0, 0, 0, 1 ]

两个内参（一样）：

projection_parameters:
   fx: 383.692
   fy: 383.692
   cx: 316.978
   cy: 239.796

• 修改显示image-track（默认 0 ，没有发布image-track）

show_track = 1

3、运行

roslaunch realsense2_camera rs_camera_vins.launch 
roslaunch vins vins_rviz.launch 
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml

4、开回环
在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.y

5、surfe稠密建图
下载港科开源代码DenseSurfelMapping，放到src下进行编译，启动vins后再启动DenseSurfelMapping：roslaunch surfel_fusion vins_realsense.launch

我的报错缺少rviz_plugins/Goal3DTool、rviz_visual_tools/RvizVisualToolsMap，网上也没找到怎么装，可以直接删掉surfel.rviz文件中有关这两个模块的调用，不影响程序运行。
（或者注释掉vins_realsense.launch文件中的，然后在运行vins时启动的rviz中订阅话题pointcloud/pointcloud2）

6、esdf建图
fiesta比voxblox简单得多，易于上手和后续修改，我就用的fiesta
按github安装编译完成fiesta之后
1) 修改launch
复制demo.launch(或者在cow_and_lady基础上改也行)重命名D435i，话题替换成自己的（默认的就可以）。
修改内参（查看方法上面已经有了）：

<param name="center_x" value="381.91"/>
<param name="center_y" value="241.553"/>
<param name="focal_x" value="609.738"/>
<param name="focal_y" value="608.557"/>

最后上一行添加一句建图：

<node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" args="-d $(find fiesta)/demo.rviz" />

2) 修改main函数
改成：

fiesta::Fiesta<sensor_msgs::Image::ConstPtr, nav_msgs::Odometry::ConstPtr> esdf_map(node);

3) 终端启动即可

roslaunch fiesta D435i.launch