ORB_SLAM3 LocalMapping中关键帧的处理ProcessNewKeyFrame和地图点的筛选MapPointCulling
地图点来源：
首先，地图点来源有三部分：
- 通过
Tracking跟踪到当前帧或者当前关键帧(投影) - 对于双目或者RGB-D相机，在创建关键帧时，对未匹配的点通过深度生成(深度)
- 在
LocalMapping中通过CreateNewMapPoints三角化未匹配的点生成(三角化)
对于1这部分在ProcessNewKeyFrame需要更新属性，对于3在CreateNewMapPoints中生成，对于2，3需要对其进行检验MapPointCulling，因为1已经检验过了，所以不再需要
ProcessNewKeyFrame
ProcessNewKeyFrame的主要作用是：处理mlNewKeyFrames中的关键帧，给关键帧添加Bow信息，更新关键帧中的地图点的属性，并更新和其他关键帧的连接。
- 从队列
mlNewKeyFrames中取出当前关键帧mpCurrentKeyFrame，其中mlNewKeyFrames为Tracking线程插入的关键帧
{
unique_lock<mutex> lock(mMutexNewKFs);
mpCurrentKeyFrame = mlNewKeyFrames.front();
mlNewKeyFrames.pop_front();
}
- 计算当前关键帧
mpCurrentKeyFrame的Bow信息，参考ORB_SLAM3 TrackReferenceKeyFrame的计算当前帧的描述子的Bow向量
mpCurrentKeyFrame->ComputeBoW();
-
对当前关键帧的地图点：
- 添加观测
AddObservation
表示该地图点可以被关键帧KF i中的第j个特征点观测到
void MapPoint::AddObservation(KeyFrame* pKF, int idx) { unique_lock<mutex> lock(mMutexFeatures); tuple<int,int> indexes; if(mObservations.count(pKF)){ indexes = mObservations[pKF]; } else{ indexes = tuple<int,int>(-1,-1); } if(pKF -> NLeft != -1 && idx >= pKF -> NLeft){ get<1>(indexes) = idx; } else{ get<0>(indexes) = idx; } //观测，表示该Mappoint可以被关键帧pkf中的第j个特征点观测到 mObservations[pKF]=indexes; //观测更新 if(!pKF->mpCamera2 && pKF->mvuRight[idx]>=0) nObs+=2; else nObs++; }- 更新法向量和观测深度
UpdateNormalAndDepth
根据所有能够观测到该地图点的关键帧，求每帧观测到该地图点的观测方向，然后对所有观测方向求和，得到该地图点的平均观测方向
根据参考关键帧观测到该地图点的距离，求得最大/最小观测距离
void MapPoint::UpdateNormalAndDepth() { map<KeyFrame*,tuple<int,int>> observations; KeyFrame* pRefKF; Eigen::Vector3f Pos; { unique_lock<mutex> lock1(mMutexFeatures); unique_lock<mutex> lock2(mMutexPos); if(mbBad) return; //该地图点的观测 observations = mObservations; //该地图点的参考关键帧 pRefKF = mpRefKF; //该地图点在世界坐标系下的位置 Pos = mWorldPos; } if(observations.empty()) return; Eigen::Vector3f normal; normal.setZero(); int n=0; //根据该地图点的观测，计算地图点的平均法向量，最大/最小深度 for(map<KeyFrame*,tuple<int,int>>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) { KeyFrame* pKF = mit->first; tuple<int,int> indexes = mit -> second; int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes); if(leftIndex != -1){ Eigen::Vector3f Owi = pKF->GetCameraCenter(); //normal = 地图点 - 关键帧光心 Eigen::Vector3f normali = Pos - Owi; normal = normal + normali / normali.norm(); n++; } if(rightIndex != -1){ Eigen::Vector3f Owi = pKF->GetRightCameraCenter(); Eigen::Vector3f normali = Pos - Owi; normal = normal + normali / normali.norm(); n++; } } //地图点 - 参考关键帧相机光心 Eigen::Vector3f PC = Pos - pRefKF->GetCameraCenter(); const float dist = PC.norm(); tuple<int ,int> indexes = observations[pRefKF]; int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes); int level; if(pRefKF -> NLeft == -1){ level = pRefKF->mvKeysUn[leftIndex].octave; } else if(leftIndex != -1){ level = pRefKF -> mvKeys[leftIndex].octave; } else{ level = pRefKF -> mvKeysRight[rightIndex - pRefKF -> NLeft].octave; } //const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave; const float levelScaleFactor = pRefKF->mvScaleFactors[level]; const int nLevels = pRefKF->mnScaleLevels; { unique_lock<mutex> lock3(mMutexPos); mfMaxDistance = dist*levelScaleFactor; mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1]; mNormalVector = normal/n; } }- 更新地图点的最佳描述子
ComputeDistinctiveDescriptors
根据所有能够观测到该地图点的关键帧，计算两两帧中分别对应的特征点的描述子距离，然后得到最具有代表性的描述子，其与其他的描述子具有最小的距离中值
void MapPoint::ComputeDistinctiveDescriptors() { // Retrieve all observed descriptors vector<cv::Mat> vDescriptors; map<KeyFrame*,tuple<int,int>> observations; { unique_lock<mutex> lock1(mMutexFeatures); if(mbBad) return; observations=mObservations; } if(observations.empty()) return; vDescriptors.reserve(observations.size()); for(map<KeyFrame*,tuple<int,int>>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) { KeyFrame* pKF = mit->first; if(!pKF->isBad()){ tuple<int,int> indexes = mit -> second; int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes); if(leftIndex != -1){ vDescriptors.push_back(pKF->mDescriptors.row(leftIndex)); } if(rightIndex != -1){ vDescriptors.push_back(pKF->mDescriptors.row(rightIndex)); } } } if(vDescriptors.empty()) return; // Compute distances between them const size_t N = vDescriptors.size(); float Distances[N][N]; for(size_t i=0;i<N;i++) { Distances[i][i]=0; for(size_t j=i+1;j<N;j++) { int distij = ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]); Distances[i][j]=distij; Distances[j][i]=distij; } } // Take the descriptor with least median distance to the rest int BestMedian = INT_MAX; int BestIdx = 0; for(size_t i=0;i<N;i++) { vector<int> vDists(Distances[i],Distances[i]+N); sort(vDists.begin(),vDists.end()); int median = vDists[0.5*(N-1)]; if(median<BestMedian) { BestMedian = median; BestIdx = i; } } { unique_lock<mutex> lock(mMutexFeatures); mDescriptor = vDescriptors[BestIdx].clone(); } } - 添加观测
const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
for(size_t i=0; i<vpMapPointMatches.size(); i++)
{
MapPoint* pMP = vpMapPointMatches[i];
if(pMP)
{
if(!pMP->isBad())
{
if(!pMP->IsInKeyFrame(mpCurrentKeyFrame))
{
pMP->AddObservation(mpCurrentKeyFrame, i);
pMP->UpdateNormalAndDepth();
pMP->ComputeDistinctiveDescriptors();
}
else // this can only happen for new stereo points inserted by the Tracking
{
mlpRecentAddedMapPoints.push_back(pMP);
}
}
}
}
- 更新关键帧关联
UpdateConnections
mpCurrentKeyFrame->UpdateConnections();
其中，UpdateConnections主要的作用是更新关键帧之间的共视连接
- 根据当前关键帧的地图点
mvMapPoints建立与其他关键帧的关联，生成对应的共视程度表mConnectedKeyFrameWeights - 根据共视程度表，更新其他关键帧与当前关键帧的共视程度，建立其双向关联
- 如果为第一次链接，更新生成树的连接
void KeyFrame::UpdateConnections(bool upParent)
{
map<KeyFrame*,int> KFcounter;
vector<MapPoint*> vpMP;
{
unique_lock<mutex> lockMPs(mMutexFeatures);
vpMP = mvpMapPoints;
}
//For all map points in keyframe check in which other keyframes are they seen
//Increase counter for those keyframes
for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
if(!pMP)
continue;
if(pMP->isBad())
continue;
map<KeyFrame*,tuple<int,int>> observations = pMP->GetObservations();
for(map<KeyFrame*,tuple<int,int> >::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
if(mit->first->mnId==mnId || mit->first->isBad() || mit->first->GetMap() != mpMap)
continue;
KFcounter[mit->first]++;
}
}
// This should not happen
if(KFcounter.empty())
return;
//If the counter is greater than threshold add connection
//In case no keyframe counter is over threshold add the one with maximum counter
int nmax=0;
KeyFrame* pKFmax=NULL;
int th = 15;
vector<pair<int,KeyFrame*> > vPairs;
vPairs.reserve(KFcounter.size());
if(!upParent)
cout << "UPDATE_CONN: current KF " << mnId << endl;
for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
{
if(!upParent)
cout << " UPDATE_CONN: KF " << mit->first->mnId << " ; num matches: " << mit->second << endl;
if(mit->second>nmax)
{
nmax=mit->second;
pKFmax=mit->first;
}
if(mit->second>=th)
{
vPairs.push_back(make_pair(mit->second,mit->first));
(mit->first)->AddConnection(this,mit->second);
}
}
if(vPairs.empty())
{
vPairs.push_back(make_pair(nmax,pKFmax));
pKFmax->AddConnection(this,nmax);
}
sort(vPairs.begin(),vPairs.end());
list<KeyFrame*> lKFs;
list<int> lWs;
for(size_t i=0; i<vPairs.size();i++)
{
lKFs.push_front(vPairs[i].second);
lWs.push_front(vPairs[i].first);
}
{
unique_lock<mutex> lockCon(mMutexConnections);
mConnectedKeyFrameWeights = KFcounter;
mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
if(mbFirstConnection && mnId!=mpMap->GetInitKFid())
{
mpParent = mvpOrderedConnectedKeyFrames.front();
mpParent->AddChild(this);
mbFirstConnection = false;
}
}
}
- 将当前关键帧插入到地图中
AddKeyFrame
mpAtlas->AddKeyFrame(mpCurrentKeyFrame);
2.MapPointCulling
MapPointCulling的主要作用是：检测mlpRecentAddedMapPoints中新增的地图点，并剔除质量不好的地图点。
- 根据相机类型设置不同的观测阈值
- 对
mlpRecentAddedMapPoints中的地图点进行筛选，剔除质量不好的地图点- 该地图点是坏点
- 该地图点的查找率低于0.25
- 从该点建立开始，到现在已经过了不小于2个关键帧，但是观测到该点的关键帧数却不超过cnThObs帧
- 从该点建立开始，已经过了3个关键帧而没有被剔除，认为该地图点的质量还行(检验合格)，只对其从
mlpRecentAddedMapPoints剔除，而不设置为坏点
其中，查找率：在TRACKING线程中判定匹配到地图点的关键帧数量与预测可以看到地图点的关键帧数量之比
mnFound：记录了实际观测到地图点的关键帧数量mnVisible：则是估计能够看到该点的关键帧数量- 地图点的这两个变量
mnFound和mnVisible都是在Tracking线程中得到更新的。 - 在根据局部地图优化位姿的时候， 通过Tracking对象的成员函数
SearchLocalPoints对局部地图中的地图点进行粗略筛选，会根据观测到地图点的视角余弦值来估计当前帧能否观测到相应的地图点，并通过地图点的接口IncreaseVisible增加mnVisible的计数。 - Tracking对象完成地图点的粗筛之后，会进行一次位姿优化。如果位姿优化之后仍然能够匹配到地图点，就认为在该帧中找到了地图点。相应的调用地图点接口
IncreaseFound增加mnFound计数
void LocalMapping::MapPointCulling()
{
// Check Recent Added MapPoints
list<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();
const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;
int nThObs;
if(mbMonocular)
nThObs = 2;
else
nThObs = 3;
const int cnThObs = nThObs;
int borrar = mlpRecentAddedMapPoints.size();
while(lit!=mlpRecentAddedMapPoints.end())
{
MapPoint* pMP = *lit;
if(pMP->isBad())
lit = mlpRecentAddedMapPoints.erase(lit);
else if(pMP->GetFoundRatio()<0.25f)
{
pMP->SetBadFlag();
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)
{
pMP->SetBadFlag();
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
lit = mlpRecentAddedMapPoints.erase(lit);
else
{
lit++;
borrar--;
}
}
}