使用Realsense D435i运行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"/>
11再编写一个launch文件,命名为my_vins_d435.launch,也保存在~/catkin_ws/src/realsense-ros/realsense2_camera/launch/目录下,内容如下:
<launch>
<include file="$(find realsense2_camera)/launch/rs_camera_vins.launch"/>
<include file="$(find vins)/launch/vins_rviz.launch"/>
<!--node pkg="vins" type="vins_node" name="vins_node">
<param name="config_file" value="~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml""/>
</node-->
</launch>
11
命令行输入roslaunch realsense2_camera my_vins_d435.launch即可。
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查看:外参查看方法https://blog.csdn.net/mystyle_/article/details/109353870
其中外参:
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
运行
source ~/catkin_ws/devel/setup.bash
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)
2 esdf建图
fiesta比voxblox简单得多,易于上手和后续修改,我就用的fiesta https://github.com/HKUST-Aerial-Robotics/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、修改mian函数
改成:
fiesta::Fiesta<sensor_msgs::Image::ConstPtr, nav_msgs::Odometry::ConstPtr> esdf_map(node);
3、终端启动即可
roslaunch fiesta D435i.launch
3 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"/