ORB-SLAM2 局部建图线程
转载自：
CSDN：https://blog.csdn.net/xiaoxiaowenqiang/article/details/79281122
Github：https://github.com/Ewenwan/MVision/blob/master/vSLAM/oRB_SLAM2/readme.md
/**
* This file is part of ORB-SLAM2.
*
* LocalMapping作用是将Tracking中送来的关键帧放在mlNewKeyFrame列表中；
* 处理新关键帧，地图点检查剔除，生成新地图点，Local BA，关键帧剔除。
* 主要工作在于维护局部地图，也就是SLAM中的Mapping。
*
*
* Tracking线程 只是判断当前帧是否需要加入关键帧，并没有真的加入地图，
* 因为Tracking线程的主要功能是局部定位，
*
* 而处理地图中的关键帧，地图点，包括如何加入，
* 如何删除的工作是在LocalMapping线程完成的
*
* 建图
* 处理新的关键帧 KeyFrame 完成局部地图构建
* 插入关键帧 ------> 处理地图点(筛选生成的地图点 生成地图点) --------> 局部 BA最小化重投影误差 -调整--------> 筛选 新插入的 关键帧
*
* mlNewKeyFrames list 列表队列存储关键帧
* 1】检查队列
* CheckNewKeyFrames();
*
* 2】处理新关键帧 Proces New Key Frames
* ProcessNewKeyFrame(); 更新地图点MapPoints 和 关键帧 KepFrame 的关联关系 UpdateConnections() 更新关联关系
*
* 3】剔除 地图点 MapPoints
* 删除地图中新添加的但 质量不好的 地图点
* a) IncreaseFound 共视点 / IncreaseVisible 投影在图像上 < 25%
* b) 观测到该 点的关键帧太少
*
* 4】生成 地图点 MapPoints
* 运动过程中和共视程度比较高的 关键帧 通过三角化 恢复出的一些地图点
*
* 5】地图点融合 MapPoints
* 检测当前关键帧和相邻 关键帧(两级相邻) 中 重复的 地图点 留下观测帧高的 地图点
*
* 6】局部 BA 最小化重投影误差
* 和当前关键帧相邻的关键帧 中相匹配的 地图点对 局部 BA最小化重投影误差优化点坐标 和 位姿
*
* 7】关键帧剔除
* 其90%以上的 地图点 能够被其他 共视 关键帧(至少3个) 观测到，认为该关键帧多余，可以删除
*
*/
#include "LocalMapping.h"
#include "LoopClosing.h"
#include "ORBmatcher.h"
#include "Optimizer.h"
#include
if(ratioDist * ratioFactor<ratioOctave || ratioDist > ratioOctave*ratioFactor)
continue;
// 步骤6.9：三角化生成3D点成功，构造成地图点 MapPoint
// Triangulation is succesfull
MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);
// 步骤6.9：为该MapPoint添加属性：
// a.观测到该MapPoint的关键帧
pMP->AddObservation(mpCurrentKeyFrame,idx1); // 地图点 添加观测帧
pMP->AddObservation(pKF2,idx2);//
mpCurrentKeyFrame->AddMapPoint(pMP,idx1);// 关键帧 添加地图点
pKF2->AddMapPoint(pMP,idx2);
// b.该MapPoint的描述子
pMP->ComputeDistinctiveDescriptors();
// c.该MapPoint的平均观测方向和深度范围
pMP->UpdateNormalAndDepth();
// d.地图添加地图点
mpMap->AddMapPoint(pMP);
// 步骤6.10：将新产生的点放入检测队列 mlpRecentAddedMapPoints
// 这些MapPoints都会经过MapPointCulling函数的检验
mlpRecentAddedMapPoints.push_back(pMP);
nnew++;
}
}
}
/**
* @brief 检查并融合 当前关键帧 与 相邻帧（一级二级相邻帧）重复的地图点 MapPoints
* 步骤1：获得当前关键帧在covisibility帧连接图中权重排名前nn的一级邻接关键帧(按观测到当前帧地图点次数选取)
* 步骤2：获得当前关键帧在 其一级相邻帧 在 covisibility图中权重排名前5 的二级邻接关键帧
* 步骤3：将当前帧的 地图点MapPoints 分别与 其一级二级相邻帧的 地图点 MapPoints 进行融合(保留观测次数最高的)
* 步骤4：找到一级二级相邻帧所有的地图点MapPoints 与当前帧 的 地图点MapPoints 进行融合
* 步骤5：更新当前帧 地图点 MapPoints 的描述子，深度，观测主方向等属性
* 步骤5：更新当前 与其它帧的连接关系 ( 观测到互相的地图点的次数等信息 )
* @return 无
*/
void LocalMapping::SearchInNeighbors()
{
// Retrieve neighbor keyframes
// 步骤1：获得当前关键帧在covisibility图中权重排名前nn的一级邻接关键帧
// 找到当前帧一级相邻与二级相邻关键帧
int nn = 10;
if(mbMonocular)
nn=20;//单目 多找一些
// 一级相邻
const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
vector<KeyFrame*> vpTargetKFs;// 最后合格的一级二级相邻关键帧
// 遍历每一个 一级相邻帧
for(vector<KeyFrame*>::const_iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++)
{
KeyFrame* pKFi = *vit;// 一级相邻关键帧
if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)//坏帧 或者 已经加入过
continue;// 跳过
vpTargetKFs.push_back(pKFi);// 加入 最后合格的相邻关键帧
pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;// 已经做过相邻匹配 标记已经加入
// 步骤2：获得当前关键帧在 其一级相邻帧的 covisibility图中权重排名前5的二级邻接关键帧
// 二级相邻
// Extend to some second neighbors
const vector<KeyFrame*> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);
// 遍历每一个 二级相邻帧
for(vector<KeyFrame*>::const_iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++)
{
KeyFrame* pKFi2 = *vit2;// 二级相邻关键帧
if(pKFi2->isBad() || pKFi2->mnFuseTargetForKF==mpCurrentKeyFrame->mnId || pKFi2->mnId==mpCurrentKeyFrame->mnId)
continue;// 二级相邻关键帧是坏帧 在一级时已经加入 或者 又找回来了找到当前帧了 跳过
vpTargetKFs.push_back(pKFi2);
}
}
// 步骤3：将当前帧的 地图点MapPoints 分别与 其一级二级相邻帧的 地图点 MapPoints 进行融合
// Search matches by projection from current KF in target KFs
ORBmatcher matcher;
vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();//与当前帧 匹配的地图点
// vector::iterator
for(auto vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++)
{
KeyFrame* pKFi = *vit;//其一级二级相邻帧
// 投影当前帧的MapPoints到相邻关键帧pKFi中，在附加区域搜索匹配关键点，并判断是否有重复的MapPoints
// 1.如果MapPoint能匹配关键帧的特征点，并且该点有对应的MapPoint，那么将两个MapPoint合并（选择观测数多的）
// 2.如果MapPoint能匹配关键帧的特征点，并且该点没有对应的MapPoint，那么为该点添加MapPoint
matcher.Fuse(pKFi,vpMapPointMatches);
}
// 步骤4：将一级二级相邻帧所有的地图点MapPoints 与当前帧（的MapPoints）进行融合
// 遍历每一个一级邻接和二级邻接关键帧 找到所有的地图点
// Search matches by projection from target KFs in current KF
// 用于存储一级邻接和二级邻接关键帧所有MapPoints的集合
vector<MapPoint*> vpFuseCandidates;// 一级二级相邻帧所有地图点
vpFuseCandidates.reserve(vpTargetKFs.size() * vpMapPointMatches.size());// 帧数量 × 每一帧地图点数量
// vector::iterator
for(auto vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++)
{
KeyFrame* pKFi = *vitKF;//其一级二级相邻帧
vector<MapPoint*> vpMapPointsKFi = pKFi->GetMapPointMatches();//地图点
// vector::iterator
for(auto vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++)
{
MapPoint* pMP = *vitMP;// 一级二级相邻帧 的每一个地图点
if(!pMP)
continue;
if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)
continue;
// 加入集合，并标记 已经加入
pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId; //标记 已经加
vpFuseCandidates.push_back(pMP); // 加入 一级二级相邻帧 地图点 集合
}
}
//一级二级相邻帧 所有的 地图点 与当前帧 融合
// 投影 地图点MapPoints到当前帧上，在附加区域搜索匹配关键点，并判断是否有重复的地图点
// 1.如果MapPoint能匹配当前帧的特征点，并且该点有对应的MapPoint，那么将两个MapPoint合并（选择观测数多的）
// 2.如果MapPoint能匹配当前帧的特征点，并且该点没有对应的MapPoint，那么为该点添加MapPoint
matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates);
// 步骤5：更新当前帧MapPoints的描述子，深度，观测主方向等属性
// Update points
vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();//当前帧 所有的 匹配地图点
for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++)
{
MapPoint* pMP=vpMapPointMatches[i];//当前帧 每个关键点匹配的地图点
if(pMP)//存在
{
if(!pMP->isBad())//非 坏点
{
pMP->ComputeDistinctiveDescriptors();// 更新 地图点的描述子(在所有观测在的描述子中选出最好的描述子)
pMP->UpdateNormalAndDepth(); // 更新平均观测方向和观测距离
}
}
}
// 步骤5：更新当前帧的MapPoints后 更新与其它帧的连接关系 观测到互相的地图点的次数等信息
// 更新covisibility图
// Update connections in covisibility graph
mpCurrentKeyFrame->UpdateConnections();
}
/**
* @brief 关键帧剔除
* 在Covisibility Graph 关键帧连接图 中的关键帧，
* 其90%以上的地图点MapPoints能被其他关键帧（至少3个）观测到，
* 则认为该关键帧为冗余关键帧。
* @param pKF1 关键帧1
* @param pKF2 关键帧2
* @return 两个关键帧之间的基本矩阵 F
*/
void LocalMapping::KeyFrameCulling()
{
// Check redundant keyframes (only local keyframes)
// A keyframe is considered redundant if the 90% of the MapPoints it sees, are seen
// in at least other 3 keyframes (in the same or finer scale)
// We only consider close stereo points
// 步骤1：根据Covisibility Graph 关键帧连接 图提取当前帧的 所有共视关键帧(关联帧)
vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();
// vector::iterator
// 对所有的局部关键帧进行遍历
for(auto vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++)
{
KeyFrame* pKF = *vit;// 当前帧的每一个 局部关联帧
if(pKF->mnId == 0)//第一帧关键帧为 初始化世界关键帧 跳过
continue;
// 步骤2：提取每个共视关键帧的 地图点 MapPoints
const vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();// 局部关联帧 匹配的 地图点
int nObs = 3;
const int thObs=nObs; //3
int nRedundantObservations=0;
int nMPs=0;
// 步骤3：遍历该局部关键帧的MapPoints，判断是否90%以上的MapPoints能被其它关键帧（至少3个）观测到
for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
{
MapPoint* pMP = vpMapPoints[i];// 该局部关键帧的 地图点 MapPoints
if(pMP)
{
if(!pMP->isBad())
{
if(!mbMonocular)// 双目/深度
{ // 对于双目，仅考虑近处的MapPoints，不超过mbf * 35 / fx
if(pKF->mvDepth[i] > pKF->mThDepth || pKF->mvDepth[i] < 0)
continue;
}
nMPs++;
// 地图点 MapPoints 至少被三个关键帧观测到
if(pMP->Observations() > thObs)// 观测帧个数 > 3
{
const int &scaleLevel = pKF->mvKeysUn[i].octave;// 金字塔层数
const map<KeyFrame*, size_t> observations = pMP->GetObservations();// 局部 观测关键帧地图
int nObs=0;
for(map<KeyFrame*, size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
KeyFrame* pKFi = mit->first;
if(pKFi==pKF)// 跳过 原地图点的帧
continue;
const int &scaleLeveli = pKFi->mvKeysUn[mit->second].octave;// 金字塔层数
// 尺度约束，要求MapPoint在该局部关键帧的特征尺度大于（或近似于）其它关键帧的特征尺度
if(scaleLeveli <= scaleLevel+1)
{
nObs++;
if(nObs >= thObs)
break;
}
}
if(nObs >= thObs)
{// 该MapPoint至少被三个关键帧观测到
nRedundantObservations++;
}
}
}
}
}
// 步骤4：该局部关键帧90%以上的MapPoints能被其它关键帧（至少3个）观测到，则认为是冗余关键帧
if(nRedundantObservations > 0.9*nMPs)
pKF->SetBadFlag();
}
}
/**
* @brief 根据两关键帧的姿态计算两个关键帧之间的基本矩阵
* F = inv(K1 转置)*E*inv(K2) = inv(K1 转置)*t叉乘R*inv(K2)
* @param pKF1 关键帧1
* @param pKF2 关键帧2
* @return 两个关键帧之间的基本矩阵 F
*/
cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2)
{
// Essential Matrix: t12叉乘R12
// Fundamental Matrix: inv(K1转置)*E*inv(K2)
cv::Mat R1w = pKF1->GetRotation(); // Rc1w
cv::Mat t1w = pKF1->GetTranslation();
cv::Mat R2w = pKF2->GetRotation(); // Rc2w
cv::Mat t2w = pKF2->GetTranslation();// t c2 w
cv::Mat R12 = R1w*R2w.t();// R12 =Rc1w * Rwc2 // c2 -->w --->c1
cv::Mat t12 = -R12*t2w + t1w; // tw2 + t1w // c2 -->w --->c1
// t12 的叉乘矩阵
cv::Mat t12x = SkewSymmetricMatrix(t12);
const cv::Mat &K1 = pKF1->mK;
const cv::Mat &K2 = pKF2->mK;
return K1.t().inv()*t12x*R12*K2.inv();
}
/**
* @brief 请求停止局部建图线程 设置停止标志
* @return 无
*/
void LocalMapping::RequestStop()
{
unique_lock<mutex> lock(mMutexStop);
mbStopRequested = true;//局部建图 请求停止
unique_lock<mutex> lock2(mMutexNewKFs);
mbAbortBA = true;//停止BA 优化
}
/**
* @brief 停止局部建图线程 设置停止标志
* @return 无
*/
bool LocalMapping::Stop()
{
unique_lock<mutex> lock(mMutexStop);
if(mbStopRequested && !mbNotStop)
{
mbStopped = true;
cout << "局部建图停止 Local Mapping STOP" << endl;
return true;
}
return false;
}
/**
* @brief 检查局部建图线程 是否停止
* @return 是否停止标志
*/
bool LocalMapping::isStopped()
{
unique_lock<mutex> lock(mMutexStop);
return mbStopped;
}
/**
* @brief 返回 请求停止局部建图线程 标志
* @return 返回 请求停止局部建图线程 标志
*/
bool LocalMapping::stopRequested()
{
unique_lock<mutex> lock(mMutexStop);
return mbStopRequested;
}
/**
* @brief 释放局部建图线程
* @return 无
*/
void LocalMapping::Release()
{
unique_lock<mutex> lock(mMutexStop);
unique_lock<mutex> lock2(mMutexFinish);
if(mbFinished)
return;
mbStopped = false;
mbStopRequested = false;
// list::iterator
for(auto lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++)
delete *lit;//删除关键帧
mlNewKeyFrames.clear();
cout << "局部建图释放 Local Mapping RELEASE" << endl;
}
/**
* @brief 返回 可以接收新的一个关键帧标志
* @return 是否 可以接收新的一个关键帧
*/
bool LocalMapping::AcceptKeyFrames()
{
unique_lock<mutex> lock(mMutexAccept);
return mbAcceptKeyFrames;
}
/**
* @brief 设置 可以接收新的一个关键帧标志
* @return 无
*/
void LocalMapping::SetAcceptKeyFrames(bool flag)
{
unique_lock<mutex> lock(mMutexAccept);
mbAcceptKeyFrames=flag;
}
/**
* @brief 设置不要停止标志
* @return 成功与否
*/
bool LocalMapping::SetNotStop(bool flag)
{
unique_lock<mutex> lock(mMutexStop);
if(flag && mbStopped)// 在已经停止的情况下 设置不要停止 错误
return false;
mbNotStop = flag;
return true;
}
/**
* @brief 停止 全局优化 BA
* @return 是否 可以接收新的一个关键帧
*/
void LocalMapping::InterruptBA()
{
mbAbortBA = true;
}
/**
* @brief 计算向量的 叉乘矩阵 变叉乘为 矩阵乘
* @return 该向量的叉乘矩阵
*/
cv::Mat LocalMapping::SkewSymmetricMatrix(const cv::Mat &v)
{
// 向量 t=(a1 a2 a3) t叉乘A
// 等于 向量t的叉乘矩阵 * A
// t的叉乘矩阵
//|0 -a3 a2 |
//|a3 0 -a1|
//|-a2 a1 0 |
return (cv::Mat_<float>(3,3) << 0, -v.at<float>(2), v.at<float>(1),
v.at<float>(2), 0,-v.at<float>(0),
-v.at<float>(1), v.at<float>(0), 0);
}
/**
* @brief 请求重置
* @return 无
*/
void LocalMapping::RequestReset()
{
{
unique_lock<mutex> lock(mMutexReset);
mbResetRequested = true;
}
while(1)
{
{
unique_lock<mutex> lock2(mMutexReset);
if(!mbResetRequested)
break;
}
usleep(3000);
}
}
/**
* @brief 重置线程
* @return 无
*/
void LocalMapping::ResetIfRequested()
{
unique_lock<mutex> lock(mMutexReset);
if(mbResetRequested)
{
mlNewKeyFrames.clear();
mlpRecentAddedMapPoints.clear();
mbResetRequested=false;
}
}
void LocalMapping::RequestFinish()
{
unique_lock<mutex> lock(mMutexFinish);
mbFinishRequested = true;
}
bool LocalMapping::CheckFinish()
{
unique_lock<mutex> lock(mMutexFinish);
return mbFinishRequested;
}
void LocalMapping::SetFinish()
{
unique_lock<mutex> lock(mMutexFinish);
mbFinished = true;
unique_lock<mutex> lock2(mMutexStop);
mbStopped = true;
}
bool LocalMapping::isFinished()
{
unique_lock<mutex> lock(mMutexFinish);
return mbFinished;
}
} //namespace ORB_SLAM