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

这篇博客接着上一篇博客中的内容继续，我们在前面完成了位姿估计器的修改，也就是用ORB-SLAM提供的ROS节点实现了在线运行。回顾一下我们的目标是把建图系统分为了三个节点，驱动节点主要是负责接收传感器的数据，位姿估计节点（我们使用的是ORB-SLAM）接收驱动节点的数据并输出相机的位姿，最后由建图节点接收图像数据和位姿数据，进行点云的拼接。整个系统的框图如下图所示：因此在篇博客中我们首先把ORB-SLAM添加了一个关键帧的输出接口，其次构建一个点云节点接收关键帧拼接生成点云数据。

1 增加Keyframe状态接口

增加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: "<
				  // 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_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;
	}
}

这样我们就实现了，发布相机所有的轨迹，发布关键帧的图像和位姿数据。修改后的ORB-SLAM部分的代码位于Github上，使用的时候请切换到V1.0.0分支

2 编写建图线程

2.1 整体思路

建图线程的作用基本是接受由位姿估计节点输出的彩色图、深度图和位姿Tcw成对的数据。然后把彩色图和深度图进行拼接构建点云。由pcl对点云进行维护和显示。

2.2 编写ROS节点

在ROS工作空间新建一个名为 "pointcloud_mapping" 的包，这个包会依赖于点云库，请确保你的电脑上已经安装了点云库。
这个节点的主要功能就是接受位姿估计节点发布的彩色图、深度图、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部分的代码位于Github上，使用的时候请切换到V1.0.0分支

2.3 Astra相机运行效果

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

 cd catkin_ws/src
 git clone https://github.com/RuPingCen/pointcloud_mapping.git
 cd ../
 catkin_make

这里需要开3个窗口
第1个窗口用于启动RGBD相机

roslaunch astra_launch astra.launch

第2个窗口用于启动ORB-SLAM

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

第3个窗口用于启动 pointcloud_mapping 节点

source devel/setup.bash
roslaunch astra_launch astra.launch

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

ORB-SLAM Astra相机在线构建octomap

2.４ TUM数据集运行效果

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

ORB-SLAM分布式建图效果

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

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

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

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

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