ORB-SLAM2系统的实时点云地图构建

这篇博客

 这篇博客介绍如何给ORB-SLAM2系统添加实时点云地图构建环节。这个项目高翔博士已经做过，在GitHub上也能找到相关代码。在参考这个项目之后，我尝试用自己的方式来实现地图构建环节，并添加了一些新的东西。下面来看一下修改后的整个环节的工作流程。

点云地图构建的流程

 构建点云地图需要用到彩色图，深度图和帧的位姿信息。为了防止构建的地图过大，降低内存负担，系统只使用关键帧来构建点云地图（也能减少点云地图中重叠的部分）。整个地图构建的流程如下：

 最后两步是自己做的修改部分，目的在于增强地图的可视效果，以及给后续导航操作提供便利。整个系统是在ROS下运行的，相关代码实现如下：

代码介绍

 在代码中，将地图构建环节定义为一个类对象，通过其中的成员函数来完成流程图中的操作。原来的项目是单独拿出一个线程来实现点云地图的更新和显示操作。但为了适应后面新加的部分操作，这里就不使用单独的线程来实现，而是在Tracking的线程中进行该类对象的函数调用。
 先来看看构建地图的类包含了哪些成员。

点云地图构建类对象

class MyPclMapping
{
public:
    typedef pcl::PointXYZRGBA PointT;
    typedef pcl::PointCloud<PointT> PointCloud;
    //类对象的构建函数
    MyPclMapping();
    //获取新关键帧的函数
    void insertKF(KeyFrame* kf,Mat &color,Mat &depth);
    //构建关键帧对应的点云地图，并将新旧地图叠加
    void generatepcl(KeyFrame *kf,Mat &color,Mat &depth);
    //返回以构建的完整点云地图。这个在进行八叉树转换时使用
    PointCloud::Ptr Getglobalmap()
    {
        unique_lock<mutex> locker(generation);
        return globalMap;
    }
    //设置当前相机在地图中的位置
    void setcurrentCamera(Mat Rwc,Mat twc);
private:
    //保存构建所使用的彩色图像，深度图和关键帧
    vector<Mat> rgbs;
    vector<Mat> depthes;
    vector<KeyFrame*> kfs;
    //构建的完整点云地图
    PointCloud::Ptr globalMap;
     Eigen::Vector3d kfplace;
     //使用PCLVisualizer对象来显示地图
     pcl::visualization::PCLVisualizer showcamera;
    Eigen::Vector3d currentcamera;
    cv::Mat currentTwc;
};

 在System类和Tracking类中定义上述类对象的指针，以便其获得关键帧和对应的图像信息（在初始化System和Tracking类对象时同时初始化地图构建的指针即可）。下面介绍如何一步步实现流程中的操作，首先是获得关键帧：

获取关键帧

 因为关键帧是由Tracking类中的CreateNewKeyFrame成员函数产生的，所以该函数的最后添加下述代码：
 pointmapping->insertKF(pKF,this->currentrgb,this->currentdepth);
 pointmapping是Tracking中定义的地图构建指针对象。关于它的insertKF函数的代码如下：

void MyPclMapping::insertKF(KeyFrame* kf,cv::Mat &color,cv::Mat &depth)
{
    //在更新已有地图时加锁，防止和Getglobalmap函数冲突
    unique_lock<mutex> locker(generation);
    cv::Mat rgb=color.clone();
    cv::Mat D=depth.clone();
    //构建关键帧对应的点云地图
    generatepcl(kf,rgb,D);
    //将这些信息保存下来
    rgbs.push_back(color.clone());
    depthes.push_back(depth.clone());
    kfs.push_back(kf);
}   

 至此第一步就完成了。需要注意的是代码中的 “currentdepth” 是原深度图经过一定变化后的Mat对象。变换的方法如下（代码添加在Tracking中的GrabImageRGBD函数后面）：

if(mDepthMapFactor!=1 || imDepth.type()!=CV_32F)//一定要注意调节这里！！！！
{
    imDepth.convertTo(imDepth,CV_32F,mDepthMapFactor);
}
currentdepth=imDepth;

 深度图要经过转换后（根据RGB-D相机的深度单位进行变换）再用于构建点云地图，否则构建出的地图会像千层饼一样（血的教训）。彩色图像使用原图即可。之后介绍点云地图的构建和叠加操作。
PS：这里的 “currentrgb” 和 “currentdepth” 是在Tracking类中新加的对象，专门用来构建点云地图的。

点云地图构建与叠加

 地图的构建和叠加通过generatepcl函数实现：

void MyPclMapping::generatepcl(KeyFrame* kf,cv::Mat &color,cv::Mat &depth)
{
     //用temp保存关键帧对应的点云地图
    PointCloud::Ptr temp(new PointCloud());
    //获得关键帧的估计位姿
    Eigen::Isometry3d T=ORB_SLAM2::Converter::toSE3Quat(kf->GetPoseInverse());
    //遍历彩色图像，构建点云地图（在关键帧的坐标系下）
    for(int v=0;v<color.rows;v+=3)//为了降低地图占用的内存大小，遍历的步长定为3
    {
        for(int u=0;u<color.cols;u+=3)
        {
            float d=depth.ptr<float>(v)[u];
            if(d<0) continue;
            PointT p;
            //计算地图点的坐标
            p.z=d;
            p.x=p.z*(u-kf->cx)/kf->fx;
            p.y=p.z*(v-kf->cy)/kf->fy;
            //给地图点上色
            p.b=color.data[v*color.step+u*color.channels()];
            p.g=color.data[v*color.step+u*color.channels()+1];
            p.r=color.data[v*color.step+u*color.channels()+2];
            temp->points.push_back(p);
        }   
    }
    PointCloud::Ptr cloud(new PointCloud());
    //将新构建的地图变换到世界坐标系下
    pcl::transformPointCloud(*temp,*cloud,T.matrix());
    cloud->is_dense=false;
    //完成新旧地图的叠加
    (*globalMap)+=(*cloud);
}

 此函数完成了点云地图的构建和更新。之后要做的是在地图中显示出相机的位置，以及完成地图到Octomap（八叉树地图）的转换。

在地图中设置当前相机位置

 原来的项目使用pcl::visualization::CloudViewer类来显示点云地图。这个类的功能较少，几乎就只有显示地图这个用途。因此，在这里用pcl::visualization::PCLVisualizer类来显示地图和相机的位置。下面是相关代码：

//参数：从相机坐标系到世界坐标系的旋转矩阵和平移向量
void MyPclMapping::setcurrentCamera(Mat Rwc,Mat twc)
{
    Mat Twc=Mat::eye(4,4,Rwc.type());
    //获得当前相机的位姿
    Rwc.copyTo(Twc(Rect(0,0,3,3)));
    twc.copyTo(Twc(Rect(3,0,1,3)));
    currentTwc=Twc.clone();
    g2o::SE3Quat q=ORB_SLAM2::Converter::toSE3Quat(currentTwc);
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();
    T.rotate(q.rotation());
    T.pretranslate(q.translation());
    Eigen::Vector3d currentpoint(0.1,0.1,0.1);
    //获得相机在世界坐标系下的坐标位置
    currentcamera=T*currentpoint;
    PointCloud::Ptr temp(Getglobalmap());//获得完整的点云地图
    //刷新PCLVisualizer对象中保存的内容
    showcamera.removeAllPointClouds();
    showcamera.removeAllShapes();
    showcamera.addPointCloud(temp);
    Eigen::Quaternionf qf(float(q.rotation().coeffs()[3]),
                        float(q.rotation().coeffs()[0]),float(q.rotation().coeffs()[1]),
                        float(q.rotation().coeffs()[2]));
    //用一个立方体来表示相机
    showcamera.addCube(Eigen::Vector3f(q.translation()[0],q.translation()[1],q.translation()[2])
                    ,qf,0.7,0.7,1.2);
    //显示一次当前地图中的内容
    showcamera.spinOnce();
}

 函数通过不断刷新PCLVisualizer类对象保存的内容，达到显示点云地图和实时相机位置的功能。相机的实时位姿要依据Tracking线程的跟踪结果，所以在Tracking类的GrabImageRGBD函数的最后加上这一行调用代码：
pointmapping->setcurrentCamera(mCurrentFrame.GetRotationInverse(),mCurrentFrame.GetCameraCenter());
 到此关于点云地图部分的操作就弄完了，之后就是如何将其转换成Octomap。

点云地图到Octomap的转换

 点云地图转换到Octomap的代码实现请参考这个博客。但这里是使用ROS中的octomap_server节点，将点云地图转成Octomap。这需要把已构建的点云地图发布到一个topic上。因此对原来系统的ros_rgbd.cpp文件进行修改，主要修改其中定义的ImageGrabber类对象：

class ImageGrabber
{
public:
    ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM)
    {
    	//修改：创建一个线程实时地向ROS发布已构建的点云地图
        pclpub=make_shared<thread>( bind(&ImageGrabber::pclpublish, this ) );
    }
    void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD);
    //修改：发布点云地图的函数
    void pclpublish();
    ORB_SLAM2::System* mpSLAM;
    //修改：地图发布线程对象
    shared_ptr<thread> pclpub;
};

 给ImageGrabber类对象添加了用于实时发布点云地图信息的函数和线程。发布地图信息的pclpublish函数代码如下：

void ImageGrabber::pclpublish()
{
        ros::NodeHandle nh;
        //将点云地图发布到 “pcltext” 话题上
        ros::Publisher pub=nh.advertise<sensor_msgs::PointCloud2>("/pcltext",1);
        string frameid="camera";
        sensor_msgs::PointCloud2 msg;
        while(1)
        {
        //获得已构建的点云地图
            PointCloud::Ptr pointmap((mpSLAM->Getpcl())->Getglobalmap());
            pcl::PointCloud<pcl::PointXYZ> pclmsg;
            //octomap_server转换需要的是没有颜色的点云地图
            //所以将以构建的彩色点云地图变成无色的
            for(int i=0;i<pointmap->points.size();i++)
            {
                pcl::PointXYZ point;
                point.x=pointmap->points[i].x;
                point.y=pointmap->points[i].y;
                point.z=pointmap->points[i].z;
                pclmsg.points.push_back(point);
            }
            //将点云地图转换成ROS中的信息数据形式
            pcl::toROSMsg(pclmsg,msg);
            msg.header.stamp=ros::Time::now();
            msg.header.frame_id=frameid;
            //发布地图信息
            pub.publish(msg);
            usleep(2000);
        }
}

 通过这个函数就将构建好的点云地图发布到了对应的ROS话题上。在这里使用了多线程的方法。
 之后用octomap_server接受 “pcltext” 话题的信息就能完成地图的转换。为了方便启动octomap_server节点，编写一个launch文件：

 在操作过程中，先启动带有点云地图发布的系统，再启动这个launch文件（要让 “pcltext” 话题上有点云地图的信息）。

地图效果

 上述就是整个地图构建环节的实现代码。在操作过程中，先启动带有点云地图发布的系统，再启动关于octomap_server的launch文件。最后通过Rviz显示构建的Octomap（点云地图已设置了显示窗口）。关于如何使用Rviz来显示八叉树地图大家可以参考这个博客。这个博客也介绍了一种构建地图的方法（）。
 最终整个系统在仿真环境（仿真环境在GitHub上开源，是其他前辈写的）中工作的效果图如下所示：

 左下角的是实时构建的点云地图。地图中使用一个立方体来表示相机当前的位置，放大来看就是：

 使用立方体的长轴表示相机的前后两个方向，短轴则是左右方向。这样的好处是可以只通过这个地图来控制机器在环境中移动，躲避障碍。这个图的上方就是转换后的Octomap。

结尾

 至此整个修改后的点云地图构建环节就介绍完了。之前想着再在点云地图中显示所有关键帧的位置，以表明相机的运动轨迹。但这样会消耗挺大内存，所以需要考虑其他方法。此外，这个系统还存在不足的地方：
 1、在检测到闭环以后，已构建的点云地图不会根据闭环进行地图的修复；
 2、立方体前后方向、左右方向区分度很差。
 看来还是任重而道远，慢慢来吧。

参考资料：
 1、https://blog.csdn.net/crp997576280/article/details/74605766
 2、https://blog.csdn.net/fb_941219/article/details/89002257

PS:转载请标明出处