ORB-SLAM2实时显示稠密点云图

先上代码

自己的代码：
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<>()。

ROS在线生成稠密点云

代码

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/types.h> 

#include<ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include<opencv2/core/core.hpp>

#include"../../../include/System.h"

using namespace std;

bool createDirs(const std::string& dirName);
void saveImg(const cv::Mat& img, const string& base_path, const string& type, double timestamp);

std::vector<std::string> vrgb;
std::vector<std::string> vdepth;

class ImageGrabber
{
public:
    ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){}

    void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD, const string& base_path);

    ORB_SLAM2::System* mpSLAM;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "RGBD");
    ros::start();

    if(argc != 4)
    {
        cerr << endl << "Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings <path_to_saveImages or null>" << endl;        
        ros::shutdown();
        return 1;
    }    

    string base_path = argv[3]; // null 表示不保存图像
    if (base_path != "null") {
        if (base_path[base_path.size()-1] != '/') {
            base_path.append("/");
        }
        if (!createDirs(base_path + "rgb/") || !createDirs(base_path + "depth/")) {
            std::cout << "Fail to create directories." << std::endl;
            return -1;
        }
    }

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);

    ImageGrabber igb(&SLAM);

    ros::NodeHandle nh;

    sleep(5);  // 等待5s开始保存图像
    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/color/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/aligned_depth_to_color/image_raw", 1);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub,depth_sub);
    sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2,base_path));

    ros::spin();

    // Stop all threads
    SLAM.Shutdown();

    // Save camera trajectory
    SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");


    if (base_path!="null") {
        std::ofstream outfile; 
        outfile.open(base_path+"rgb.txt", std::ios::trunc); // ios::trunc表示在打开文件前将文件清空，由于是写入，文件不存在则创建
        if (!outfile.is_open()) {
            std::cout << "Open rgb.txt failed!" << std::endl;
            return -1;
        }
        outfile << "# color images\n" << "# timestamp filename\n";
        for (std::string line:vrgb) {
            outfile << line << "\n";
        }
        outfile.close();


        outfile.open(base_path+"depth.txt", std::ios::trunc);
        if (!outfile.is_open()) {
            std::cout << "Open depth.txt failed!" << std::endl;
            return -1;
        }
        outfile << "# depth images\n" << "# timestamp filename\n";
        for (std::string line:vdepth) {
            outfile << line << "\n";
        }
        outfile.close();
    }


    ros::shutdown();

    return 0;
}

void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD, const string& base_path)
{
    // 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;
    }

    if (base_path != "null") {
        saveImg(cv_ptrRGB->image, base_path, "rgb", cv_ptrRGB->header.stamp.toSec());
        saveImg(cv_ptrD->image, base_path, "depth", cv_ptrD->header.stamp.toSec());
    }

    cv::Mat imRGB;
    cv::cvtColor(cv_ptrRGB->image, imRGB, cv::COLOR_BGR2RGB);
    mpSLAM->TrackRGBD(imRGB,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());
}

void saveImg(const cv::Mat& img, const string& base_path, const string& type, double timestamp)
{
    std::string path = base_path + type + "/" + std::to_string(timestamp) + ".png";
    std::cout << path << std::endl;
    if (type == "rgb") {
        cv::Mat image;
        cv::cvtColor(img, image, cv::COLOR_BGR2RGB);
        cv::imwrite(path, image);
    }else {
        cv::imwrite(path, img);
    }

    std::string line = std::to_string(timestamp) + " " + type + "/" + std::to_string(timestamp) + ".png";
    if (type == "rgb") {
        vrgb.push_back(line);
    }else {
        vdepth.push_back(line);
    }
}

// Linux下递归创建目录
bool createDirs(const std::string& dirName)
{
    uint32_t beginCmpPath = 0;
    uint32_t endCmpPath = 0;

    std::string fullPath = "";

    // LOGD("path = %s\n", dirName.c_str());
    
    if ('/' != dirName[0]) { //Relative path  
        fullPath = getcwd(nullptr, 0); //get current path
        beginCmpPath = fullPath.size();
        // LOGD("current Path: %s\n", fullPath.c_str());
        fullPath = fullPath + "/" + dirName;      
    }else { //Absolute path
        fullPath = dirName;
        beginCmpPath = 1;
    }

    if (fullPath[fullPath.size() - 1] != '/') {
        fullPath += "/";
    }  

    endCmpPath = fullPath.size();

    
    // create dirs;
    for (uint32_t i = beginCmpPath; i < endCmpPath ; i++ ) {
        if ('/' == fullPath[i]) {
        std::string curPath = fullPath.substr(0, i);
        if (access(curPath.c_str(), F_OK) != 0) {
            // if (mkdir(curPath.c_str(), S_IRUSR|S_IRGRP|S_IROTH|S_IWUSR|S_IWGRP|S_IWOTH) == -1) {
            if (mkdir(curPath.c_str(), S_IRWXU|S_IRWXG|S_IRWXO) == -1) {
            // LOGD("mkdir(%s) failed(%s)\n", curPath.c_str(), strerror(errno));
            return false;
            }
        }
        }
    }
    
    return true;
}

运行

• 根据实际情况修改发布的话题/camera/color/image_raw和/camera/aligned_depth_to_color/image_raw。

message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/color/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/aligned_depth_to_color/image_raw", 1);

• 最后一个参数为null表示不保存图像；将null改成对应路径，表示在该路径下以TUM数据集的形式保存rgb和深度图像。

ROS运行：
打开一个终端，启动相机
roslaunch realsense2_camera rs_rgbd.launch
打开另一个终端，进入到工作空间的目录下后
source ./Examples/ROS/ORB_SLAM2/build/devel/setup.bash
rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt config/Intel_D435i.yaml null

进一步工作

动态SLAM相关

Github：ORBSLAM2_Dynamic
博客：动态环境下的ORB-SLAM2_实现鲁棒的定位

语义SLAM相关

语义分割python实现，作为服务端；语义建图c++实现，作为服务端。
Github：Semantic_Mapping_on_ORBSLAM2
博客：基于ORB-SLAM2的语义地图构建，分成服务端和客户端

较早实现：C++调用python进行语义分割，设置了不少绝对路径，编译存在问题较多。
Github：ORBSLAM2_Semantic_Mapping
博客：基于ORB-SLAM2的语义地图构建