【ORB-SLAM3】CreateNewKeyFrame()函数阅读

void Tracking::CreateNewKeyFrame()

void Tracking::CreateNewKeyFrame()
{// 如果局部建图线程正在初始化且没做完或关闭了,就无法插入关键帧if(mpLocalMapper->IsInitializing() && !mpAtlas->isImuInitialized())return;if(!mpLocalMapper->SetNotStop(true))return;// Step 1：将当前帧构造成关键帧KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB);if(mpAtlas->isImuInitialized()) //  || mpLocalMapper->IsInitializing())pKF->bImu = true;pKF->SetNewBias(mCurrentFrame.mImuBias);// Step 2：将当前关键帧设置为当前帧的参考关键帧// 在UpdateLocalKeyFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧mpReferenceKF = pKF;mCurrentFrame.mpReferenceKF = pKF;if(mpLastKeyFrame){pKF->mPrevKF = mpLastKeyFrame;mpLastKeyFrame->mNextKF = pKF;}elseVerbose::PrintMess("No last KF in KF creation!!", Verbose::VERBOSITY_NORMAL);// Reset preintegration from last KF (Create new object)if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD){mpImuPreintegratedFromLastKF = new IMU::Preintegrated(pKF->GetImuBias(),pKF->mImuCalib);}// 这段代码和 Tracking::UpdateLastFrame 中的那一部分代码功能相同// Step 3：对于双目或rgbd摄像头，为当前帧生成新的地图点；单目无操作if(mSensor!=System::MONOCULAR && mSensor != System::IMU_MONOCULAR) // TODO check if incluide imu_stereo{// 根据Tcw计算mRcw、mtcw和mRwc、mOwmCurrentFrame.UpdatePoseMatrices();// cout << "create new MPs" << endl;// We sort points by the measured depth by the stereo/RGBD sensor.// We create all those MapPoints whose depth < mThDepth.// If there are less than 100 close points we create the 100 closest.int maxPoint = 100;if(mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)maxPoint = 100;// Step 3.1：得到当前帧有深度值的特征点（不一定是地图点）vector<pair<float,int> > vDepthIdx;int N = (mCurrentFrame.Nleft != -1) ? mCurrentFrame.Nleft : mCurrentFrame.N;vDepthIdx.reserve(mCurrentFrame.N);for(int i=0; i<N; i++){float z = mCurrentFrame.mvDepth[i];if(z>0){// 第一个元素是深度,第二个元素是对应的特征点的idvDepthIdx.push_back(make_pair(z,i));}}if(!vDepthIdx.empty()){// Step 3.2：按照深度从小到大排序sort(vDepthIdx.begin(),vDepthIdx.end());// Step 3.3：从中找出不是地图点的生成临时地图点 // 处理的近点的个数int nPoints = 0;for(size_t j=0; j<vDepthIdx.size();j++){int i = vDepthIdx[j].second;bool bCreateNew = false;// 如果这个点对应在上一帧中的地图点没有,或者创建后就没有被观测到,那么就生成一个临时的地图点MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];if(!pMP)bCreateNew = true;else if(pMP->Observations()<1){bCreateNew = true;mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);}// 如果需要就新建地图点，这里的地图点不是临时的，是全局地图中新建地图点，用于跟踪if(bCreateNew){Eigen::Vector3f x3D;if(mCurrentFrame.Nleft == -1){mCurrentFrame.UnprojectStereo(i, x3D);}else{x3D = mCurrentFrame.UnprojectStereoFishEye(i);}MapPoint* pNewMP = new MapPoint(x3D,pKF,mpAtlas->GetCurrentMap());// 这些添加属性的操作是每次创建MapPoint后都要做的pNewMP->AddObservation(pKF,i);//Check if it is a stereo observation in order to not//duplicate mappointsif(mCurrentFrame.Nleft != -1 && mCurrentFrame.mvLeftToRightMatch[i] >= 0){mCurrentFrame.mvpMapPoints[mCurrentFrame.Nleft + mCurrentFrame.mvLeftToRightMatch[i]]=pNewMP;pNewMP->AddObservation(pKF,mCurrentFrame.Nleft + mCurrentFrame.mvLeftToRightMatch[i]);pKF->AddMapPoint(pNewMP,mCurrentFrame.Nleft + mCurrentFrame.mvLeftToRightMatch[i]);}pKF->AddMapPoint(pNewMP,i);pNewMP->ComputeDistinctiveDescriptors();pNewMP->UpdateNormalAndDepth();mpAtlas->AddMapPoint(pNewMP);mCurrentFrame.mvpMapPoints[i]=pNewMP;nPoints++;}else{// 因为从近到远排序，记录其中不需要创建地图点的个数nPoints++;}// Step 3.4：停止新建地图点必须同时满足以下条件：// 1、当前的点的深度已经超过了设定的深度阈值（35倍基线）// 2、nPoints已经超过100个点，说明距离比较远了，可能不准确，停掉退出if(vDepthIdx[j].first>mThDepth && nPoints>maxPoint){break;}}//Verbose::PrintMess("new mps for stereo KF: " + to_string(nPoints), Verbose::VERBOSITY_NORMAL);}}// Step 4：插入关键帧// 关键帧插入到列表 mlNewKeyFrames中，等待local mapping线程临幸mpLocalMapper->InsertKeyFrame(pKF);// 插入好了，允许局部建图停止mpLocalMapper->SetNotStop(false);// 当前帧成为新的关键帧，更新mnLastKeyFrameId = mCurrentFrame.mnId;mpLastKeyFrame = pKF;
}

1、将当前帧构造为关键帧 

    // Step 1：将当前帧构造成关键帧KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB);if(mpAtlas->isImuInitialized()) //  || mpLocalMapper->IsInitializing())pKF->bImu = true;pKF->SetNewBias(mCurrentFrame.mImuBias);

在KeyFrame.h中，写了关键帧类的定义。其中构造函数为，KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB);表示构造一个关键帧需要的参数为：Frame类型的当前帧：mCurrentFrame；保存所有关键帧和地图点的地图集：mpAtlas->GetCurrentMap()；保存关键帧的数据库：mpKeyFrameDB

其实从ORB-SLAM3的框图中可以看出，关键帧衔接了前端和后端，后端开始和地图点有关。从代码的角度看，通过构造关键帧，将普通帧 mCurrentFrame 携带的信息复制给 关键帧pKF 。一个关键帧对象上除了有普通帧的帧信息，还新增了和地图生成与管理相关的操作。

2、将当前关键帧设置为当前帧的参考关键帧

3、对于双目或rgbd摄像头，为当前帧生成新的地图点

（1）从当前帧中筛选出深度值为正的特征点，深度值无效或为负的点会被过滤，并存储为(深度, 特征点索引)的键值对。

        vector<pair<float,int> > vDepthIdx;int N = (mCurrentFrame.Nleft != -1) ? mCurrentFrame.Nleft : mCurrentFrame.N;vDepthIdx.reserve(mCurrentFrame.N);for(int i=0; i<N; i++){float z = mCurrentFrame.mvDepth[i];if(z>0){// 第一个元素是深度,第二个元素是对应的特征点的idvDepthIdx.push_back(make_pair(z,i));}}

（2）按深度从小到大排序

这是因为，近的点深度更准确，更适合用于建图。（也是因为这个原因，深度相机更适合室内环境）

            sort(vDepthIdx.begin(),vDepthIdx.end());

（3）从中找出不是地图点的生成临时地图点

①如果这个点对应在上一帧中的地图点没有,  或者创建后就没有被观测到,那么就需要生成一个临时的地图点。（将bCreateNew标志位设置为true，表示需要创建）：

                // 如果这个点对应在上一帧中的地图点没有,或者创建后就没有被观测到,那么就生成一个临时的地图点MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];if(!pMP)bCreateNew = true;else if(pMP->Observations()<1){bCreateNew = true;mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);}

②创建地图点：根据mCurrentFrame.Nleft == -1是否成立，判断使用什么反投影函数

                    if(mCurrentFrame.Nleft == -1){mCurrentFrame.UnprojectStereo(i, x3D);}else{x3D = mCurrentFrame.UnprojectStereoFishEye(i);}

• mCurrentFrame.Nleft == -1  ， 双目 / RGBD 相机：当某个特征点的深度信息或者双目信息有效时,将它反投影到三维世界坐标系中。

从深度图到 3D 网格与点云：完整实现_深度图转点云-CSDN博客

bool Frame::UnprojectStereo(const int &i, Eigen::Vector3f &x3D)
{const float z = mvDepth[i];if(z>0) {const float u = mvKeysUn[i].pt.x;const float v = mvKeysUn[i].pt.y;const float x = (u-cx)*z*invfx;const float y = (v-cy)*z*invfy;Eigen::Vector3f x3Dc(x, y, z);x3D = mRwc * x3Dc + mOw;return true;} elsereturn false;
}

• mCurrentFrame.Nleft ！= -1  ，单目 / 鱼眼相机：表示当前帧为单目模式（无右图像），或使用鱼眼相机但未启用立体模式，需要根据位姿将第i个点投到世界坐标系下。

如何将相机坐标系得到的位姿转换到世界系下机体的位姿_imu坐标系转换到机体坐标系-CSDN博客

Eigen::Vector3f Frame::UnprojectStereoFishEye(const int &i){return mRwc * mvStereo3Dpoints[i] + mOw;
}

将x3D添加到地图点中：

 MapPoint* pNewMP = new MapPoint(x3D,pKF,mpAtlas->GetCurrentMap());

④停止生成地图点的条件：当前的点的深度已经超过了设定的深度阈值（35倍基线）&& nPoints已经超过100个点，说明距离比较远了，可能不准确，停掉退出

（4）插入关键帧

    // Step 4：插入关键帧// 关键帧插入到列表 mlNewKeyFrames中，等待local mapping线程临幸mpLocalMapper->InsertKeyFrame(pKF);// 插入好了，允许局部建图停止mpLocalMapper->SetNotStop(false);// 当前帧成为新的关键帧，更新mnLastKeyFrameId = mCurrentFrame.mnId;mpLastKeyFrame = pKF;

其中，InsertKeyFrame(KeyFrame *pKF)成员函数，将关键帧插入到list类型的mlNewKeyFrames中：

void LocalMapping::InsertKeyFrame(KeyFrame *pKF)
{unique_lock<mutex> lock(mMutexNewKFs);// 将关键帧插入到列表中mlNewKeyFrames.push_back(pKF);mbAbortBA=true;
}