ORB-SLAM2的布置(四)ORB-SLAM2构建点云

提要

高博的工作是对基本 ORB SLAM2 的扩展,基本思想是为每个关键帧构造相应的点云,然后依据从 ORB SLAM2 中获取的关键帧位置信息将所有的点云拼接起来,形成一个全局点云地图。

https://github.com/gaoxiang12/ORBSLAM2_with_pointcloud_map

具体的依赖包括:
OpenCV (推荐 3.2 版本)
DBoW2 和 g2o(源文件已经包含在了 github repo 中,随后一起编译即可,这里先不管)
ROS (推荐 melodic)
Eigen3(推荐 3.2 版本)
Pangolin

PCL:由于添加了点云相关的操作,需要安装 PCL 库文件

sudo apt install libpcl-dev

在这里插入图片描述

从高博的 github repo 下载源文件

git clone https://github.com/gaoxiang12/ORBSLAM2_with_pointcloud_map.git

在这里插入图片描述

下载的文件内容
在这里插入图片描述

我们只需要其中的子文件夹 ORB_SLAM2_modified,为了方便后续操作,可以将子文件夹 ORB_SLAM2_modified单独拿出来,放到用户home(~)路径下。

然后把原本 ORB SLAM2 中的 Vocabulary 文件夹和它里面的 ORBvoc.txt.tar.gz 文件拷贝过来,放到 ORB_SLAM2_modified 路径下。

cp -r Vocabulary/ ~/ORB_SLAM2_modified/

在这里插入图片描述

编译

开始进行编译

首先把~/ORB_SLAM2_modified/cmake-build-debug, ~/ORB_SLAM2_modified/Thirdparty/DBoW2/build , ~/ORB_SLAM2_modified/Thirdparty/g2o/build 这三个 build 文件夹删掉,

然后运行 ORB_SLAM2_modified 中的 build.sh,进行编译

 ./build.sh

在这里插入图片描述

编译完成
在这里插入图片描述

测试

完成后进行测试

普通模式

我们测试可以使用完成数据集

可以将上一轮步骤下载的rgbd_dataset_freiburg1_xyz文件复制粘贴到/home/heying/ORB_SLAM2_modified/Examples/datasets
在这里插入图片描述

在使用数据之前,需要对 rgbd_dataset_freiburg1_xyz中的 RGB 文件和 Depth 文件进行匹配。TUM 数据官网 提供了匹配的程序 associate.py,这个也是上一轮下载的py文件
https://svncvpr.in.tum.de/cvpr-ros-pkg/trunk/rgbd_benchmark/rgbd_benchmark_tools/src/rgbd_benchmark_tools/associate.py

将python文件保存,然后运行

python associate.py rgb.txt depth.txt > association.txt

在这里插入图片描述

运行,要注意调用的文件的路径

./bin/rgbd_tum Vocabulary/ORBvoc.bin Examples/RGB-D/TUM1.yaml Examples/datasets/rgbd_dataset_freiburg1_xyz Examples/datasets/rgbd_dataset_freiburg1_xyz/associations.txt

在这里插入图片描述

注意了 有些同学操作的时候可能会出现这种情况
终端没有报错,摄像头的数据也出来了,位姿信息也出来了,点云却是这样的
在这里插入图片描述

不用紧张,你只需要鼠标移动到【viewer】窗口,然后用鼠标滚轮缩小界面就行,出现这三种颜色是因为点云显示的坐标是这三种颜色,窗口离这个坐标太近了,全被挡住了。所以只需要鼠标移动一下即可(tnnd,还以为是啥严重的问题,卡了我一个小时,偶然移动一下才知道是这破原因)


普通模式参数的修改

可以看到在上述显示中,点云地图是黑白的,视觉效果不太好,在高博的 github issue 中有建立彩色点云地图的方法https://github.com/gaoxiang12/ORBSLAM2_with_pointcloud_map/issues/10#issuecomment-309663764

首先修改文件~/ORB_SLAM2_modified/include/Tracking.h

  // Current Frame
    Frame mCurrentFrame;
    cv::Mat mImRGB; 		//Modified place 1

在这里插入图片描述
完成后保存并退出

然后修改 ~/ORB_SLAM2_modified/src/Tracking.cc

第一处

cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double &timestamp)
{
    
    
    mImRGB = imRGB;	//Modified place 1
    mImGray = imRGB;
    mImDepth = imD;

在这里插入图片描述

第二处

    // insert Key Frame into point cloud viewer
    mpPointCloudMapping->insertKeyFrame( pKF, this->mImGray, this->mImDepth );
    mpPointCloudMapping->insertKeyFrame( pKF, this->mImRGB, this->mImDepth );   //Modified place 2

在这里插入图片描述
完成后保存并退出

执行build.sh,重新编译

build.sh

在这里插入图片描述

普通模式颜色测试

然后启动测试

./bin/rgbd_tum Vocabulary/ORBvoc.bin Examples/RGB-D/TUM1.yaml Examples/datasets/rgbd_dataset_freiburg1_xyz Examples/datasets/rgbd_dataset_freiburg1_xyz/associations.txt

在这里插入图片描述


可以看到出现了彩色的信息
在这里插入图片描述


ROS模式

如果要以 ROS 的形式运行,还需要对下载回的文件中的 ROS 文件进行编译。这里也要做一些额外设置


首先设置一下环境变量,也和之前的一样
将ROS 所在目录加入 ROS_PACKAGE_PATH 环境变量中
打开位于主目录的.bashrc文件,进行修改

export ROS_PACKAGE_PATH=${
    
    ROS_PACKAGE_PATH}:~/ORB_SLAM2_modified/Examples/ROS

在这里插入图片描述


完成后保存并退出

然后设置修改ROS文件夹的CMakeLists.txt文件夹
我们需要做的就是参照~/ORB_SLAM2_modified/CMakeLists.txt 文件,把 PCL 相关的设置添加到 ~/ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/CMakeLists.txt文件中。
将这个文件打开,进行以下修改

find_package(Eigen3 3.1.0 REQUIRED)
find_package(Pangolin REQUIRED)
find_package( PCL 1.7 REQUIRED )    ####### 修改1


include_directories(
${
    
    PROJECT_SOURCE_DIR}
${
    
    PROJECT_SOURCE_DIR}/../../../
${
    
    PROJECT_SOURCE_DIR}/../../../include
${
    
    Pangolin_INCLUDE_DIRS}
${
    
    PCL_INCLUDE_DIRS}  ####### 修改2
)

add_definitions( ${
    
    PCL_DEFINITIONS} )   ####### 修改3
link_directories( ${
    
    PCL_LIBRARY_DIRS} ) ####### 修改4

set(LIBS
${
    
    OpenCV_LIBS}
${
    
    EIGEN3_LIBS}
${
    
    Pangolin_LIBRARIES}
${
    
    PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
${
    
    PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so
${
    
    PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so
${
    
    PCL_LIBRARIES} ####### 修改5
)

在这里插入图片描述

做完以上修改之后,再删掉 ~/ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/build文件夹

然后运行 build_ros.sh 编译 ROS 文件

.build_ros.sh

在这里插入图片描述

编译完成
在这里插入图片描述

Examples/RGB-D/TUM1.yaml 中保存了相机的各种参数,其中有一个参数名为 DepthMapFactor
在TUM 数据官网中
https://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats#intrinsic_camera_calibration_of_the_kinect

factor = 5000 # for the 16-bit PNG files
# OR: factor = 1 # for the 32-bit float images in the ROS bag files

因此,在运行 ROS 之前需要将这个参数改为 DepthMapFactor: 1.0

具体操作时,复制了TUM1.yaml文件,命名为 TUM1_ROS.yaml,修改其中的参数 DepthMapFactor: 1.0
在这里插入图片描述

然后要记得在上一节的流程中下载的bag包的复制粘贴到/home/heying/ORB_SLAM2_modified/Examples/datasets
在这里插入图片描述

ROS模式测试

修改之后,运行以下命令进行测试

roscore
rosrun ORB_SLAM2  RGBD  Vocabulary/ORBvoc.bin Examples/RGB-D/TUM1_ROS.yaml

在这里插入图片描述

播放bag包,不过需要对播放出来的数据话题做转换

rosbag play --pause Examples/datasets/rgbd_dataset_freiburg1_xyz.bag /camera/rgb/image_color:=/camera/rgb/image_raw /camera/depth/image:=/camera/depth_registered/image_raw

在这里插入图片描述

当按下空格键播放后,便可以看到输出的点云地图
在这里插入图片描述

地图保存

参考文件
PCL:读取指定路径下的pcd点云 | 将点云保存至指定路径

问题这些只能实时查看点云地图,不能保存。我们可以稍微修改一下文件
~/ORB_SLAM2_modified/src/pointcloudmapping.cc,在其中调用 PCL 库的 pcl::io::savePCDFileBinary函数就可以保存点云地图了。


首先打开文件

gedit  src/pointcloudmapping.cc

在这里插入图片描述
进行修改

//添加头文件
#include <pcl/io/pcd_io.h>                        //Modified place3 地图保存的头文件

在这里插入图片描述

//保存地图,这里为了管理,新建了pcd文件,将所有保存的pcd放入里面
        pcl::io::savePCDFileBinary("/home/heying/ORB_SLAM2_modified/pcd/vslam.pcd", *globalMap);   //Modified place 4 保存地图的命令

在这里插入图片描述

完成后保存并退出,然后进行编译

cd build/
make -j4

在这里插入图片描述

编译完成
在这里插入图片描述


然后再运行前述的各个 SLAM 命令就可以在 ~/ORB_SLAM2_modified/pcd路径下产生一个名为 vslam.pcd 的点云文件。

roscore
rosrun ORB_SLAM2  RGBD  Vocabulary/ORBvoc.bin Examples/RGB-D/TUM1_ROS.yaml
rosbag play --pause Examples/datasets/rgbd_dataset_freiburg1_xyz.bag /camera/rgb/image_color:=/camera/rgb/image_raw /camera/depth/image:=/camera/depth_registered/image_raw

正常运行
在这里插入图片描述

当播放完成后程序现在不会自动关闭,需要按下CTRL+C停止,
然后可以看到在~/ORB_SLAM2_modified/pcd路径下产生一个名为 vslam.pcd 的点云文件。
在这里插入图片描述


为了查看这个 pcd 文件,我们需要安装相应的工具

sudo apt-get install pcl-tools

在这里插入图片描述


完成后通过 pcl_viewer 查看 vslam.pcd

pcl_viewer vslam.pcd

在这里插入图片描述
开启成功
在这里插入图片描述

至此,使用离线包实现摄像头SLAM构建点云地图的流程完成

猜你喜欢

转载自blog.csdn.net/Xiong2840/article/details/128372804