转载自: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文件即可