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

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

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

不知语冰 2020-10-30 21:54:05 502 收藏 7

分类专栏： vins

版权

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 keys.gnupg.net --recv-key C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE

• 添加repositories（Ubuntu 16.04）

sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial 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-kinetic-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.yaml

5 surfe稠密建图

下载港科开源代码DenseSurfelMapping，放到src下进行编译，启动vins后再启动DenseSurfelMapping：roslaunch surfel_fusion vins_realsense.launch。

我的报错缺少rviz_plugins/Goal3DTool、rviz_visual_tools/RvizVisualToolsMap，网上也没找到怎么装，可以直接删掉surfel.rviz文件中有关这两个模块的调用，不影响程序运行。
（或者注释掉vins_realsense.launch文件中的<node type="rviz" name="rviz" pkg="rviz" args="-d $(find surfel_fusion)/launch/surfel.rviz" />，然后在运行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"/>

最后</launch>上一行添加一句建图：

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

2、修改mian函数

改成：

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

3、终端启动即可

roslaunch fiesta D435i.launch

7 yolo语义建图（未完…）

下载

mkdir -p catkin_workspace/src
cd catkin_workspace/src
git clone --recursive https://github.com/leggedrobotics/darknet_ros
cd ../

编译:

catkin_make -DCMAKE_BUILD_TYPE=Release

此时会开始编译整个项目，编译前会检查{catkin_ws}/darknet_ros/darknet_ros/yolo_network_config/weights文件下有没有yolov2-tiny.weights, yolov3.weights, yolov2.weights三个模型文件，为了节省时间可以提前自行下载(也可以从CMakeLists.txt中删除检查)
订阅话题
在darknet_ros/config/ros.yaml文件中,修改

subscribers:
  camera_reading:
    topic:/camera/color/image_raw  你自己的话题

运行

source devel/setup.bash
roslaunch darknet_ros darknet_ros.launch

替换模型
在darknet_ros.launch文件中,修改模型

arg name="network_param_file"         default="$(find darknet_ros)/config/yolov2-tiny.yaml"/

在git上下载的这版代码不包括yolov3-tiny,可到yolo官方下载对应的cfg文件,并仿照手动新建yaml文件即可