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

本章节中我们着重介绍如何使用双目相机完成点云重建。

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

1、增加双目下的Keyframe状态接口

再双目下增加关键帧状态接口和再RGB-D模式下是一样的。

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

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

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

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

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

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

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

cv::Mat GrabImageStereo(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节点中的调用接口,把 zed.cc 文件第135行的

Tcw=mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec());

改为:

Tcw=mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec(),isKeyFrame);

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

void ImageGrabber::GrabStereo(const sensor_msgs::Image::ConstPtr msgLeft,const sensor_msgs::Image::ConstPtr msgRight)
{
    // Copy the ros image message to cv::Mat.
    cv_bridge::CvImageConstPtr cv_ptrLeft;
    try
    {
        cv_ptrLeft = cv_bridge::toCvShare(msgLeft);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv_bridge::CvImageConstPtr cv_ptrRight;
    try
    {
        cv_ptrRight = cv_bridge::toCvShare(msgRight);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    
    
	bool  isKeyFrame =true;
	cv::Mat Tcw;
	
    if(do_rectify)
    {
        cv::Mat imLeft, imRight;
        cv::remap(cv_ptrLeft->image,imLeft,M1l,M2l,cv::INTER_LINEAR);
        cv::remap(cv_ptrRight->image,imRight,M1r,M2r,cv::INTER_LINEAR);
        Tcw=mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec(),isKeyFrame);
    }
    else
    {
		 //Tcw=mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec());
        Tcw=mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec(),isKeyFrame);
    }
     
     if (!Tcw.empty())
	{
		cv::Mat Twc =Tcw.inv();
		cv::Mat RWC= Twc.rowRange(0,3).colRange(0,3);  
		cv::Mat tWC=  Twc.rowRange(0,3).col(3);

		Eigen::Matrix<double,3,3> eigMat ;
		eigMat <<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);
		Eigen::Quaterniond q(eigMat);

		geometry_msgs::PoseStamped tcw_msg; 					
		tcw_msg.pose.position.x=tWC.at<float>(0);
		tcw_msg.pose.position.y=tWC.at<float>(1);			 
		tcw_msg.pose.position.z=tWC.at<float>(2);

		tcw_msg.pose.orientation.x=q.x();
		tcw_msg.pose.orientation.y=q.y();
		tcw_msg.pose.orientation.z=q.z();
		tcw_msg.pose.orientation.w=q.w();			 

		std_msgs::Header header ;
		header.stamp =msgLeft->header.stamp;
		header.seq = msgLeft->header.seq;
		header.frame_id="world";

		sensor_msgs::Image::ConstPtr rgb_msg = msgLeft;
		sensor_msgs::Image::ConstPtr depth_msg=msgRight;

		tcw_msg.header=header;

		// odometry information
		nav_msgs::Odometry odom_msg;
		odom_msg.pose.pose.position.x=tWC.at<float>(0);
		odom_msg.pose.pose.position.y=tWC.at<float>(1);			 
		odom_msg.pose.pose.position.z=tWC.at<float>(2);

		odom_msg.pose.pose.orientation.x=q.x();
		odom_msg.pose.pose.orientation.y=q.y();
		odom_msg.pose.pose.orientation.z=q.z();
		odom_msg.pose.pose.orientation.w=q.w();

		odom_msg.header=header;
		odom_msg.child_frame_id="base_link"; 

		camerapath.header =header;
		camerapath.poses.push_back(tcw_msg);
		//Tcw位姿信息
		pub_tcw.publish(tcw_msg);	                
		pub_odom.publish(odom_msg);	  
		//相机轨迹
		pub_camerapath.publish(camerapath);  
		if( isKeyFrame)
		{
			pub_rgb.publish(rgb_msg);
			pub_depth.publish(depth_msg);	
		}
	}
	else
	{
	  cout<<"Twc is empty ..."<<endl;
	}

}

2、双目计算深度

在前面我们已经编写过相关的博客，主要是基于ELAS这个库实现的。这里我们直接引用过来。大家可以参照博客实现利用双目相机计算点云。

双目相机计算稠密深度点云（一）
双目相机计算稠密深度点云（二）

下图是ELAS在KITTI数据集上的一个计算效果

(ELAS在KITTI数据集上的运行效果)

elas

3、pointcloud_mapping 测试

3.1 KITTI数据集测试

启动ORB-SLAM 双目ROS节点

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/crp/crp/SLAM/ORB_SLAM2/Examples/ROS
rosrun ORB_SLAM2 Stereo Vocabulary/ORBvoc.txt Examples/Stereo/KITTI00-02.yaml false

启动ELAS

roslaunch elas_ros kitti_no_rviz.launch

启动 pointcloud_mapping 接受ELAS发布的点云

roslaunch pointcloud_mapping kitti.launch

播放KITTI数据集

roslaunch publish_image_datasets publish_kitti.launch 

3.2 ZED 相机测试

启动ORB-SLAM 双目ROS节点

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

启动ELAS

roslaunch elas_ros zed_no_rviz.launch

启动 pointcloud_mapping 接受ELAS发布的点云

roslaunch pointcloud_mapping zed.launch

启动ZED相机

roslaunch zed_cpu_ros zed_cpu_ros.launch

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

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

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