4.ORB-SLAM3中如何实现稠密建图（二）：稠密建图如何控制三大线程与稠密建图代码解析
书接上回，我们完成了进入追踪线程前的讲述
目录
1 稠密建图在追踪线程中的改动
2 稠密建图在局部建图线程中的改动
3 稠密建图在闭环检测线程中的改动
3.1 修正闭环进行本质图优化后
3.2 纯视觉地图融合后LoopClosing::MergeLocal
3.3 进行全局BA之后LoopClosing::RunGlobalBundleAdjustment
4 稠密建图的一些API
4.0 构造函数
4.1 PointCloudMapping::insertKeyFrame 插入关键帧
4.2 稠密建图的显示线程 PointCloudMapping::viewer
4.3 地图更新 PointCloudMapping::updatecloud
4.4 由关键帧和深度图生成稠密点云 PointCloudMapping::generatePointCloud
1 稠密建图在追踪线程中的改动
我们回顾一下如何进入追踪线程的：
首先，ROS接收RGB图像和深度图像信息并做一个软同步将其送进了GrabRGBD函数内：
第二，在此函数中，将ROS格式的图像转换成cv::Mat的格式通过这个函数进入追踪线程：
现在我们来看在追踪线程中是如何进行稠密建图的：
追踪线程中只需改动一处，就是我们进行稠密重建仅对关键帧进行重建～，对于非关键帧不进行稠密重建，因此我们只需改动CreateNewKeyFrame函数。
如果确定加入了关键帧的话，把这帧图像的RGB信息和深度图像信息给予稠密建图线程：4.1节有该函数的详细介绍
if(mpSystem->densemapping) mpPointCloudMapping->insertKeyFrame( pKF, this->mImRGB, this->mImDepth,vpKFs);
2 稠密建图在局部建图线程中的改动
这里改动只有一个，就是新筛选的关键帧不仅要送入闭环检测线程，还要送入稠密建图队列中。
// Step 9 如果距离IMU第一阶段初始化成功累计时间差小于100s，进行VIBA if ((mTinit<50.0f) && mbInertial) { // Step 9.1 根据条件判断是否进行VIBA1（IMU第二次初始化） // 条件：1、当前关键帧所在的地图还未完成IMU初始化---并且--------2、正常跟踪状态---------- if(mpCurrentKeyFrame->GetMap()->isImuInitialized() && mpTracker->mState==Tracking::OK) // Enter here everytime local-mapping is called { // 当前关键帧所在的地图还未完成VIBA 1 if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA1()){ // 如果累计时间差大于5s，开始VIBA1（IMU第二阶段初始化） if (mTinit>5.0f) { cout << "start VIBA 1" << endl; mpCurrentKeyFrame->GetMap()->SetIniertialBA1(); if (mbMonocular) InitializeIMU(1.f, 1e5, true); else InitializeIMU(1.f, 1e5, true); cout << "end VIBA 1" << endl; } } // Step 9.2 根据条件判断是否进行VIBA2（IMU第三次初始化） // 当前关键帧所在的地图还未完成VIBA 2 else if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2()){ if (mTinit>15.0f){ cout << "start VIBA 2" << endl; mpCurrentKeyFrame->GetMap()->SetIniertialBA2(); if (mbMonocular) InitializeIMU(0.f, 0.f, true); else InitializeIMU(0.f, 0.f, true); cout << "end VIBA 2" << endl; } } // scale refinement // Step 9.3 在关键帧小于100时，会在满足一定时间间隔后多次进行尺度、重力方向优化 if (((mpAtlas->KeyFramesInMap())<=200) && ((mTinit>25.0f && mTinit<25.5f)|| (mTinit>35.0f && mTinit<35.5f)|| (mTinit>45.0f && mTinit<45.5f)|| (mTinit>55.0f && mTinit<55.5f)|| (mTinit>65.0f && mTinit<65.5f)|| (mTinit>75.0f && mTinit<75.5f))){ if (mbMonocular) // 使用了所有关键帧，但只优化尺度和重力方向以及速度和偏执（其实就是一切跟惯性相关的量） ScaleRefinement(); } } } } #ifdef REGISTER_TIMES vdLBASync_ms.push_back(timeKFCulling_ms); vdKFCullingSync_ms.push_back(timeKFCulling_ms); #endif // Step 10 将当前帧加入到闭环检测队列中 mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame); for(auto pKF : mlNewKeyFrameForDenseMap) mpPointCloudMapping->insertKeyFrame(pKF);
3 稠密建图在闭环检测线程中的改动
3.1 修正闭环进行本质图优化后
在检测到回环并且已经回环之后，地图有较大的改动，因此我们需要更新稠密建图的内容：
在CorrectLoop中：
做完图优化后需要进行这一步
if(pLoopMap->IsInertial() && pLoopMap->isImuInitialized()) { Optimizer::OptimizeEssentialGraph4DoF(pLoopMap, mpLoopMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections); } else { //cout << "Loop -> Scale correction: " << mg2oLoopScw.scale() << endl; // Step 7. 进行EssentialGraph优化，LoopConnections是形成闭环后新生成的连接关系，不包括步骤7中当前帧与闭环匹配帧之间的连接关系 Optimizer::OptimizeEssentialGraph(pLoopMap, mpLoopMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, bFixedScale); } #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndOpt = std::chrono::steady_clock::now(); double timeOptEss = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndOpt - time_EndFusion).count(); vdLoopOptEss_ms.push_back(timeOptEss); #endif mpAtlas->InformNewBigChange(); // 创建一个新的线程对象并调用了名为 updatecloud 的函数。 // 具体而言，它是针对 PointCloudMapping 类的 updatecloud 函数，使用了 mpPointCloudMapping 的实例，并传入了 mpCurrentKF->GetMap() 的引用作为参数。 // 这段代码在多线程环境下更新点云数据。 // 也就是说 : 我用一个线程指针mpPointCloudMapping调用其中的方法updatecloud其中传入一个地图参数为*mpCurrentKF->GetMap() mpPointCloudMapping->mabIsUpdating = false; // 强制让已有的更新停止，进行新的 mpThreadDML = new thread(&PointCloudMapping::updatecloud, mpPointCloudMapping, std::ref(*mpCurrentKF->GetMap())); // detach() 是线程对象的方法，它会将线程与其执行的函数分离，使得线程可以独立运行，不再与其所属的对象相关联。 // 一旦调用了 detach()，该线程就会在后台运行，不再受到主线程的控制。 // 通常，这样的操作可以避免在程序结束时出现资源释放的问题，因为线程会自行结束其生命周期。 mpThreadDML->detach(); cout << "Map updated!" << endl;用mabIsUpdating标志位通知稠密建图线程不要再去做点云更新了，毫无意义了已经。然后开启一个新线程去进行稠密建图。
// 创建一个新的线程对象并调用了名为 updatecloud 的函数。
// 具体而言，它是针对 PointCloudMapping 类的 updatecloud 函数，使用了 mpPointCloudMapping 的实例，并传入了 mpCurrentKF->GetMap() 的引用作为参数。
// 这段代码在多线程环境下更新点云数据。
// 也就是说 : 我用一个线程指针mpPointCloudMapping调用其中的方法updatecloud其中传入一个地图参数为*mpCurrentKF->GetMap()// detach() 是线程对象的方法，它会将线程与其执行的函数分离，使得线程可以独立运行，不再与其所属的对象相关联。
// 一旦调用了 detach()，该线程就会在后台运行，不再受到主线程的控制。
// 通常，这样的操作可以避免在程序结束时出现资源释放的问题，因为线程会自行结束其生命周期。
3.2 纯视觉地图融合后LoopClosing::MergeLocal
在全局BA后仍然需要更新地图，更新方法和前面一样：
mpPointCloudMapping->mabIsUpdating = false; // 强制让已有的更新停止，进行新的 mpThreadDML = new thread(&PointCloudMapping::updatecloud, mpPointCloudMapping, std::ref(*pMergeMap)); mpThreadDML->detach(); cout << "Map updated!" << endl;
3.3 进行全局BA之后LoopClosing::RunGlobalBundleAdjustment
仍然需要优化地图，因为全局BA后相机位姿也有很大的变化：
/** * @brief MergeLocal CorrectLoop 中调用 * @param pActiveMap 当前地图 * @param nLoopKF 检测到回环成功的关键帧，不是与之匹配的老关键帧 */ void LoopClosing::RunGlobalBundleAdjustment(Map* pActiveMap, unsigned long nLoopKF) { Verbose::PrintMess("Starting Global Bundle Adjustment", Verbose::VERBOSITY_NORMAL); #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_StartFGBA = std::chrono::steady_clock::now(); nFGBA_exec += 1; vnGBAKFs.push_back(pActiveMap->GetAllKeyFrames().size()); vnGBAMPs.push_back(pActiveMap->GetAllMapPoints().size()); #endif // imu 初始化成功才返回true，只要一阶段成功就为true const bool bImuInit = pActiveMap->isImuInitialized(); if(!bImuInit) Optimizer::GlobalBundleAdjustemnt(pActiveMap,10,&mbStopGBA,nLoopKF,false); else // 仅有一个地图且内部关键帧<200，并且IMU完成了第一阶段初始化后才会进行下面 Optimizer::FullInertialBA(pActiveMap,7,false,nLoopKF,&mbStopGBA); #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndGBA = std::chrono::steady_clock::now(); double timeGBA = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndGBA - time_StartFGBA).count(); vdGBA_ms.push_back(timeGBA); if(mbStopGBA) { nFGBA_abort += 1; } #endif // 记录GBA已经迭代次数,用来检查全局BA过程是否是因为意外结束的 int idx = mnFullBAIdx; // Optimizer::GlobalBundleAdjustemnt(mpMap,10,&mbStopGBA,nLoopKF,false); // Update all MapPoints and KeyFrames // Local Mapping was active during BA, that means that there might be new keyframes // not included in the Global BA and they are not consistent with the updated map. // We need to propagate the correction through the spanning tree { unique_lock<mutex> lock(mMutexGBA); if(idx!=mnFullBAIdx) return; if(!bImuInit && pActiveMap->isImuInitialized()) return; if(!mbStopGBA) { Verbose::PrintMess("Global Bundle Adjustment finished", Verbose::VERBOSITY_NORMAL); Verbose::PrintMess("Updating map ...", Verbose::VERBOSITY_NORMAL); mpLocalMapper->RequestStop(); // Wait until Local Mapping has effectively stopped while(!mpLocalMapper->isStopped() && !mpLocalMapper->isFinished()) { usleep(1000); } // Get Map Mutex unique_lock<mutex> lock(pActiveMap->mMutexMapUpdate); // cout << "LC: Update Map Mutex adquired" << endl; //pActiveMap->PrintEssentialGraph(); // Correct keyframes starting at map first keyframe list<KeyFrame*> lpKFtoCheck(pActiveMap->mvpKeyFrameOrigins.begin(),pActiveMap->mvpKeyFrameOrigins.end()); // 通过树的方式更新未参与全局优化的关键帧，一个关键帧与其父节点的共视点数最多，所以选其作为参考帧 while(!lpKFtoCheck.empty()) { KeyFrame* pKF = lpKFtoCheck.front(); const set<KeyFrame*> sChilds = pKF->GetChilds(); //cout << "---Updating KF " << pKF->mnId << " with " << sChilds.size() << " childs" << endl; //cout << " KF mnBAGlobalForKF: " << pKF->mnBAGlobalForKF << endl; Sophus::SE3f Twc = pKF->GetPoseInverse(); //cout << "Twc: " << Twc << endl; //cout << "GBA: Correct KeyFrames" << endl; // 广度优先搜索 for(set<KeyFrame*>::const_iterator sit=sChilds.begin();sit!=sChilds.end();sit++) { KeyFrame* pChild = *sit; if(!pChild || pChild->isBad()) continue; // 专门处理没有参与优化的新关键帧 if(pChild->mnBAGlobalForKF!=nLoopKF) { //cout << "++++New child with flag " << pChild->mnBAGlobalForKF << "; LoopKF: " << nLoopKF << endl; //cout << " child id: " << pChild->mnId << endl; Sophus::SE3f Tchildc = pChild->GetPose() * Twc; //cout << "Child pose: " << Tchildc << endl; //cout << "pKF->mTcwGBA: " << pKF->mTcwGBA << endl; pChild->mTcwGBA = Tchildc * pKF->mTcwGBA;//*Tcorc*pKF->mTcwGBA; Sophus::SO3f Rcor = pChild->mTcwGBA.so3().inverse() * pChild->GetPose().so3(); if(pChild->isVelocitySet()){ pChild->mVwbGBA = Rcor * pChild->GetVelocity(); } else Verbose::PrintMess("Child velocity empty!! ", Verbose::VERBOSITY_NORMAL); //cout << "Child bias: " << pChild->GetImuBias() << endl; pChild->mBiasGBA = pChild->GetImuBias(); pChild->mnBAGlobalForKF = nLoopKF; // 标记成更新过的 } lpKFtoCheck.push_back(pChild); } //cout << "-------Update pose" << endl; pKF->mTcwBefGBA = pKF->GetPose(); //cout << "pKF->mTcwBefGBA: " << pKF->mTcwBefGBA << endl; pKF->SetPose(pKF->mTcwGBA); /*cv::Mat Tco_cn = pKF->mTcwBefGBA * pKF->mTcwGBA.inv(); cv::Vec3d trasl = Tco_cn.rowRange(0,3).col(3); double dist = cv::norm(trasl); cout << "GBA: KF " << pKF->mnId << " had been moved " << dist << " meters" << endl; double desvX = 0; double desvY = 0; double desvZ = 0; if(pKF->mbHasHessian) { cv::Mat hessianInv = pKF->mHessianPose.inv(); double covX = hessianInv.at<double>(3,3); desvX = std::sqrt(covX); double covY = hessianInv.at<double>(4,4); desvY = std::sqrt(covY); double covZ = hessianInv.at<double>(5,5); desvZ = std::sqrt(covZ); pKF->mbHasHessian = false; } if(dist > 1) { cout << "--To much distance correction: It has " << pKF->GetConnectedKeyFrames().size() << " connected KFs" << endl; cout << "--It has " << pKF->GetCovisiblesByWeight(80).size() << " connected KF with 80 common matches or more" << endl; cout << "--It has " << pKF->GetCovisiblesByWeight(50).size() << " connected KF with 50 common matches or more" << endl; cout << "--It has " << pKF->GetCovisiblesByWeight(20).size() << " connected KF with 20 common matches or more" << endl; cout << "--STD in meters(x, y, z): " << desvX << ", " << desvY << ", " << desvZ << endl; string strNameFile = pKF->mNameFile; cv::Mat imLeft = cv::imread(strNameFile, CV_LOAD_IMAGE_UNCHANGED); cv::cvtColor(imLeft, imLeft, CV_GRAY2BGR); vector<MapPoint*> vpMapPointsKF = pKF->GetMapPointMatches(); int num_MPs = 0; for(int i=0; i<vpMapPointsKF.size(); ++i) { if(!vpMapPointsKF[i] || vpMapPointsKF[i]->isBad()) { continue; } num_MPs += 1; string strNumOBs = to_string(vpMapPointsKF[i]->Observations()); cv::circle(imLeft, pKF->mvKeys[i].pt, 2, cv::Scalar(0, 255, 0)); cv::putText(imLeft, strNumOBs, pKF->mvKeys[i].pt, CV_FONT_HERSHEY_DUPLEX, 1, cv::Scalar(255, 0, 0)); } cout << "--It has " << num_MPs << " MPs matched in the map" << endl; string namefile = "./test_GBA/GBA_" + to_string(nLoopKF) + "_KF" + to_string(pKF->mnId) +"_D" + to_string(dist) +".png"; cv::imwrite(namefile, imLeft); }*/ if(pKF->bImu) { //cout << "-------Update inertial values" << endl; pKF->mVwbBefGBA = pKF->GetVelocity(); //if (pKF->mVwbGBA.empty()) // Verbose::PrintMess("pKF->mVwbGBA is empty", Verbose::VERBOSITY_NORMAL); //assert(!pKF->mVwbGBA.empty()); pKF->SetVelocity(pKF->mVwbGBA); pKF->SetNewBias(pKF->mBiasGBA); } lpKFtoCheck.pop_front(); } //cout << "GBA: Correct MapPoints" << endl; // Correct MapPoints // 更新mp点 const vector<MapPoint*> vpMPs = pActiveMap->GetAllMapPoints(); for(size_t i=0; i<vpMPs.size(); i++) { MapPoint* pMP = vpMPs[i]; if(pMP->isBad()) continue; // NOTICE 并不是所有的地图点都会直接参与到全局BA优化中,但是大部分的地图点需要根据全局BA优化后的结果来重新纠正自己的位姿 // 如果这个地图点直接参与到了全局BA优化的过程,那么就直接重新设置器位姿即可 if(pMP->mnBAGlobalForKF==nLoopKF) { // If optimized by Global BA, just update pMP->SetWorldPos(pMP->mPosGBA); } else // 如故这个地图点并没有直接参与到全局BA优化的过程中,那么就使用器参考关键帧的新位姿来优化自己的位姿 { // Update according to the correction of its reference keyframe // 说明这个关键帧，在前面的过程中也没有因为“当前关键帧”得到全局BA优化 //? 可是,为什么会出现这种情况呢? 难道是因为这个地图点的参考关键帧设置成为了bad? KeyFrame* pRefKF = pMP->GetReferenceKeyFrame(); if(pRefKF->mnBAGlobalForKF!=nLoopKF) continue; /*if(pRefKF->mTcwBefGBA.empty()) continue;*/ // Map to non-corrected camera // cv::Mat Rcw = pRefKF->mTcwBefGBA.rowRange(0,3).colRange(0,3); // cv::Mat tcw = pRefKF->mTcwBefGBA.rowRange(0,3).col(3); // 转换到其参考关键帧相机坐标系下的坐标 Eigen::Vector3f Xc = pRefKF->mTcwBefGBA * pMP->GetWorldPos(); // Backproject using corrected camera // 然后使用已经纠正过的参考关键帧的位姿,再将该地图点变换到世界坐标系下 pMP->SetWorldPos(pRefKF->GetPoseInverse() * Xc); } } pActiveMap->InformNewBigChange(); pActiveMap->IncreaseChangeIndex(); // TODO Check this update // mpTracker->UpdateFrameIMU(1.0f, mpTracker->GetLastKeyFrame()->GetImuBias(), mpTracker->GetLastKeyFrame()); mpLocalMapper->Release(); #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndUpdateMap = std::chrono::steady_clock::now(); double timeUpdateMap = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndUpdateMap - time_EndGBA).count(); vdUpdateMap_ms.push_back(timeUpdateMap); double timeFGBA = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndUpdateMap - time_StartFGBA).count(); vdFGBATotal_ms.push_back(timeFGBA); #endif Verbose::PrintMess("Map updated!", Verbose::VERBOSITY_NORMAL); mpPointCloudMapping->mabIsUpdating = false; // 强制让已有的更新停止，进行新的 mpThreadDML = new thread(&PointCloudMapping::updatecloud, mpPointCloudMapping, std::ref(*pActiveMap)); mpThreadDML->detach(); cout << "Map updated!" << endl; } mbFinishedGBA = true; mbRunningGBA = false; } }
4 稠密建图的一些API
4.0 构造函数
pcl::PointCloud<pcl::PointXYZRGBA> pcl_filter; pcl::PointCloud<pcl::PointXYZRGBA> pcl_local_filter; ros::Publisher pclPoint_pub; ros::Publisher pclPoint_local_pub; ros::Publisher octomap_pub; sensor_msgs::PointCloud2 pcl_point; sensor_msgs::PointCloud2 pcl_local_point; pcl::PointCloud<pcl::PointXYZRGBA> pcl_cloud_local_kf; pcl::PointCloud<pcl::PointXYZRGBA> pcl_cloud_kf; 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); mPointCloud = boost::make_shared<PointCloud>(); viewerThread = std::make_shared<std::thread>(&PointCloudMapping::NormalshowPointCloud, this); // make_unique是c++14的 }定义了一些ROS的发布器以及初始化了显示器，显示器在4.2节有详细介绍。
4.1 PointCloudMapping::insertKeyFrame 插入关键帧
/** * @brief 从追踪线程插入关键帧 * @param kf 关键帧 * @param color RGB图像 * @param depth 深度图像 * @param vpKFs 所有地图的所有的关键帧 */ void PointCloudMapping::insertKeyFrame(KeyFrame* kf, const cv::Mat& color, const cv::Mat& depth,vector<KeyFrame*> vpKFs) { unique_lock<mutex> locker(mKeyFrameMtx); cout << GREEN <<"receive a keyframe, id = "<<kf->mnId<<" 第"<<kf->mnId<<"个"<<endl; mvKeyFrames.push(kf); keyframes.push_back( kf ); currentvpKFs = vpKFs; cv::Mat colorImg_ ,depth_; mvColorImgs.push( color.clone() ); mvDepthImgs.push( depth.clone() ); // std::condition_variable mKeyFrameUpdatedCond; // 这行代码是在使用条件变量（std::condition_variable）中的 notify_one() 方法。 // 这个方法的作用是通知一个等待在这个条件变量上的线程，告诉它们某些条件已经发生变化，可能是在多线程环境下用于线程间的同步。 // 当条件变量被通知时，等待在这个条件变量上的一个线程会被唤醒，继续执行。 mKeyFrameUpdatedCond.notify_one(); }注释已经写的很明白了，就是放入一些变量并发送通知。
4.2 稠密建图的显示线程 PointCloudMapping::viewer
这个进程不断在进行....
主要做的工作在这里：
生成每一个关键帧的点云（4.4节）并转换到世界坐标系下。
对其进行体素滤波并显示。
/** * @brief 显示线程 */ void PointCloudMapping::viewer() { pcl::visualization::CloudViewer viewer("viewer"); while (1) { { unique_lock<mutex> lck_shutdown(shutDownMutex); if (shutDownFlag) { break; } } if (bStop || mabIsUpdating) { continue; } int N; std::list<KeyFrame *> lNewKeyFrames; { unique_lock<mutex> lck(keyframeMutex); N = mlNewKeyFrames.size(); lNewKeyFrames = mlNewKeyFrames; if(N == 0) continue; else { mlNewKeyFrames.clear(); } } double generatePointCloudTime = 0, transformPointCloudTime = 0; for (auto pKF : lNewKeyFrames) { if (pKF->isBad()) continue; generatePointCloud(pKF); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr p(new pcl::PointCloud<pcl::PointXYZRGBA>); pcl::transformPointCloud(*(pKF->mptrPointCloud), *(p), Converter::toMatrix4d(pKF->GetPoseInverse())); { std::unique_lock<std::mutex> lck(mMutexGlobalMap); *globalMap += *p; } } pcl::PointCloud<pcl::PointXYZRGBA>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGBA>); // 去除孤立点这个比较耗时，用处也不是很大，可以去掉 // statistical_filter->setInputCloud(globalMap); // statistical_filter->filter(*tmp); voxel->setInputCloud(globalMap); voxel->filter(*globalMap); viewer.showCloud(globalMap); // 这个比较费时，建议不需要实时显示的可以屏蔽或改成几次显示一次 } }
4.3 地图更新 PointCloudMapping::updatecloud
/** * @brief 更新当前点云 * @param curMap 当前地图 */ void PointCloudMapping::updatecloud(Map &curMap) { std::unique_lock<std::mutex> lck(updateMutex); mabIsUpdating = true; 1.获取当前地图的所有关键帧 currentvpKFs = curMap.GetAllKeyFrames(); // loopbusy = true; cout << "开始点云更新" << endl; pcl::PointCloud<pcl::PointXYZRGBA>::Ptr tmpGlobalMap(new pcl::PointCloud<pcl::PointXYZRGBA>); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr curPointCloud(new pcl::PointCloud<pcl::PointXYZRGBA>); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr tmpGlobalMapFilter(new pcl::PointCloud<pcl::PointXYZRGBA>()); 2.遍历每一个关键帧 /// 这段代码的目的似乎是将当前关键帧的点云数据转换到世界坐标系下，然后将其合并到全局地图中，并对全局地图进行体素滤波处理，以最终更新或维护一个更精确的全局地图。 for (int i = 0; i < currentvpKFs.size(); i++) { /// 2.1在闭环检测时如果地图有本质图优化则说明地图有较大改变,停止更新地图 if (!mabIsUpdating) { std::cout << "中断点云更新" <<std::endl; return; } /// 2.2 如果这个关键帧没有被优化掉 且 这个关键帧生成了点云 // 在PointCloudMapping::generatePointCloud中关键帧生成点云 if (!currentvpKFs[i]->isBad() && currentvpKFs[i]->mptrPointCloud) { // 生成的点云是在相机坐标系下的,要转化成世界坐标系下 pcl::transformPointCloud( *(currentvpKFs[i]->mptrPointCloud), *(curPointCloud), Converter::toMatrix4d(currentvpKFs[i]->GetPoseInverse())); *tmpGlobalMap += *curPointCloud; voxel->setInputCloud(tmpGlobalMap); voxel->filter(*tmpGlobalMapFilter); // 将 tmpGlobalMap 指向的点云数据和 tmpGlobalMapFilter 指向的点云数据进行交换。 tmpGlobalMap->swap(*tmpGlobalMapFilter); } } cout << "点云更新完成" << endl; { std::unique_lock<std::mutex> lck(mMutexGlobalMap); globalMap = tmpGlobalMap; } mabIsUpdating = false; }对当前地图进行更新，这里的调用场景是在闭环检测线程进行全局BA之后（3.3节）。将所有关键帧重新进行依据深度图的重建。
4.4 由关键帧和深度图生成稠密点云 PointCloudMapping::generatePointCloud
这里由RGB图像和深度图生成稠密点云：就是把一个关键帧生成稠密点云。
这里用了循环展开进行多线程计算（不懂原理可以参考：深入理解计算机系统 第五章 优化）。
void PointCloudMapping::generatePointCloud(KeyFrame *kf) //,Eigen::Isometry3d T { pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pPointCloud(new pcl::PointCloud<pcl::PointXYZRGBA>); // point cloud is null ptr for (int m = 0; m < kf->imDepth.rows; m += 4) { for (int n = 0; n < kf->imDepth.cols; n += 4) { float d = kf->imDepth.ptr<float>(m)[n]; if (d < 0.05 || d > 6) continue; pcl::PointXYZRGBA p; p.z = d; p.x = (n - kf->cx) * p.z / kf->fx; p.y = (m - kf->cy) * p.z / kf->fy; p.b = kf->imLeftRgb.ptr<uchar>(m)[n * 3]; p.g = kf->imLeftRgb.ptr<uchar>(m)[n * 3 + 1]; p.r = kf->imLeftRgb.ptr<uchar>(m)[n * 3 + 2]; pPointCloud->points.push_back(p); } } pPointCloud->height = 1; pPointCloud->width = pPointCloud->points.size(); pPointCloud->is_dense = true; kf->mptrPointCloud = pPointCloud; }