SLAM实战项目(2) — ROS下运行ORB-SLAM2稠密地图重建

1 运行步骤

(1) 创建工作空间

(2) 修改CmakeList.txt

ROS学习文档：Introduction · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程

1 运行步骤

(1) 创建工作空间

mkdir -p /orbslam2_ws/src
cd /orbslam2_ws/src
cd ..
catkin_make
source ./devel/setup.bash
cd /orbslam2_ws/src
//将稠密建图的代码复制到/src目录下
cd ..
catkin_make
source ./devel/setup.bash

(2) 修改CmakeList.txt

修改在/Examples/ROS/ORB-SLAM2下的CmakeList.txt

find_package(PCL 1.10 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
set(LIBS ${PCL_LIBRARIES})

(3) 编译

到你的ORB-SLAM2文件夹下打开终端

chmod +x ./build_ros.sh
./build_ros.sh

(4) 下载bag文件

下载地址：Computer Vision Group - Dataset Download

(4) 编写roslaunch文件

在./orbslam2_ws/src/ORB_SLAM2想创建launch文件夹，创建orbslam.launch文件

<launch>
    <!--添加被执行的节点-->
    <node name = "ORB_SLAM2" pkg="ORB_SLAM2" type="RGBD" args="/home/qinlong/studydata/orbslam2_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/qinlong/studydata/orbslam2_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml" output="screen"/>
    <!--添加rosbag play-->
    <node pkg="rosbag" type="play" name="bag_play" args="/home/qinlong/bagfiles/rgbd_dataset_freiburg3_long_office_household.bag "/> 
</launch>

(5) 运行

在./orbslam2_ws下source ./devel/setup.bash，然后运行roslaunch ORB_SLAM2 orbslam2.launch可以得到相同的效果。

2 运行报错

报错1：

/usr/bin/ld: warning: libboost_filesystem.so.1.71.0, needed by /opt/ros/noetic/lib/libroscpp.so, may conflict with libboost_filesystem.so.1.80.0
/usr/bin/ld: warning: libopencv_imgcodecs.so.4.2, needed by /opt/ros/noetic/lib/libcv_bridge.so, may conflict with libopencv_imgcodecs.so.3.4
/usr/bin/ld: warning: libopencv_core.so.4.2, needed by /opt/ros/noetic/lib/libcv_bridge.so, may conflict with libopencv_core.so.3.4
/usr/bin/ld: ../../../../lib/libORB_SLAM2.so: undefined reference to `g2o::OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(g2o::Solver*)'
/usr/bin/ld: warning: libboost_filesystem.so.1.71.0, needed by /opt/ros/noetic/lib/libroscpp.so, may conflict with libboost_filesystem.so.1.80.0
/usr/bin/ld: warning: libopencv_imgcodecs.so.4.2, needed by /opt/ros/noetic/lib/libcv_bridge.so, maycollect2: error: ld returned 1 exit status
 conflict with libopencv_imgcodecs.so.3.4
/usr/bin/ld: warning: libopencv_core.so.4.2, needed by /opt/ros/noetic/lib/libcv_bridge.so, may conflict with libopencv_core.so.3.4
/usr/bin/ld: ../../../../lib/libORB_SLAM2.so: undefined reference to `g2o::OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(g2o::Solver*)'
collect2: error: ld returned 1 exit status
make[2]: *** [CMakeFiles/RGBD.dir/build.make:254：../RGBD] 错误 1
make[2]: *** [CMakeFiles/Mono.dir/build.make:254：../Mono] 错误 1
make[1]: *** [CMakeFiles/Makefile2:157：CMakeFiles/RGBD.dir/all] 错误 2
make[1]: *** 正在等待未完成的任务....
make[1]: *** [CMakeFiles/Makefile2:184：CMakeFiles/Mono.dir/all] 错误 2
make: *** [Makefile:130：all] 错误 2

解决办法：这可能是由于在编译 libORB_SLAM2.so 时没有正确链接 g2o 库或者 g2o 库的版本不兼容导致的。使当前项目重新链接到g2o，可以更改CMakeists.txt文件里的内容，增加如下代码

target_link_libraries(project_name g2o -Wl,-rpath=${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o
)

报错2：

/usr/bin/ld: warning: libopencv_core.so.4.2, needed by /opt/ros/noetic/lib/libcv_bridge.so, may conflict with libopencv_core.so.3.4

（十一）ORBSLAM2在ROS下运行_小C酱油兵的博客-CSDN博客

解决办法：报错原因是ROS和自己安装的opencv版本不一致，没有报错的情况下可以忽略警告，报错时根据提示注释掉VR相关代码。

报错3：

在rosrun和rosbag后，bag的信息传不进去

rosrun ORB_SLAM3 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml
rosbag play rgbd_dataset_freiburg3_long_office_household.bag

解决办法：

查看话题：rostopic list

/cam0/image_raw
/cam1/image_raw
/camera/depth/camera_info
/camera/depth/image
/camera/depth_registered/image_raw
/camera/left/image_raw
/camera/rgb/camera_info
/camera/rgb/image_color
/camera/rgb/image_raw
/camera/right/image_raw
/clock
/cortex_marker_array
/imu0
/leica/position
/rosout
/rosout_agg
/tf

查看ros_rgbd.cc下的话题名称

message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "camera/depth_registered/image_raw", 1);

将ros_rgbd.cc下的话题名称修改

message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_color", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/depth/image", 1);

报错4：

经过上一步后发现能传入数据，但是生成的点云异常，

解决办法1： ROS中使用OpenCV读取深度图时，深度图中的0值会被解析为无效（NaN）值，从而未能成功获取深度信息的像素点，可以将深度值赋值为0，在PointCloudMapping::GeneratePointCloud修改如下代码：

void PointCloudMapping::GeneratePointCloud(KeyFrame* kf, cv::Mat &color_img, cv::Mat &depth_img)
    {
        PointCloud::Ptr tmp (new PointCloud());
        for ( int m=0; m<depth_img.rows; m+=3 )
        {
            for ( int n=0; n<depth_img.cols; n+=3 )
            {
                float d = depth_img.ptr<float>(m)[n];

                //修改部分
                if(std::isnan(d))
                {
                    d = 0.0;
                }
                ...
            }
        }
    }

解决办法2：如果进行了滤波处理，可以修改double resolution = 0.00000001，修改后颜色显示正常，但存在明显的漂移，但效果较以前好很多。