orb-slam3单目LocalMapping代码简介
·
在SYSTEM类中,开了新线程,一直调用LocalMapping中的Run函数
mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run,mpLocalMapper);
第一步,用InsertKeyFrame往mlploopkeyframequeue队列中插入pKF
void LoopClosing::InsertKeyFrame(KeyFrame *pKF)
{
unique_lock<mutex> lock(mMutexLoopQueue);
if(pKF->mnId!=0)
mlpLoopKeyFrameQueue.push_back(pKF);
}
存在需要处理后的KF后,调用ProcessNewKeyFrame(),主要作用是更新地图点的观测数据,地图点记录关键帧对其的观测,并更新帧之间的共视关系,将关键帧插入地图中
void LocalMapping::ProcessNewKeyFrame()
{
{
unique_lock<mutex> lock(mMutexNewKFs);
mpCurrentKeyFrame = mlNewKeyFrames.front();
mlNewKeyFrames.pop_front();
}
// Compute Bags of Words structures
//计算当前关键帧的词袋模型。1:计算某一描述子的位置。2.压缩整张图片的表示
mpCurrentKeyFrame->ComputeBoW();
// Associate MapPoints to the new keyframe and update normal and descriptor
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);
}
}
}
}
// Update links in the Covisibility Graph
mpCurrentKeyFrame->UpdateConnections();
// Insert Keyframe in Map
mpAtlas->AddKeyFrame(mpCurrentKeyFrame);
}
MapPointCulling:对地图点进行处理,删除不满足要求的地图点
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--;
}
}
}
CreateNewMapPoints()检查相机基线,如果视差明显,将ORB提取的两关键帧进行匹配,通过对体几何实现2D to 3D,来创建新的地图点
SearchInNeighbors(),用于更新共视图何地图点的融合和修复
LocalBundleAdjustment:使用g2o来优化关键帧的位资和地图点的坐标
KeyFrameCulling()遍历邻居关键帧,通过地图点来判断是否为冗余关键帧(一个关键帧上的90%地图点能被其他多个关键帧观测到)
ResetIfRequested();判断系统是否需要重置
更多推荐


所有评论(0)