ORB_SLAM2新增稠密建图代码笔记

置顶好好仔仔

已于 2022-05-25 18:15:50 修改

阅读量3.1k

点赞数 12

文章标签：算法 c++ 自动驾驶学习

于 2022-05-25 18:15:09 首次发布

本文链接：https://blog.csdn.net/luoyihao123456/article/details/124963453

版权

接上一篇博客“ORB_SLAM2+kinect稠密建图实战项目总结”

ORB_SLAM2+kinect稠密建图实战项目总结_好好仔仔的博客-CSDN博客

本篇在此基础上对整个项目的逻辑结构以及代码实现进行梳理。

orb_slam2新增稠密建图功能的整体思路

1.生成关键帧在相机坐标系下的点云，再将此点云变换到世界坐标系保存为全局点云地图。因为涉及关键帧，故此部分与Tracking相关。 //跟踪线程

2.全局BA后用更新的位姿调整全局点云地图。此部分与LoopClosing相关。 //闭环线程

3.显示全局点云地图。创建一个pointcloudmapping类，在该类中实现点云地图显示。//显示线程

4.全局点云地图的关闭和保存。//主线程

基于上述思路，增加PointCloudMapping类。该类要具备以下功能：

初始化        PointCloudMapping

插入关键帧        insertKeyFrame

产生相机坐标系点云        generatePointCloud

BA更新点云        updatecloud

保存        save

显示        viewer（将相机坐标系下的点云转换到世界坐标系下并显示出来）

关闭        shutdown

增加PointCloude类，类里定义一些与点云相关的成员变量，记录对应帧的点云信息。

class PointCloude
{
    typedef pcl::PointXYZRGBA PointT;
    typedef pcl::PointCloud<PointT> PointCloud;
public:
    PointCloud::Ptr pcE;
public:
    Eigen::Isometry3d T;
    int pcID;  
};

因此在源文件中新增4个文件：PointCloude.h；PointCloude.cpp pointcloudmapping.h；pointcloudmapping.cc

0. 初始化参数配置

首先，在初始化配置中修改yaml文件，新增：

PointCloudMapping.Resolution: 0.01//点云密度
meank: 50//统计滤波中查询点邻居点的数量
thresh: 2.0//判断是否为离群点的阈值

System保存从yaml文件中读取到的点云滤波器的参数

// 滤波参数
float resolution = fsSettings["PointCloudMapping.Resolution"];
float meank = fsSettings["meank"];
float thresh = fsSettings["thresh"];

用读取到的参数对 PointCloudMapping进行初始化，构造函数如下

//对PointCloudMapping进行初始化
PointCloudMapping::PointCloudMapping(double resolution_,double meank_,double thresh_)
{
    this->resolution = resolution_;
    this->meank = meank_;
    this->thresh = thresh_;
    statistical_filter.setMeanK(meank);
    statistical_filter.setStddevMulThresh(thresh);
    voxel.setLeafSize( resolution, resolution, resolution);//设置体素滤波中每个体素的大小
    globalMap = boost::make_shared< PointCloud >( );//创建一个智能指针globalMap，记录全局地图

//开启一个名为 viewerThread的线程,完成初始化
    viewerThread = make_shared<thread>( bind(&PointCloudMapping::viewer, this ) );
}
/*
bind是一个函数适配器，接受一个可调用对象，生成一个新的可调用对象来适应原对象的参数列表
auto newCallable = bind(callable, arg_list);
该形式表达的意思是：当调用newCallable时，会调用callable，并传给它arg_list中的参数
https://blog.csdn.net/qq_38410730/article/details/103637778
*/

1. 跟踪线程修改

System控制整个ORB-SLAM2系统的运作，要进行点云建图pointcloudmapping初始化（地图显示）、跟踪、闭环。所以后面将以System为主线展开，依次说明各部分的修改内容。

在System中修改与跟踪Tracking有关的代码

// 将mpPointCloudMapping指针类放入跟踪线程
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
mpMap, mpPointCloudMapping, mpKeyFrameDatabase, strSettingsFile, mSensor);
/*Tracking* mpTracker;
mpTracker是一个指向 Tracking类的指针
通过new  Tracking（）创建一个实例
*/

智能指针相关知识：

由于这里用到了诸多make_shared，shared_ptr等，所以插播一下智能指针相关知识。

参考：https://cloud.tencent.com/developer/article/1432854

智能指针是为了避免因对动态内存操作不当产生野指针的问题。那为何要使用动态内存呢？

1、程序不知道自己需要多少对象；2、程序需要在多个对象间共享数据;3、程序不知道所需对象的准确类型；

智能指针是一个模板类，它可以通过指向某个对象，创建或销毁该对象的内存空间。

智能指针包含：shared_ptr、unique_ptr、weak_ptr

shared_ptr

多个指针指向相同的对象，通过引用计数确定当前内存是否要释放。

初始化：1 指定类型，传入指针通过构造函数初始化 2. 使用make_shared函数初始化

使用make_shared返回指向此对象的shared_ptr

注意：不能将指针直接赋值给智能指针，因为一个是类，一个是指针

例如std::shared_ptr<int> p4 = new int(1);的写法是错误的

注意：不要用一个原始指针初始化多个shared_ptr，会造成二次销毁。
    int *p5 = new int;
  std::shared_ptr<int> p6(p5);
  std::shared_ptr<int> p7(p5);// logical error p6释放完内存后p7再去释放，会造成二次销毁
unique_ptr

同一时刻只能有一个unique_ptr指向给定对象（禁止拷贝）

从unique_ptr指针创建时开始，直到离开作用域。离开作用域时，若其指向对象，则将其所指对象销毁

可以改变智能指针所指对象，如创建智能指针时通过构造函数指定、通过reset方法重新指定、通过release方法释放所有权、通过移动语义转移所有权。

unique_ptr指向某个对象时，不允许其他智能指针再去指向该对象，除非其主动释放所有权。

weak_ptr

配合shared_ptr使用，协助shared_ptr工作，像旁观者那样观测资源的使用情况

成员函数expired()等价use_count()==0，表示没有可用资源了

如果expired()返回false，调用lock()可以获得一个可用的shared_ptr对象。

多个指针指向相同的对象，通过引用计数确定当前内存是否要释放。

对应于System中调用的跟踪线程，在Tracking中进行相应的修改

// 将关键帧插入到点云地图中
Tracking::CreateNewKeyFrame()

// 取出所有的关键帧
vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();

//使用PointCloudMapping::insertKeyFrame函数，将新关键帧转换为点云插入到全局点云里
mpPointCloudMapping->insertKeyFrame( pKF, this->mImRGB, this->mImDepth, idk, vpKFs);

Tracking中调用了PointCloudMapping::insertKeyFrame函数，故在PointCloudMapping中

void PointCloudMapping::insertKeyFrame(KeyFrame* kf, cv::Mat& color, cv::Mat& depth,int idk,vector<KeyFrame*> vpKFs)// kf 是新的关键帧，用color引用传入的mImRGB，idk是关键帧的序号， vpKFs是所有指向关键帧的指针的vector
{
    cout<<"receive a keyframe, id = "<<idk<<" 第"<<kf->mnId<<"个"<<endl;
// unique_lock尝试加锁，如果没有锁定成功，会立即返回
    unique_lock<mutex> lock(keyframeMutex);

//将新关键帧保存到名为keyframes的vector中
    keyframes.push_back( kf );

//用 currentvpKFs保存地图中现有的所有关键帧
    currentvpKFs = vpKFs;

//创建一个 PointCloude实例，目的是获取本帧图像关联的点云id, 相对于世界坐标系的变换Twc
    PointCloude pointcloude;

//记录关键帧索引
    pointcloude.pcID = idk;

// kf->GetPose() 返回Tcw，并将关键帧位姿Tcw转化为符合PCL格式
    pointcloude.T = ORB_SLAM2::Converter::toSE3Quat( kf->GetPose() );

//联合深度图和RGB图像,找到对应点的空间位置,并且构造空间点云.
    pointcloude.pcE = generatePointCloud(kf,color,depth);

//存储所有关键帧的点云、位姿、id信息到PointCloudMapping类中的成员变量pointcloud
    pointcloud.push_back(pointcloude);

//点云更新通知
    keyFrameUpdated.notify_one();
}

插播：

keyFrameUpdated是一个条件变量condition_variable

当 std::condition_variable 对象的 wait 函数被调用的时候，它使用 std::unique_lock(通过 std::mutex) 来锁住当前线程，直到另外一个线程在相同的 std::condition_variable 对象上调用了 notification 函数来唤醒当前线程。

对于generatePointCloud函数，在PointCloudMapping中实现如下

// Ptr是一个指向 PointCloud类的指针，PointCloud里面装的数据类型是 PointT
pcl::PointCloud< PointCloudMapping::PointT >::Ptr PointCloudMapping::generatePointCloud(KeyFrame* kf, cv::Mat& color, cv::Mat& depth)
{
// 创建一个名为tmp指向空 PointCloud 的智能指针
    PointCloud::Ptr tmp( new PointCloud() );
    
    for ( int m=0; m<depth.rows; m+=3 )//循环遍历新的关键帧的深度值的行
    {
        for ( int n=0; n<depth.cols; n+=3 )//循环遍历新的关键帧的深度值的列
        {
            float d = depth.ptr<float>(m)[n];//depth是一个opencv里的mat类型，使用指针进行遍历
            if (d < 0.01 || d>5)
                continue;//排除掉深度值过大或过小的点
            PointT p;
            p.z = d;
            p.x = ( n - kf->cx) * p.z / kf->fx;//cx，fx，fy是相机内参
            p.y = ( m - kf->cy) * p.z / kf->fy;//相当于把像素坐标转化为相机坐标
            
            p.b = color.ptr<uchar>(m)[n*3];//保存每个点的rgb值
            p.g = color.ptr<uchar>(m)[n*3+1];
            p.r = color.ptr<uchar>(m)[n*3+2];
                
            tmp->points.push_back(p);//将每个点保存到 PointCloud中的 points里面
        }
    }

2. 闭环线程修改

在System中，将mpPointCloudMapping指针类放入闭环线程

// 将mpPointCloudMapping指针类放入闭环线程
mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary,
mSensor!=MONOCULAR,mpPointCloudMapping);
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);

对应于LoopClosing，进行如下初始化

LoopClosing::LoopClosing(Map *pMap, KeyFrameDatabase *pDB, ORBVocabulary *pVoc, const bool bFixScale,shared_ptr<PointCloudMapping> pPointCloud):
    mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap),
    mpPointCloudMapping( pPointCloud ),
    mpKeyFrameDB(pDB), mpORBVocabulary(pVoc), mpMatchedKF(NULL), mLastLoopKFid(0), mbRunningGBA(false), mbFinishedGBA(true),
    mbStopGBA(false), mpThreadGBA(NULL), mbFixScale(bFixScale), mnFullBAIdx(0)

在 LoopClosing中RunGlobalBundleAdjustment函数里，添加如下

//开始全局BA时，停止点云更新   
 cout << "Starting Global Bundle Adjustment" << endl;
 mpPointCloudMapping->loopbusy = true;

//全局BA之后,继续更新点云信息
loopcount++;
while(loopcount!=mpPointCloudMapping->loopcount)
    mpPointCloudMapping->updatecloud();
cout<<"mpPointCloudMapping->loopcount="<<mpPointCloudMapping->loopcount<<endl;
cout << "Map updated!" << endl;

对应于RunGlobalBundleAdjustment调用的updatecloud，在PointCloudMapping中添加updatecloud函数的实现

更新点云信息
void PointCloudMapping::updatecloud()
{
	if(!cloudbusy)//如果点云不忙
	{
		loopbusy = true;//将回环设置为忙
		cout<<"startloopmappoint"<<endl;

//提取每一个关键帧,提取每一帧上面的点云,用BA后的位姿转换到世界坐标系下
//新建临时点云tmp1，tmp1存储所有的变换后的点云
        PointCloud::Ptr tmp1(new PointCloud);
		for (int i=0;i<currentvpKFs.size();i++)//遍历所有关键帧
		{
		    for (int j=0;j<pointcloud.size();j++)//遍历每一个点云，pointcloud存储的是每个关键帧的点云、位姿、id信息
		    {   
				if(pointcloud[j].pcID==currentvpKFs[i]->mnFrameId) //找到具有相同id的关键帧对应的点云，用全局BA后的位姿来替换旧的位姿，计算新的点云
				{   
					Eigen::Isometry3d T = ORB_SLAM2::Converter::toSE3Quat(currentvpKFs[i]->GetPose() );
					PointCloud::Ptr cloud(new PointCloud);//创建一个临时的cloud指针，用来存放变换后的世界坐标系
					pcl::transformPointCloud( *pointcloud[j].pcE, *cloud, T.inverse().matrix());
//将点云*pcE转换到世界坐标系下*cloud
//pcl::transformPointCloud(*source_cloud, *target_cloud, transform)
					*tmp1 +=*cloud;
					continue;
				}
			}
		}
        cout<<"finishloopmap"<<endl;
        PointCloud::Ptr tmp2(new PointCloud());//tmp2存储滤波后的点云
//点云比较稠密,体素滤波来降采样
        voxel.setInputCloud( tmp1 );
        voxel.filter( *tmp2 );
        globalMap->swap( *tmp2 );//交换内容
        viewer.showCloud( globalMap );
        loopbusy = false;//释放闭环
        loopcount++;//更新完之后,闭环次数加一
	}
}

3. 显示线程的修改

在System中，初始化PointCloudMapping实例,并用mpPointCloudMapping指向它，初始化时就产生了viewerThread线程，用于地图的显示

// 初始化PointCloudMapping实例,并用mpPointCloudMapping指向它
mpPointCloudMapping = make_shared<PointCloudMapping>( resolution,meank,thresh );
/*
shared_ptr<PointCloudMapping> mpPointCloudMapping;
mpPointCloudMapping是一个智能指针，它指向了一个PointCloudMapping实例，该实例通过make_shared<PointCloudMapping>( resolution,meank,thresh )完成初始化。
*/

PointCloudMapping初始化时就开启viewerThread线程

//开启一个名为 viewerThread的线程,完成初始化
    viewerThread = make_shared<thread>( bind(&PointCloudMapping::viewer, this ) );
}

PointCloudMapping中viewer的实现如下

void PointCloudMapping::viewer()
{
    pcl::visualization::CloudViewer viewer("viewer");//调用pcl下命名空间 visualization下的 CloudViewer类，创建 viewer对象
//点云视窗类CloudViewer是简单的可视化点云工具类，但不能用于多线程程序中。
    while(1)
    {      
        {
            unique_lock<mutex> lck_shutdown( shutDownMutex );
            if (shutDownFlag)
            {
                break;
            }
        }
        {
            unique_lock<mutex> lck_keyframeUpdated( keyFrameUpdateMutex );
            keyFrameUpdated.wait( lck_keyframeUpdated );//等待关键帧更新，更新完后释放锁。注意 keyFrameUpdated是一个条件变量
        }
        size_t N=0;
        {
            unique_lock<mutex> lck( keyframeMutex );
            N = keyframes.size();
        }
        if(loopbusy || bStop)//跳过闭环忙和停止命令
        {
          //cout<<"loopbusy || bStop"<<endl;
            continue;
        }
        //cout<<lastKeyframeSize<<"    "<<N<<endl;
        if(lastKeyframeSize == N)//如果已经处理过的点云最后一帧的序号等于所有帧中最后一帧的序号，说明当前点云已经全部处理完了
            cloudbusy = false;
        //cout<<"待处理点云个数 = "<<N<<endl;//感觉这里可以continue，不会执行for循环了
          cloudbusy = true;
        for ( size_t i=lastKeyframeSize; i<N ; i++ )
        {
//遍历关键帧序列的每一帧,提取出每一帧中的点云,并且变换到世界坐标系下,最后加入到全局地图中;
          
            PointCloud::Ptr p (new PointCloud);

将每一个关键帧中的点云通过刚体变换变换到世界坐标系下;
            pcl::transformPointCloud( *(pointcloud[i].pcE), *p, pointcloud[i].T.inverse().matrix());
            *globalMap += *p;
        }
      
        PointCloud::Ptr tmp1 ( new PointCloud );
        //全局点云作为输入,通过统计滤波器
        statistical_filter.setInputCloud(globalMap);
        statistical_filter.filter( *tmp1 );//tmp1用来保存滤波后的全局点云
统计滤波的输出作为体素滤波器的输入,通过滤波的点输出到全局地图中
        PointCloud::Ptr tmp(new PointCloud());
        voxel.setInputCloud( tmp1 );
        voxel.filter( *globalMap );
        viewer.showCloud( globalMap );
        cout<<"show global map, size="<<N<<"   "<<globalMap->points.size()<<endl;
        lastKeyframeSize = N;
        cloudbusy = false;
    }
}

4. 显示线程的保存与关闭

System中的save

void System::save()
{
    mpPointCloudMapping->save();
}

PointCloudMapping中的save

void PointCloudMapping::save()
{
	pcl::io::savePCDFile( "result.pcd", *globalMap );
	cout<<"globalMap save finished"<<endl;
}

System中Shutdown

//关闭点云建图
mpPointCloudMapping->shutdown();

PointCloudMapping中的Shutdown

void PointCloudMapping::shutdown()
{
    {
        unique_lock<mutex> lck(shutDownMutex);//有一个互斥对象，给这个对象加把锁
        shutDownFlag = true;
        keyFrameUpdated.notify_one();//keyFrameUpdated是condition_variable 条件变量对象，当调用 wait 函数时，使用 std::unique_lock(mutex) 锁住当前线程。当调用notify_one时，线程被唤醒。
    }
    viewerThread->join();//调用join()，等viewerThread线程执行完后才返回，继续执行后面的程序。
}
// viewerThread是一个shared_ptr<thread>指针

以上便是ORB-SLAM2新增稠密建图功能模块

由于水平有限，望各位大佬批评指正。