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

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

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

问题二：
ORB_SLAM2_PointCloud 代码只支持RGBD模式的相机，而双目、单目计算深度的方式不同，双目依靠立体匹配来计算深度。

因此接下来的博客我们先解决这两个问题，对代码进行优化，我们调整一下系统架构，我们设计三个主要的节点包，第一个节点作为驱动节点，采集摄像头传感器的数据。第二个节点主要做姿态估计，提供Tcw。第三个节点作为建图节点，收集第一和第二节点的数据构建点云地图或者八叉树地图。

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

1 ORB_SLAM2 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库的原因,参考下列文章进行解决[1]. 将系统中的库文件直接拷贝出来，然后在 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
#include
#include
#include

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 

#include

#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: "<
				  // cout<<"    t: "<(0)<<"   "<(1)<<"    "<(2)<
				   
				   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()<
				  
				  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 这几个参数即可）

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 535.4
Camera.fy: 539.2
Camera.cx: 320.1
Camera.cy: 247.6

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0

Camera.width: 640
Camera.height: 480

# Camera frames per second 
Camera.fps: 30.0

# IR projector baseline times fx (aprox.)
Camera.bf: 40.0

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

# Close/Far threshold. Baseline times.
ThDepth: 40.0

# Deptmap values factor
DepthMapFactor: 1.0

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000

# ORB Extractor: Scale factor between levels in the scale pyramid 	
ORBextractor.scaleFactor: 1.2

# ORB Extractor: Number of levels in the scale pyramid	
ORBextractor.nLevels: 8

# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast			
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500

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的接口函数，这里暂时留给大家，我们在下一篇博客中修改。

总结：　到这里我们利用ORB_SLAM2提供的ROS节点作为位姿估计节点，在线运行了RGBD和Stereo的例子，这里我们还没有实现点云地图的构建，后面一篇博客我们把建图单独作为一个节点，接收位姿估计节点输出的Tcw数据，实现点云地图的构建。

[1] https://blog.csdn.net/sinat_38343378/article/details/78883919

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

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

欢迎大家在评论区交流讨论（[email protected]）