OctoMap-实时点云及拓扑图

pointcloud_mapping

此章节我们学习实时将rosbag或是相机发布的rgb彩色图和depth深度图合成点云图,并根据相机的位姿信息对点云进行融合。

生成点云后,将点云数据发布到 /pointcloud_mapping/Local/PointCloudOutput 话题,通过 octomap_server_node 节点生成对应的八叉树拓扑图

具体流程如下:

1、产生新的关键帧

2、构建关键帧对应的点云图

3、将新的点云图和旧的进行叠加

4、更新八叉树拓扑图

5、更新相机位姿

IO介绍

配置信息

  1. 相机内参
  2. 配置信息

输入数据:

  1. 彩色图 :/RGBD/RGB/Image
  2. 深度图:/RGBD/Depth/Image
  3. 相机位姿:/RGBD/CameraPose

输出数据:

  1. 变换后的点云
  2. 点云转换得到的八叉树图

使用方式

使用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:

orbslam2.png

点云与Octomap:

octomap.png

使用相机作为输入源

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")