先上代码
自己的代码：
ShenXinda/ORBSLAM2_Dense
代码参考：
gaoxiang12/ORBSLAM2_with_pointcloud_map
熊猫飞天 / ORB_SLAM2_PointCloud
原理很简单：增加了一个PointcloudMapping类，在类的构造函数里开一个显示稠密点云的线程；在运行ORBSLAM2的时候，将Tracking生成的KeyFrame以及彩色和深度图像插入队列中，显示稠密点云的线程从队列中获取图像进行点云的生成。主要用到的是pcl库，进行点云的生成、坐标变换、滤波和显示。
下面给出源文件：
// PointcloudMapping.h
#include <mutex>
#include <condition_variable>
#include <thread>
#include <queue>
#include <boost/make_shared.hpp>
#include <opencv2/opencv.hpp>
#include <Eigen/Core> // Eigen核心部分
#include <Eigen/Geometry> // 提供了各种旋转和平移的表示
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/transforms.h>
#include <pcl/common/projection_matrix.h>
#include "KeyFrame.h"
#include "Converter.h"
#ifndef POINTCLOUDMAPPING_H
#define POINTCLOUDMAPPING_H
typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef pcl::PointXYZRGBA PointT; // A point structure representing Euclidean xyz coordinates, and the RGB color.
typedef pcl::PointCloud<PointT> PointCloud;
namespace ORB_SLAM2 {
class Converter;
class KeyFrame;
class PointCloudMapping {
public:
PointCloudMapping(double resolution=0.01);
~PointCloudMapping();
void insertKeyFrame(KeyFrame* kf, const cv::Mat& color, const cv::Mat& depth); // 传入的深度图像的深度值单位已经是m
void requestFinish();
bool isFinished();
void getGlobalCloudMap(PointCloud::Ptr &outputMap);
private:
void showPointCloud();
void generatePointCloud(const cv::Mat& imRGB, const cv::Mat& imD, const cv::Mat& pose, int nId);
double mCx, mCy, mFx, mFy, mResolution;
std::shared_ptr<std::thread> viewerThread;
std::mutex mKeyFrameMtx;
std::condition_variable mKeyFrameUpdatedCond;
std::queue<KeyFrame*> mvKeyFrames;
std::queue<cv::Mat> mvColorImgs, mvDepthImgs;
bool mbShutdown;
bool mbFinish;
std::mutex mPointCloudMtx;
PointCloud::Ptr mPointCloud;
// filter
pcl::VoxelGrid<PointT> voxel;
pcl::StatisticalOutlierRemoval<PointT> statistical_filter;
};
}
#endif
// PointcloudMapping.cc
#include "PointcloudMapping.h"
namespace ORB_SLAM2 {
PointCloudMapping::PointCloudMapping(double resolution)
{
mResolution = resolution;
mCx = 0;
mCy = 0;
mFx = 0;
mFy = 0;
mbShutdown = false;
mbFinish = false;
voxel.setLeafSize( resolution, resolution, resolution);
statistical_filter.setMeanK(50);
statistical_filter.setStddevMulThresh(1.0); // The distance threshold will be equal to: mean + stddev_mult * stddev
mPointCloud = boost::make_shared<PointCloud>(); // 用boost::make_shared<>
viewerThread = std::make_shared<std::thread>(&PointCloudMapping::showPointCloud, this); // make_unique是c++14的
}
PointCloudMapping::~PointCloudMapping()
{
viewerThread->join();
}
void PointCloudMapping::requestFinish()
{
{
unique_lock<mutex> locker(mKeyFrameMtx);
mbShutdown = true;
}
mKeyFrameUpdatedCond.notify_one();
}
bool PointCloudMapping::isFinished()
{
return mbFinish;
}
void PointCloudMapping::insertKeyFrame(KeyFrame* kf, const cv::Mat& color, const cv::Mat& depth)
{
unique_lock<mutex> locker(mKeyFrameMtx);
mvKeyFrames.push(kf);
mvColorImgs.push( color.clone() ); // clone()函数进行Mat类型的深拷贝，为什幺深拷贝？？
mvDepthImgs.push( depth.clone() );
mKeyFrameUpdatedCond.notify_one();
cout << "receive a keyframe, id = " << kf->mnId << endl;
}
void PointCloudMapping::showPointCloud()
{
pcl::visualization::CloudViewer viewer("Dense pointcloud viewer");
while(true) {
KeyFrame* kf;
cv::Mat colorImg, depthImg;
{
std::unique_lock<std::mutex> locker(mKeyFrameMtx);
while(mvKeyFrames.empty() && !mbShutdown){
// !mbShutdown为了防止所有关键帧映射点云完成后进入无限等待
mKeyFrameUpdatedCond.wait(locker);
}
if (!(mvDepthImgs.size() == mvColorImgs.size() && mvKeyFrames.size() == mvColorImgs.size())) {
std::cout << "这是不应该出现的情况！" << std::endl;
continue;
}
if (mbShutdown && mvColorImgs.empty() && mvDepthImgs.empty() && mvKeyFrames.empty()) {
break;
}
kf = mvKeyFrames.front();
colorImg = mvColorImgs.front();
depthImg = mvDepthImgs.front();
mvKeyFrames.pop();
mvColorImgs.pop();
mvDepthImgs.pop();
}
if (mCx==0 || mCy==0 || mFx==0 || mFy==0) {
mCx = kf->cx;
mCy = kf->cy;
mFx = kf->fx;
mFy = kf->fy;
}
{
std::unique_lock<std::mutex> locker(mPointCloudMtx);
generatePointCloud(colorImg, depthImg, kf->GetPose(), kf->mnId);
viewer.showCloud(mPointCloud);
}
std::cout << "show point cloud, size=" << mPointCloud->points.size() << std::endl;
}
// 存储点云
string save_path = "/home/xshen/pcd_files/resultPointCloudFile.pcd";
pcl::io::savePCDFile(save_path, *mPointCloud);
cout << "save pcd files to : " << save_path << endl;
mbFinish = true;
}
void PointCloudMapping::generatePointCloud(const cv::Mat& imRGB, const cv::Mat& imD, const cv::Mat& pose, int nId)
{
std::cout << "Converting image: " << nId;
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
PointCloud::Ptr current(new PointCloud);
for(size_t v = 0; v < imRGB.rows ; v+=3){
for(size_t u = 0; u < imRGB.cols ; u+=3){
float d = imD.ptr<float>(v)[u];
if(d <0.01 || d>10){
// 深度值为0 表示测量失败
continue;
}
PointT p;
p.z = d;
p.x = ( u - mCx) * p.z / mFx;
p.y = ( v - mCy) * p.z / mFy;
p.b = imRGB.ptr<uchar>(v)[u*3];
p.g = imRGB.ptr<uchar>(v)[u*3+1];
p.r = imRGB.ptr<uchar>(v)[u*3+2];
current->points.push_back(p);
}
}
Eigen::Isometry3d T = Converter::toSE3Quat( pose );
PointCloud::Ptr tmp(new PointCloud);
// tmp为转换到世界坐标系下的点云
pcl::transformPointCloud(*current, *tmp, T.inverse().matrix());
// depth filter and statistical removal，离群点剔除
statistical_filter.setInputCloud(tmp);
statistical_filter.filter(*current);
(*mPointCloud) += *current;
pcl::transformPointCloud(*mPointCloud, *tmp, T.inverse().matrix());
// 加入新的点云后，对整个点云进行体素滤波
voxel.setInputCloud(mPointCloud);
voxel.filter(*tmp);
mPointCloud->swap(*tmp);
mPointCloud->is_dense = true;
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
double t = std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
std::cout << ", Cost = " << t << std::endl;
}
void PointCloudMapping::getGlobalCloudMap(PointCloud::Ptr &outputMap)
{
std::unique_lock<std::mutex> locker(mPointCloudMtx);
outputMap = mPointCloud;
}
}
生成过程
系统入口
system.h和system.cc：
增加头文件
#include "pointcloudmapping.h"
在ORB__SLAM2的命名空间里加入一行声明
class PointCloudMapping;
缺少这一行代码，会出现PointCloudMapping类未定义错误？ -> 关于在头文件很多的项目中，明明包含了某些头文件，却报错类未定义
PS：上面问题是由于头文件循环引用造成的，解决办法要么是整理头文件顺序，要么就是加一行声明。
增加private成员
shared_ptr<PointCloudMapping> mpPointCloudMapping;
创建PointCloudMapping对象，用共享指针make_shared保存，并在初始化Tracking线程的时候传入
mpPointCloudMapping = make_shared<PointCloudMapping>( 0.01 );
//Initialize the Tracking thread
//(it will live in the main thread of execution, the one that called this constructor)
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor, mpPointCloudMapping);
Shutdown函数加入mpPointCloudMapping->requestFinish();，接下去的while循环中对!mpPointCloudMapping->isFinished()进行判断，如果稠密点云图的创建还未结束，则暂时还不能结束系统。
void System::Shutdown()
{
mpLocalMapper->RequestFinish();
mpLoopCloser->RequestFinish();
if(mpViewer)
{
mpViewer->RequestFinish();
while(!mpViewer->isFinished())
usleep(5000);
}
mpPointCloudMapping->requestFinish();
// Wait until all thread have effectively stopped
while(!mpPointCloudMapping->isFinished() || !mpLocalMapper->isFinished() || !mpLoopCloser->isFinished() || mpLoopCloser->isRunningGBA())
{
usleep(5000);
}
if(mpViewer)
pangolin::BindToContext("ORB-SLAM2: Map Viewer");
}
增加获取稠密点云地图的方法，调用的是tracking类的getPointCloudMap()方法，并由outputMap保存（注意传入的是引用）。
void System::getPointCloudMap(pcl::PointCloud<pcl::PointXYZRGBA> ::Ptr &outputMap)
{
mpTracker->getPointCloudMap(outputMap);
}
跟踪线程
Tracking.h和Tracking.cc：
增加pcl相关头文件
#include "PointcloudMapping.h"
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <condition_variable>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
添加protected成员变量
cv::Mat mImRGB;
cv::Mat mImDepth;
shared_ptr<PointCloudMapping> mpPointCloudMapping;
构造函数中增加参数shared_ptr<PointCloudMapping> pPointCloud，并在初始化列表中增加构建稠密点云地图的对象指针的初始化mpPointCloudMapping(pPointCloud)。
在Tracking::GrabImageRGBD()中保存RGB和Depth图像
mImRGB=imRGB;
mImDepth=imDepth;
在Tracking::CreateNewKeyFrame() 中将关键帧插入到点云地图
mpPointCloudMapping->insertKeyFrame( pKF, this->mImRGB, this->mImDepth );
增加获取稠密点云地图的方法，调用的是PointCloudMapping类的getGlobalCloudMap()方法。
void Tracking::getPointCloudMap(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &outputMap)
{
mpPointCloudMapping->getGlobalCloudMap(outputMap);
}
遇到问题
程序运行段错误，异常终止
定位发现是pcl库在进行滤波的代码有问题。statistical_filter.filter(*current); 和voxel.filter(*tmp);中current和tmp用于接收滤波后的输出点云，如果定义空的点云current和tmp进行接收会发生段错误，需要定义和输入点云一样大小的点云进行接收（具体原因不知道）。
Eigen::Isometry3d T = Converter::toSE3Quat( pose );
PointCloud::Ptr tmp(new PointCloud);
// tmp为转换到世界坐标系下的点云
pcl::transformPointCloud(*current, *tmp, T.inverse().matrix());
// depth filter and statistical removal，离群点剔除
statistical_filter.setInputCloud(tmp);
statistical_filter.filter(*current);
(*mPointCloud) += *current;
//定义和输入点云mPointCloud一样大小的点云tmp进行接收
pcl::transformPointCloud(*mPointCloud, *tmp, T.inverse().matrix());
// 加入新的点云后，对整个点云进行体素滤波
voxel.setInputCloud(mPointCloud);
voxel.filter(*tmp);
mPointCloud->swap(*tmp);
定义了Eigen类型成员的字节对齐问题
Eigen字节对齐问题
解决方法：在public下写一个宏EIGEN_MAKE_ALIGNED_OPERATOR_NEW。（自己尝试并没有解决，所以在类中的成员尽量定义成cv::Mat类型，而不是Eigen::xx类型）
其它
问题：PCL错误提示： what()::[pcl::PCDWriter::writeASCII] Could not open file for writing!
原因：pcd文件生成的路径不存在，在下图所示处修改（pointcloudmapping.cc文件中）。
问题：编译的时候卡死，鼠标动不了，或者一卡一卡的。
解决：发现时系统交换空间已经满了，即瞬时的内存不够用了（make -jxx时，开的线程数目可以少一点）。
问题：
解决：pcl::PointCloud<pcl::PointXYZRGBA>需要用boost::make_shared<>()。