这篇博客

（PS:修改于2020-9-21，添加了关于System和Tracking类中相关函数的修改(“小调整”中的内容)）
 这篇博客介绍如何给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类对象时同时初始化地图构建的指针即可）:
1、对System类中成员和函数的一些修改：

//加在System.h中，声明一些新成员和函数
MyPclMapping* pointcloud;
MyPclMapping* Getpcl()
{return pointcloud;
}

//加在System类构造函数体中，用于初始化MyPclMapping和Tracking的类对象
pointcloud=new MyPclMapping();
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,mpMap,pointcloud ,mpKeyFrameDatabase, strSettingsFile, mSensor);

2、对Tracking类中成员和函数的一些修改：

//在Tracking.h中添加MyPclMapping类对象指针，并调整Trakcing类的构造函数中的形参
Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,MyPclMapping* pCloud,KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor);
MyPclMapping* pointmapping;

//在Tracking类构造函数中使用成员初始化表初始化MyPclMapping类对象指针
Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap,MyPclMapping* pCloud, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor):mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc),mpKeyFrameDB(pKFDB), mpInitializer(static_cast<Initializer*>(NULL)), mpSystem(pSys), mpViewer(NULL),mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0),pointmapping(pCloud)

（这一部分是添加的内容）
下面介绍如何一步步实现流程中的操作，首先是获得关键帧：

获取关键帧

因为关键帧是由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><!-- 使用的功能包的名称；启动的节点；启动后节点的名字--><node pkg="octomap_server" type="octomap_server_node" name="octomap_server"><!--分辨率（米/像素） --><param name="resolution" value="0.05" /><!-- name of the fixed frame, needs to be "/map" for SLAM这里需要和发布的点云地图中的frameid一致 --><param name="frame_id" type="string" value="/camera" /><!-- max range / depth resolution of the kinect in meter超出100的读数，它的值用100代替 --><param name="sensor_model/max_range" value="100.0" /><param name="latch" value="true" /><!-- max/min height for occupancy map, should be in meters --><param name="pointcloud_max_z" value="1000" /><param name="pointcloud_min_z" value="0" /><!-- 让节点订阅点云地图发布的话题 --><remap from="/cloud_in" to="/pcltext" /></node>
</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:转载请标明出处