ORB-SLAM2 在线构建稠密点云（二）

更新日志

• 2021-4-26 更新 pointcloud_mapping 节点，整理评论区提出的错误信息。
• 2020-2-3 创建 pointcloud_mapping 节点，并修改了ORB-SLAM2在ROS中编译错误的情况。

在之前的一篇博客（ORB-SLAM2 在线构建稠密点云（一））中我们是把修改后的 ORB_SLAM2_PointCloud 代码编译成一个库，然后新建一个ROS节点调用这个库，实现利用RGBD相机在线建图的，这种方式有以下个弊端。

问题一：
每次我们修改 ORB_SLAM2_PointCloud 以后，我们需要把 libORB_SLAM2.so 文件重新复制到ROS工作空间中，如果我们修改过 .h　头文件的化我们还需要把头文件也复制过去，这就造成了一定的不方便。其实在ORB 中写好有编译文件可以支持ROS的，因此我们需要在前面的基础上进行一定的优化，使用ORB提供的ROS 节点进行修改。

问题二：
ORB-SLAM2的核心是估计位姿，后续也推出了VINS-mono 和ORB-SLAM3这些系统，如果每次我们想使用其他的SLAM系统或者是新推出SLAM系统。我们又需要重新去读这个SLAM系统的代码，并重复修改建图和地图管理程序，则工作量很繁琐。

问题三：
ORB_SLAM2_PointCloud 代码只支持RGBD模式的相机，而双目、单目计算深度的方式不同，双目依靠立体匹配来计算深度。后续使用双目计算深度则很难在现有的代码上进行修改。

因此接下来的博客我们先解决这两个问题，对代码结构进行优化，我们使用ROS环境，把系统分为3个节点，数据获取（驱动）、位姿估计（SLAM系统）、地图构建（点云和八叉树。

修改思路
把建图系统分为了三个节点，如下图所示。第一个节点作为驱动节点，采集摄像头传感器的数据。第二个节点利用ORB-SLAM主要做姿态估计，提供Tcw。第三个节点作为建图节点，收集第一和第二节点的建图节点接收图像数据和位姿数据，进行点云的拼接。

修改以后的代码：

• ORB-SLAM2 修改后的代码 必须切换到V1.0.0这个分支，master分支是原版的ORB-SLAM2
• pointcloud_mapping ROS节点 [1]

1、 ORB_SLAM2 ROS 编译错误解决

首先在编译原版ORB_SLAM2 ROS节点的时候会遇到一个错误（我使用的系统版本是ubuntu1604+ROS Kinetic）,解决这个错误以后我们再修改对应的ROS节点，实现实时构建。

首先在github上下载ORB_SLAM2的代码，按照ORB_SLAM2在github上提供的步骤我们先编译 ORB_SLAM2 ，然后开始编译ORB_SLAM2 的ROS节点

1、 指向ORB的ROS目录作为工作ROS包。

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/crp/crp/SLAM/ORB_SLAM2/Examples/ROS

２、运行自带的脚本

./build_ros.sh

这时候在ubuntu1604+ROS-K版本上会提示以下错误，
/usr/lib/x86_64-linux-gnu/libboost_system.so: error adding symbols: DSO missing from command line

这是由于没有找到boost库的原因,参考下列文章进行解决[3]. 将系统中的库文件直接拷贝出来，然后在 Examples/ROS/ORB_SLAM2 目录下的　CMakeLists.txt　文件中指向这个库文件即可。这样就可以编译通过了。

３、接下来我们按照github上提供的启动ROS节点命令进行测试，确认工程编译没有问题

• 1）单目

rosrun ORB_SLAM2 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE

• 2）双目

rosrun ORB_SLAM2 Stereo Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml true

rosbag play --pause MH_03_medium.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw

在线运行EUROC数据集

ORB_SLAM2在使用rosbag运行的时候，出现跟踪丢失的现象比较频繁，主要集中中相机旋转的时候。

• 3）RGB-D

修改 ros_rgbd.cc 第69行的接收深度图topic为 “/camera/depth/image” ，彩色图topic改为 “/camera/rgb/image_color” ，重新用 ./build_ros.sh 脚本编译，然后启动RGBD节点

然后启动RGBD节点

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml 

rosbag play --pause rgbd_dataset_freiburg1_room.bag

使用TUM提供的RGBD rosbag　没有成功运行，目前只发现了图像的频率只有12-13HZ，而深度图的频率是13-14HZ，彩色图和深度图频率不一致，不知道是不是RGB图和Depth图时间戳的问题(有知道的小伙伴烦请指导一下)。这个在使用真实相机的时候会得到解决。这里我自己写了一个节点把TUM的文件夹中的时间对齐以后的图片发布在topic上得到了一个比较好的效果：

在线运行TUM数据集

２、RGBD相机在线运行

RGBD相机我们使用的是Astra的RGBD相机，这个相机自带有ROS节点，驱动可参考wiki进行安装。

2.1 添加ROS节点文件

修改 Examples/ROS/ORB_SLAM2/src 目录下的文件。复制 ros_rgbd.cc 文件然后重命名为 astra.cc ,在 astra.cc 填入以下内容：

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>

#include <ros/ros.h>
#include <ros/spinner.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Path.h>
#include <tf/transform_broadcaster.h>
#include <cv_bridge/cv_bridge.h>

#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include<opencv2/core/core.hpp>

#include"../../../include/System.h"

using namespace std;

class ImageGrabber
{
public:
	ros::NodeHandle nh;
	ros::Publisher  pub_rgb,pub_depth,pub_tcw,pub_camerapath;
	size_t mcounter=0;	 
	nav_msgs::Path  camerapath;

    ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM),nh("~")
    {
	   //创建ROS的发布节点

	   pub_rgb= nh.advertise<sensor_msgs::Image> ("RGB/Image", 10); 
	   pub_depth= nh.advertise<sensor_msgs::Image> ("Depth/Image", 10); 
	   pub_tcw= nh.advertise<geometry_msgs::PoseStamped> ("CameraPose", 10); 
	   pub_camerapath= nh.advertise<nav_msgs::Path> ("Path", 10); 
    }

    void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD);

    ORB_SLAM2::System* mpSLAM;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "RGBD");
    ros::start();

    if(argc != 3)
    {
        cerr << endl << "Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings" << endl;        
        ros::shutdown();
        return 1;
    }    

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);

    ImageGrabber igb(&SLAM);

    ros::NodeHandle nh;

    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 10);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/depth/image", 10);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub,depth_sub);
    sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));

    ros::spin();

    // Stop all threads
    SLAM.Shutdown();

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

    ros::shutdown();

    return 0;
}

void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD)
{
    // Copy the ros image message to cv::Mat.
    cv_bridge::CvImageConstPtr cv_ptrRGB;
    try
    {
        cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv_bridge::CvImageConstPtr cv_ptrD;
    try
    {
        cv_ptrD = cv_bridge::toCvShare(msgD);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    bool  isKeyFrame =true;
    cv::Mat Tcw;
    Tcw = mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());
    if (!Tcw.empty())
	{
				  //cv::Mat Twc =Tcw.inv();
				  //cv::Mat TWC=orbslam->mpTracker->mCurrentFrame.mTcw.inv();  
				  cv::Mat RWC= Tcw.rowRange(0,3).colRange(0,3);  
				  cv::Mat tWC= Tcw.rowRange(0,3).col(3);

				  tf::Matrix3x3 M(RWC.at<float>(0,0),RWC.at<float>(0,1),RWC.at<float>(0,2),
							      RWC.at<float>(1,0),RWC.at<float>(1,1),RWC.at<float>(1,2),
							      RWC.at<float>(2,0),RWC.at<float>(2,1),RWC.at<float>(2,2));
				  tf::Vector3 V(tWC.at<float>(0), tWC.at<float>(1), tWC.at<float>(2));
				  
				 tf::Quaternion q;
				  M.getRotation(q);
				  
			      tf::Pose tf_pose(q,V);
				  
				   double roll,pitch,yaw;
				   M.getRPY(roll,pitch,yaw);
				   //cout<<"roll: "<<roll<<"  pitch: "<<pitch<<"  yaw: "<<yaw;
				  // cout<<"    t: "<<tWC.at<float>(0)<<"   "<<tWC.at<float>(1)<<"    "<<tWC.at<float>(2)<<endl;
				   
				   if(roll == 0 || pitch==0 || yaw==0)
					return ;
				   // ------
				  
				  std_msgs::Header header ;
				  header.stamp =msgRGB->header.stamp;
				  header.seq = msgRGB->header.seq;
				  header.frame_id="camera";
				  //cout<<"depth type: "<< depth. type()<<endl;
				  
				  sensor_msgs::Image::ConstPtr rgb_msg = msgRGB;
				  sensor_msgs::Image::ConstPtr depth_msg=msgD;
				  
				 geometry_msgs::PoseStamped tcw_msg;
				 tcw_msg.header=header;
				 tf::poseTFToMsg(tf_pose, tcw_msg.pose);
				  
				 camerapath.header =header;
				 camerapath.poses.push_back(tcw_msg);
				  
				 pub_tcw.publish(tcw_msg);	                      //Tcw位姿信息
				 pub_camerapath.publish(camerapath);  //相机轨迹
				 if( isKeyFrame)
				{
					pub_rgb.publish(rgb_msg);
					pub_depth.publish(depth_msg);
				}
	}
	else
	{
	  cout<<"Twc is empty ..."<<endl;
	}
}

2.2 修改配置文件

a. 修改 Examples/ROS/ORB_SLAM2 目录下的 CMakeLists.txt 文件，增加如下内容：

b. 重新用 ./build_ros.sh 脚本编译工程

c. 在 ORB_SLAM2/Examples/ROS/ORB_SLAM2 目录下新建文件 Astra.yaml ，根据你使用的相机矫正参数，修改RGB-D相机的内参数。 （实际上只需要修改　fx fy cx cy 这几个参数即可）

2.3 启动运行RGBD在线节点

打开两个终端输入以下内容，启动节点

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/crp/crp/SLAM/ORB_SLAM2/Examples/ROS
rosrun ORB_SLAM2 astra Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Astra.yaml

启动相机

roslaunch astra_launch astra.launch

该节点一共会发布4个topic, 以下是测试效果：
/RGBD/CameraPose 和 /RGBD/Path 分别是发布相机的位姿和轨迹
/RGBD/RGB/Image 和 /RGBD/Depth/Image 这是用于发布关键帧的彩色图像帧和深度图像帧

下面分别是用RGB-D相机和TUM数据集作为演示效果。

ORB-SLAM Astra相机运行

在线运行TUM数据集

读过代码的同学可以发现在节点文件中有一个状态变量“isKeyFrame”，这个是用来指示这个图像帧是否是关键帧用的，因为在构建点云的时候我们不需要用每一帧都进行拼接，这样对点云的维护会增大计算量，我们只挑选关键帧来进行重构点云。这个功能需要修改原版ORB_SLAM2的接口函数，这里暂时留给大家，我们在下一篇博客中修改。

3、在ORB-SLAM2上增加Keyframe状态接口

由于我们建图的线程更新需要很大的计算量。其次，有些图像之间的重叠度很大，我们没有必要都为其计算点云，因此我们利用ORB-SLAM2挑选的关键帧来进行建图。

增加keyframe的接口需要自己弄清楚ORB_SLAM2的调用过程，然后逐层添加关键帧状态标志位。在ROS节点中我们是通过System::TrackRGBD() 这个接口函数实现调用ORB_SLAM库
，在System::TrackRGBD() 又调用了mpTracker->GrabImageRGBD()函数,　在Tracking这个类中，mpTracker->GrabImageRGBD() 最终调用函数Tracking::Track() 计算位姿态，并用函数NeedNewKeyFrame()　决定是否插入关键帧。

• cv::Mat System::TrackRGBD(im,depthmap, timestamp)
• • cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);
• • • Tracking::Track()
• • • • NeedNewKeyFrame()

在函数Tracking::Track() 中有一个字段标记了是否产生新的关键帧（如下），我们使用这个字段来判断是否有新关键帧产生

// Check if we need to insert a new keyframe
            if(NeedNewKeyFrame())
			 CreateNewKeyFrame();

找到这样一个关系以后我们依次对调用过程中的函数添加一个状态变量输出，具体作法如下：
step1: 我们复制函数　cv::Mat System::TrackRGBD() ，然后增加一个状态变量　isKeyframe

cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp,bool& isKeyframe)

step2: 同样对函数Tracking::GrabImageRGBD()　增加一个状态变量　isKeyframe

cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double &timestamp,bool& isKeyframe);

step３: 同样对函数Tracking::Track()　增加一个状态变量　isKeyframe

  void Track(bool& isKeyframe);

step４: 同样对函数Tracking::Track()中的代码段

// Check if we need to insert a new keyframe
            if(NeedNewKeyFrame())
				 CreateNewKeyFrame();

修改为：
　
step５: 修改我们在上一篇博客中编写的在线运行相机的ROS节点中的调用接口,把 astra.cc 文件第135行的

Tcw = mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());

改为:

Tcw = mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec(),isKeyFrame);

由于我调整了一下Tcw的发布位置，因此修改后的函数为：

void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD)
{
    // Copy the ros image message to cv::Mat.
    cv_bridge::CvImageConstPtr cv_ptrRGB;
    try
    {
        cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv_bridge::CvImageConstPtr cv_ptrD;
    try
    {
        cv_ptrD = cv_bridge::toCvShare(msgD);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    bool  isKeyFrame =true;
    cv::Mat Tcw;
    Tcw = mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec(),isKeyFrame);
    if (!Tcw.empty())
	{
				  //cv::Mat Twc =Tcw.inv();
				  //cv::Mat TWC=orbslam->mpTracker->mCurrentFrame.mTcw.inv();  
				  cv::Mat RWC= Tcw.rowRange(0,3).colRange(0,3);  
				  cv::Mat tWC= Tcw.rowRange(0,3).col(3);

				  tf::Matrix3x3 M(RWC.at<float>(0,0),RWC.at<float>(0,1),RWC.at<float>(0,2),
							      RWC.at<float>(1,0),RWC.at<float>(1,1),RWC.at<float>(1,2),
							      RWC.at<float>(2,0),RWC.at<float>(2,1),RWC.at<float>(2,2));
				  tf::Vector3 V(tWC.at<float>(0), tWC.at<float>(1), tWC.at<float>(2));
				  
				 tf::Quaternion q;
				  M.getRotation(q);
				  
			      tf::Pose tf_pose(q,V);
				  
				   double roll,pitch,yaw;
				   M.getRPY(roll,pitch,yaw);
				   //cout<<"roll: "<<roll<<"  pitch: "<<pitch<<"  yaw: "<<yaw;
				  // cout<<"    t: "<<tWC.at<float>(0)<<"   "<<tWC.at<float>(1)<<"    "<<tWC.at<float>(2)<<endl;
				   
				   if(roll == 0 || pitch==0 || yaw==0)
					return ;
				   // ------
				  
				  std_msgs::Header header ;
				  header.stamp =msgRGB->header.stamp;
				  header.seq = msgRGB->header.seq;
				  header.frame_id="camera";

				  //cout<<"depth type: "<< depth. type()<<endl;
				  
				  sensor_msgs::Image::ConstPtr rgb_msg = msgRGB;
				  sensor_msgs::Image::ConstPtr depth_msg=msgD;
 
				 geometry_msgs::PoseStamped tcw_msg;
				 tcw_msg.header=header;
				 tf::poseTFToMsg(tf_pose, tcw_msg.pose);
				  
				 camerapath.header =header;
				 camerapath.poses.push_back(tcw_msg);
				 pub_camerapath.publish(camerapath);  //相机轨迹
				 if( isKeyFrame)
				{
					pub_tcw.publish(tcw_msg);	                      //Tcw位姿信息
					pub_rgb.publish(rgb_msg);
					pub_depth.publish(depth_msg);
				}
	}
	else
	{
	  cout<<"Twc is empty ..."<<endl;
	}
}

4、 pointcloud_mapping 节点

首先 pointcloud_mapping 节点的代码可以在github上下载 [1]，使用的时候请切换到V1.0.0分支。这个代码包的功能主要是利用接收ORB-SLAM2输出的位姿和关键帧这两个话题进行建图。

这个节点的主要功能就是接受位姿估计节点发布的彩色图、深度图、Tcw位姿数据，然后利用这些信息重建出点云，并旋转到全局坐标系下，对点云进行拼接。
ROS部分接口代码为：　所有的操作在　PointCloudMapper　这个类中实现。

int main(int argc, char **argv)
{
    std::string cameraParamFile;
    ros::init(argc, argv, "pointcloud_mapping", ros::init_options::AnonymousName);
    if(!ros::ok())
    {
	     cout<<"ros init error..."<<endl;
	    return 0;
    }
 	float fx =515.2888; //Astra camera
	float cx =317.9098;
	float fy =517.6610;
	float cy =241.5734;
	float resolution =0.01;
	Mapping::PointCloudMapper mapper(fx,fy,cx,cy,resolution);;
	mapper.viewer();
	cout<<"ros shutdown ..."<<endl;
    return 0;
}

首先把 pointcloud_mapping 这个ROS包拷贝到的你的ROS工作空间

 cd catkin_ws/src
 git clone -b v1.0.0 https://github.com/RuPingCen/pointcloud_mapping.git
 cd ../
 catkin_make

4.1 利用TUM的RGB-D bag包运行

我们还是需要启动相机和ORB-SLAM2 的节点 。 打开两个终端输入以下内容，
step1： 启动ORB-SLAM2节点

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/crp/crp/SLAM/ORB_SLAM2/Examples/ROS
rosrun ORB_SLAM2 astra Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/TUM1_ROSbag.yaml

step2： 启动 pointcloud_mapping 节点 RGBD建图

source devel/setup.bash
roslaunch pointcloud_mapping tum1.launch

step3： 播放TUM bag包

rosbag play rgbd_dataset_freiburg1_room.bag /camera/rgb/image_color:=/camera/rgb/image_raw /camera/depth/image:=/camera/depth/image

下图是在TUM数据集上运行的一个效果图，从图中可以看到点云有明显的没有对齐的现象，我猜测主要是由于深度图和位姿之间的时间戳没有对齐的原因，需要我们用一些额外的方法解决这个问题。

ORB-SLAM分布式建图效果

4.2 利用Astra RGB-D相机运行

首先我们还是需要启动相机和ORB-SLAM2 的节点 。 打开两个终端输入以下内容，
step1： 启动ORB-SLAM2节点

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/crp/crp/SLAM/ORB_SLAM2/Examples/ROS
rosrun ORB_SLAM2 astra Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Astra.yaml

step2： 启动 pointcloud_mapping 节点 RGBD建图

source devel/setup.bash
roslaunch pointcloud_mapping  astra.launch

step3： 启动相机

roslaunch astra_launch astra.launch

该节点一共会发布4个topic, 以下是测试效果：
/RGBD/CameraPose 和 /RGBD/Path 分别是发布相机的位姿和轨迹
/RGBD/RGB/Image 和 /RGBD/Depth/Image 这是用于发布关键帧的彩色图像帧和深度图像帧

下图是一个用Astra RGBD相机构建的3D点云的过程，实测发现真实的相机运行效果会比用TUM的ROS bag包数据好。

ORB-SLAM Astra相机在线构建octomap

关于八叉树地图和点云地图之间在ROS里面在线转换可以参考我之前的博客 Octomap 在ROS环境下实时显示

修改后的 ORB-SLAM 部分的代码位于Github [2]上，使用的时候请切换到V1.0.0分支
修改后的 pointcloud_mapping 部分的代码位于Github [1]上，使用的时候请切换到V1.0.0分支

参考资料

[1] https://github.com/RuPingCen/pointcloud_mapping
[2] https://github.com/RuPingCen/ORB_SLAM2/tree/v1.0.0
[3] https://blog.csdn.net/sinat_38343378/article/details/78883919

上一篇 ：ORB-SLAM2 在线构建稠密点云（一）　　　 下一篇 ：ORB-SLAM2 在线构建稠密点云（三）

如果大家觉得文章对你有所帮助，麻烦大家帮忙点个赞。O(∩_∩)O

欢迎大家在评论区交流讨论（cenruping@vip.qq.com）