OctoMap-实时点云及拓扑图¶
pointcloud_mapping¶
此章节我们学习实时将rosbag或是相机发布的rgb彩色图和depth深度图合成点云图,并根据相机的位姿信息对点云进行融合。
生成点云后,将点云数据发布到 /pointcloud_mapping/Local/PointCloudOutput
话题,通过 octomap_server_node
节点生成对应的八叉树拓扑图。
具体流程如下:
1、产生新的关键帧
2、构建关键帧对应的点云图
3、将新的点云图和旧的进行叠加
4、更新八叉树拓扑图
5、更新相机位姿
IO介绍¶
配置信息
- 相机内参
- 配置信息
输入数据:
- 彩色图 :/RGBD/RGB/Image
- 深度图:/RGBD/Depth/Image
- 相机位姿:/RGBD/CameraPose
输出数据:
- 变换后的点云
- 点云转换得到的八叉树图
使用方式¶
使用rosbag作为输入源¶
1、查看rosbag发布的数据 `
该数据集可以在这个链接下载:https://vision.in.tum.de/rgbd/dataset/freiburg1/rgbd_dataset_freiburg1_room.bag
使用命令:rosbag info rgbd_dataset_freiburg1_room.bag
ty@ty-PC:~/Lesson/SLAM/orbslam_01/Resource/data$ rosbag info rgbd_dataset_freiburg1_room.bag
path: rgbd_dataset_freiburg1_room.bag
version: 2.0
duration: 49.3s
start: May 10 2011 20:51:46.96 (1305031906.96)
end: May 10 2011 20:52:36.24 (1305031956.24)
size: 845.5 MB
messages: 41803
compression: bz2 [2724/2724 chunks; 30.08%]
uncompressed: 2.7 GB @ 56.9 MB/s
compressed: 843.8 MB @ 17.1 MB/s (30.08%)
types: sensor_msgs/CameraInfo [c9a58c1b0b154e0e6da7578cb991d214]
sensor_msgs/Image [060021388200f6f0f447d0fcd9c64743]
sensor_msgs/Imu [6a62c6daae103f4ff57a132d6f95cec2]
tf/tfMessage [94810edda583a504dfda3829e70d7eec]
visualization_msgs/MarkerArray [f10fe193d6fac1bf68fad5d31da421a7]
topics: /camera/depth/camera_info 1361 msgs : sensor_msgs/CameraInfo
/camera/depth/image 1360 msgs : sensor_msgs/Image
/camera/rgb/camera_info 1362 msgs : sensor_msgs/CameraInfo
/camera/rgb/image_color 1362 msgs : sensor_msgs/Image
/cortex_marker_array 4914 msgs : visualization_msgs/MarkerArray
/imu 24569 msgs : sensor_msgs/Imu
/tf 6875 msgs : tf/tfMessage
可见其中rgb图像话题对应
/camera/rgb/image_color 1362 msgs : sensor_msgs/Image
depth深度图像话题对应
/camera/depth/image 1360 msgs : sensor_msgs/Image
2、启动rosbag数据
rosbag play -l --pause rgbd_dataset_freiburg1_room.bag
!注意:这里启动后数据暂时不会发布,等下边的接收及处理节点启动起来后,再到这个控制台按下空格,才真正发布数据!
3、启动数据处理节点
这里要使用添加新功能后的ORB_SLAM2版本:https://gitee.com/tangyang/ORB_SLAM2
指定输入的rgb话题和depth话题(刚刚通过rosbag info xxx.bag得到的)
rosrun ORB_SLAM2 RGBD_pose Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/TUM1_rosbag.yaml _rgb:=/camera/rgb/image_color _depth:=/camera/depth/image
过一小会儿,会有两个新的窗口打开,暂时没有数据内容,因为我们还未真正发布数据。
4、启动点云及Octomap渲染节点
由于我们启动的rosbag的名称rgbd_dataset_freiburg1_room.bag
中包含数字1,说明对应的相机设备为tum1,故使用tum1.launch(其中不同的只是相机内参)
这里要先确认你的octomap环境已经配置好,参考链接:https://gitee.com/tangyang/pointcloud_publisher
cd src
# 下载代码
git clone https://gitee.com/tangyang/pointcloud_mapping.git
cd ..
catkin_make -j4
roslaunch pointcloud_mapping tum1.launch
此时会有一个rviz窗口和一个点云的viewer窗口启动
5、最后,回到rogbag控制台,按下空格!
刚刚开启的那些窗口,应该开始进行实时建图与定位,且都会显示出数据了。
orb_slam2:
点云与Octomap:
使用相机作为输入源¶
1、插上相机设备
很多时候,忘了把相机连接电脑,也是出问题的原因。
2、启动相机节点
这里使用的是奥比中光的Astra相机(乐视体感相机),代码:https://gitee.com/tangyang/ros_astra_camera。
roslaunch astra_camera astrapro.launch
3、启动数据处理节点
这里同样要使用添加新功能后的ORB_SLAM2版本:https://gitee.com/tangyang/ORB_SLAM2
指定输入的rgb话题和depth话题(可以通过rqt_image_view查看确认)
rosrun ORB_SLAM2 RGBD_pose Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/TUM1_rosbag.yaml _rgb:=/camera/rgb/image_raw _depth:=/camera/depth_registered/image_raw
过一小会儿,会有两个新的窗口打开。
这里的
_rgb
和_depth
参数其实也可以不写,因为代码中这两个的默认值就和我们指定的是一样的。
4、启动点云及Octomap渲染节点
这里要先确认你的octomap环境已经配置好,参考链接:https://gitee.com/tangyang/pointcloud_publisher
roslaunch pointcloud_mapping astra.launch
此时会有一个rviz窗口和一个点云的viewer窗口启动
!注意:
如果点云没有更新或rviz界面没有变化,但是slam页面又是正常有数据的,那说明关键帧还没有生成,拿起摄像头转一些角度动一动就可以了。
查看融合后的点云¶
在使用点云渲染节点 pointcloud_mapping 后,会在其源码目录输出.pcd文件,我们可以使用pcl_viewer进行查看
pcl_viewer src/pointcloud_mapping/resultPointCloudFile.pcd
其他¶
可以在执行cmake和make编译命令之前,在根目录的 CMakeLists.txt
文件中(12行)添加以下两行配置,来屏蔽大量的代码过时警告(对编译结果没有影响),方便编译出错时候排查问题。
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-declarations")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated")