算法解析
关键帧用于减少BA过程的计算量
代表了相机的“关键”运动
主要属性：位姿、内参、特征点数组以及
特征点是否有关联的地图点
“宽进严出”策略
源码解读KeyFrame.cc
#include "KeyFrame.h"
#include "Converter.h"
#include "ORBmatcher.h"
#include <mutex>
#include "MapPoint.h"
#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
#include "ORBVocabulary.h"
#include "ORBextractor.h"
#include "Frame.h"
#include "KeyFrameDatabase.h"
namespace ORB_SLAM2
{
// 下一个关键帧的id
// nNextID名字改为nLastID更合适，表示上一个KeyFrame的ID号
// static long unsigned int nNextId;
long unsigned int KeyFrame::nNextId=0;
/**
* class KeyFrame 关键帧类
* 关键帧，和普通的Frame不一样，但是可以由Frame来构造; 许多数据会被三个线程同时访问，所以用锁的地方很普遍
* 关键帧的构造函数
* @param F 父类普通帧的对象
* @param pMap 所属的地图指针
* @param pKFDB 使用的词袋模型的指针
* 初始化的参数列表先不看
* mHalfBaseline(F.mb/2),计算双目相机长度的一半
*
* 每个KeyFrame基本属性是它是一个Frame，KeyFrame初始化的时候需要Frame，
* mnFrameId记录了该KeyFrame是由哪个Frame初始化的
* const long unsigned int mnFrameId;
*
* 既然这么多和Frame重复的地方,为什么不选择使用继承呢?
* Grid (to speed up feature matching)
* 和Frame类中的定义相同
* const int mnGridCols;
*
* Variables used by the tracking
* long unsigned int mnTrackReferenceForFrame; 记录它
*
* ong unsigned int mnFuseTargetForKF; 标记在局部建图线程中,和哪个关键帧进行融合的操作
*
* Variables used by the local mapping
* local mapping中记录当前处理的关键帧的mnId，表示当前局部BA的关键帧id。mnBALocalForKF 在map point.h里面也有同名的变量。
* long unsigned int mnBALocalForKF;
*
* local mapping中记录当前处理的关键帧的mnId, 只是提供约束信息但是却不会去优化这个关键帧
* long unsigned int mnBAFixedForKF;
*
* Variables used by the keyframe database 下面的这些变量都是临时的,由外部调用暂时存放一些数据
* 标记了当前关键帧是id为mnLoopQuery的回环检测的候选关键帧
* long unsigned int mnLoopQuery;
*
* 当前关键帧和这个形成回环的候选关键帧中,具有相同word的个数
* int mnLoopWords;
*
* 和上面的变量作用差不多，不过这个变量是用来存储在辅助进行重定位的时候，要进行重定位的那个帧的id
* long unsigned int mnRelocQuery;
*
* 和那个要进行重定位的帧,所具有相同的单词的个数
* int mnRelocWords;
*
* Variables used by loop closing
* 经过全局BA优化后的相机的位姿
* cv::Mat mTcwGBA;
* 进行全局BA优化之前的当前关键帧的位姿. 之所以要记录这个是因为在全局优化之后还要根据该关键帧在优化之前的位姿来更新地图点,which地图点的参考关键帧就是该关键帧
* cv::Mat mTcwBefGBA;
* 记录是由于哪个"当前关键帧"触发的全局BA,用来防止重复写入的事情发生(浪费时间)
* long unsigned int mnBAGlobalForKF;
*
* KeyPoints, stereo coordinate and descriptors (all associated by an index)
* 和Frame类中的定义相同
* const std::vector<cv::KeyPoint> mvKeys;
*
* const std::vector<float> mvuRight; // negative value for monocular points
* const std::vector<float> mvDepth; // negative value for monocular points
*
*
* //BoW
* DBoW2::BowVector mBowVec; ///< Vector of words to represent images 当前图像的词袋模型表示
* DBoW2::FeatureVector mFeatVec; ///< Vector of nodes with indexes of local features //?
* Pose relative to parent (this is computed when bad flag is activated)
* cv::Mat mTcp;
*
* Scale
* const int mnScaleLevels;
* const float mfScaleFactor;
* const float mfLogScaleFactor;
* const std::vector<float> mvScaleFactors;// 尺度因子，scale^n，scale=1.2，n为层数
* const std::vector<float> mvLevelSigma2;// 尺度因子的平方
* const std::vector<float> mvInvLevelSigma2;
*
* mage bounds and calibration
* const int mnMinX;
* const int mnMinY;
* const int mnMaxX;
* const int mnMaxY;
* const cv::Mat mK;
*
* The following variables need to be accessed trough a mutex to be thread safe.
* SE3 Pose and camera center
cv::Mat Tcw; ///< 当前相机的位姿
cv::Mat Twc; ///< 当前相机位姿的逆
cv::Mat Ow; ///< 相机光心(左目)在世界坐标系下的坐标,这里和普通帧中的定义是一样的
cv::Mat Cw; ///< Stereo middel point. Only for visualization
std::vector<MapPoint*> mvpMapPoints; MapPoints associated to keypoints
KeyFrameDatabase* mpKeyFrameDB; // BoW
ORBVocabulary* mpORBvocabulary; /// 词袋对象,目测是封装了很多操作啊
// Grid over the image to speed up feature matching ,其实应该说是二维的,第三维的 vector中保存的是这个网格内的特征点的索引
std::vector< std::vector <std::vector<size_t> > > mGrid;
// Covisibility Graph
std::map<KeyFrame*,int> mConnectedKeyFrameWeights; 与该关键帧连接的关键帧与权重
std::vector<KeyFrame*> mvpOrderedConnectedKeyFrames; 排序后的关键帧,和下面的这个变量相对应
std::vector<int> mvOrderedWeights; 排序后的权重(从大到小)
// ===================== Spanning Tree and Loop Edges ========================
// std::set是集合，相比vector，进行插入数据这样的操作时会自动排序
bool mbFirstConnection; ///< 是否是第一次生成树
KeyFrame* mpParent; ///< 当前关键帧的父关键帧
std::set<KeyFrame*> mspChildrens; ///< 存储当前关键帧的子关键帧
std::set<KeyFrame*> mspLoopEdges; ///< 和当前关键帧形成回环关系的关键帧
// Bad flags
bool mbNotErase;当前关键帧已经和其他的关键帧形成了回环关系，因此在各种优化的过程中不应该被删除
bool mbToBeErased;
bool mbBad;
float mHalfBaseline; 对于双目相机来说,双目相机基线长度的一半. Only for visualization
Map* mpMap;
std::mutex mMutexPose; 在对位姿进行操作时相关的互斥锁
std::mutex mMutexConnections; 在操作当前关键帧和其他关键帧的公式关系的时候使用到的互斥锁
std::mutex mMutexFeatures; 在操作和特征点有关的变量的时候的互斥锁
*
*/
KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB):
mnFrameId(F.mnId), mTimeStamp(F.mTimeStamp), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS),
mfGridElementWidthInv(F.mfGridElementWidthInv), mfGridElementHeightInv(F.mfGridElementHeightInv),
mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0),
mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0),
fx(F.fx), fy(F.fy), cx(F.cx), cy(F.cy), invfx(F.invfx), invfy(F.invfy),
mbf(F.mbf), mb(F.mb), mThDepth(F.mThDepth), N(F.N), mvKeys(F.mvKeys), mvKeysUn(F.mvKeysUn),
mvuRight(F.mvuRight), mvDepth(F.mvDepth), mDescriptors(F.mDescriptors.clone()),
mBowVec(F.mBowVec), mFeatVec(F.mFeatVec), mnScaleLevels(F.mnScaleLevels), mfScaleFactor(F.mfScaleFactor),
mfLogScaleFactor(F.mfLogScaleFactor), mvScaleFactors(F.mvScaleFactors), mvLevelSigma2(F.mvLevelSigma2),
mvInvLevelSigma2(F.mvInvLevelSigma2), mnMinX(F.mnMinX), mnMinY(F.mnMinY), mnMaxX(F.mnMaxX),
mnMaxY(F.mnMaxY), mK(F.mK), mvpMapPoints(F.mvpMapPoints), mpKeyFrameDB(pKFDB),
mpORBvocabulary(F.mpORBvocabulary), mbFirstConnection(true), mpParent(NULL), mbNotErase(false),
mbToBeErased(false), mbBad(false),
mHalfBaseline(F.mb/2),
mpMap(pMap)
{
// 在nNextID的基础上加1就得到了mnID，为当前KeyFrame的ID号
// long unsigned int mnId;
mnId=nNextId++; // 获取id
// 根据指定的普通帧, 初始化用于加速匹配的网格对象信息; 其实就把每个网格中有的特征点的索引如数复制过来
mGrid.resize(mnGridCols);
for(int i=0; i<mnGridCols;i++)
{
mGrid[i].resize(mnGridRows);
for(int j=0; j<mnGridRows; j++)
mGrid[i][j] = F.mGrid[i][j];
}
// 设置当前关键帧的位姿
SetPose(F.mTcw);
}
/**
* @brief Bag of Words Representation 计算词袋表示
* @detials 计算mBowVec，并且将描述子分散在第4层上，即mFeatVec记录了属于第i个node的ni个描述子
* @see ProcessNewKeyFrame()
*/
void KeyFrame::ComputeBoW()
{
// 只有当词袋向量或者节点和特征序号的特征向量为空的时候执行
if(mBowVec.empty() || mFeatVec.empty())
{
// 那么就从当前帧的描述子中转换得到词袋信息
vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
// Feature vector associate features with nodes in the 4th level (from leaves up)
// We assume the vocabulary tree has 6 levels, change the 4 otherwise //?
mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4);
}
}
/**
* @brief Pose functions 这里的set,get需要用到锁 设置当前关键帧的位姿
* @param Tcw_ 位姿
*/
void KeyFrame::SetPose(const cv::Mat &Tcw_){
unique_lock<mutex> lock(mMutexPose);
Tcw_.copyTo(Tcw);
cv::Mat Rcw = Tcw.rowRange(0,3).colRange(0,3);
cv::Mat tcw = Tcw.rowRange(0,3).col(3);
cv::Mat Rwc = Rcw.t();
// 和普通帧中进行的操作相同
Ow = -Rwc*tcw;
// 计算当前位姿的逆
Twc = cv::Mat::eye(4,4,Tcw.type());
Rwc.copyTo(Twc.rowRange(0,3).colRange(0,3));
Ow.copyTo(Twc.rowRange(0,3).col(3));
// center为相机坐标系（左目）下，立体相机中心的坐标
// 立体相机中心点坐标与左目相机坐标之间只是在x轴上相差mHalfBaseline,
// 因此可以看出，立体相机中两个摄像头的连线为x轴，正方向为左目相机指向右目相机 (齐次坐标)
cv::Mat center = (cv::Mat_<float>(4,1) << mHalfBaseline, 0 , 0, 1);
// 世界坐标系下，左目相机中心到立体相机中心的向量，方向由左目相机指向立体相机中心
Cw = Twc*center;
}
// 获取位姿
cv::Mat KeyFrame::GetPose()
{
unique_lock<mutex> lock(mMutexPose);
return Tcw.clone();
}
// 获取位姿的逆
cv::Mat KeyFrame::GetPoseInverse()
{
unique_lock<mutex> lock(mMutexPose);
return Twc.clone();
}
// 获取(左目)相机的中心
cv::Mat KeyFrame::GetCameraCenter()
{
unique_lock<mutex> lock(mMutexPose);
return Ow.clone();
}
// 获取双目相机的中心,这个只有在可视化的时候才会用到
cv::Mat KeyFrame::GetStereoCenter()
{
unique_lock<mutex> lock(mMutexPose);
return Cw.clone();
}
// 获取姿态
cv::Mat KeyFrame::GetRotation()
{
unique_lock<mutex> lock(mMutexPose);
return Tcw.rowRange(0,3).colRange(0,3).clone();
}
// 获取平移向量也就是位置
cv::Mat KeyFrame::GetTranslation()
{
unique_lock<mutex> lock(mMutexPose);
return Tcw.rowRange(0,3).col(3).clone();
}
/**
* Covisibility graph functions
* 更新了mConnectedKeyFrameWeights 为关键帧之间添加连接
* @param pKF 关键帧
* @param weight 权重，该关键帧与pKF共同观测到的3d点数量
*/
void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight)
{
{
// 如果被占用就一直等着,这个添加连接的操作不能够被放弃
unique_lock<mutex> lock(mMutexConnections);
// 下面操作的目的是,判断当前关键帧是否已经和其他的关键帧创建了联系; 但是看下面的逻辑,其实一句 mConnectedKeyFrameWeights[pKF]=weight 就可以解决了啊
// std::map::count函数只可能返回0或1两种情况
if(!mConnectedKeyFrameWeights.count(pKF)) // count函数返回0，mConnectedKeyFrameWeights中没有pKF，之前没有连接
mConnectedKeyFrameWeights[pKF]=weight;
else if(mConnectedKeyFrameWeights[pKF]!=weight) // 之前连接的权重不一样
mConnectedKeyFrameWeights[pKF]=weight;
else
return;
}
// 如果添加了更新的连接关系就要更新一下,主要是重新进行排序
UpdateBestCovisibles();
}
/**
* @brief 按照权重对连接的关键帧进行排序
*
* 更新后的变量存储在mvpOrderedConnectedKeyFrames和mvOrderedWeights中
*/
void KeyFrame::UpdateBestCovisibles()
{
unique_lock<mutex> lock(mMutexConnections);
// http://stackoverflow.com/questions/3389648/difference-between-stdliststdpair-and-stdmap-in-c-stl (std::map 和 std::list<std::pair>的区别)
vector<pair<int,KeyFrame*> > vPairs;
vPairs.reserve(mConnectedKeyFrameWeights.size());
// 取出所有连接的关键帧，mConnectedKeyFrameWeights的类型为std::map<KeyFrame*,int>，而vPairs变量将共视的3D点数放在前面，利于排序
for(map<KeyFrame*,int>::iterator mit=mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
vPairs.push_back(make_pair(mit->second,mit->first));
// 按照权重进行排序
sort(vPairs.begin(),vPairs.end());
list<KeyFrame*> lKFs; // keyframe
list<int> lWs; // weight
for(size_t i=0, iend=vPairs.size(); i<iend;i++)
{
lKFs.push_front(vPairs[i].second);
lWs.push_front(vPairs[i].first);
}
// 权重从大到小
mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
}
/**
* 得到与该关键帧连接的关键帧(没有排序的)
*/
set<KeyFrame*> KeyFrame::GetConnectedKeyFrames()
{
unique_lock<mutex> lock(mMutexConnections);
set<KeyFrame*> s;
for(map<KeyFrame*,int>::iterator mit=mConnectedKeyFrameWeights.begin();mit!=mConnectedKeyFrameWeights.end();mit++)
s.insert(mit->first);
return s;
}
/**
* 得到与该关键帧连接的关键帧(已按权值排序)
* @return
*/
vector<KeyFrame*> KeyFrame::GetVectorCovisibleKeyFrames()
{
unique_lock<mutex> lock(mMutexConnections);
return mvpOrderedConnectedKeyFrames;
}
/**
* @brief 得到与该关键帧连接的前N个关键帧(已按权值排序)
* NOTICE 如果连接的关键帧少于N，则返回所有连接的关键帧,所以说返回的关键帧的数目其实不一定是N个
* @param N 前N个
* @return 连接的关键帧
*/
vector<KeyFrame*> KeyFrame::GetBestCovisibilityKeyFrames(const int &N)
{
unique_lock<mutex> lock(mMutexConnections);
if((int)mvpOrderedConnectedKeyFrames.size()<N)
// 如果不够达到的数目就直接吧现在所有的关键帧都返回了
return mvpOrderedConnectedKeyFrames;
else
return vector<KeyFrame*>(mvpOrderedConnectedKeyFrames.begin(),mvpOrderedConnectedKeyFrames.begin()+N);
}
/**
* @brief 得到与该关键帧连接的权重大于等于w的关键帧
* @param w 权重
* @return 连接的关键帧
*/
vector<KeyFrame*> KeyFrame::GetCovisiblesByWeight(const int &w)
{
unique_lock<mutex> lock(mMutexConnections);
// 如果没有和当前关键帧连接的关键帧
if(mvpOrderedConnectedKeyFrames.empty())
return vector<KeyFrame*>();
// http://www.cplusplus.com/reference/algorithm/upper_bound/
// 从mvOrderedWeights找出第一个大于w的那个迭代器
// 这里应该使用lower_bound，因为lower_bound是返回小于等于，而upper_bound只能返回第一个大于的
// ↑不对! lower_bound 是"大于等于", 并且注意这两个函数都是假设数组已经是从小到大排序
// 比较两个int型权重的大小的比较函数
// static bool weightComp( int a, int b)
// {
// return a>b;
// }
vector<int>::iterator it = upper_bound( mvOrderedWeights.begin(), //起点
mvOrderedWeights.end(), //终点
w, //目标阈值
KeyFrame::weightComp); //比较函数,由于我们这里从大到小排序,所以需要重定义比较函数
// 如果没有找到(最大的权重也比给定的阈值小)
if(it==mvOrderedWeights.end() && *mvOrderedWeights.rbegin()<w)
// 返回空
return vector<KeyFrame*>();
else
{
int n = it-mvOrderedWeights.begin();
return vector<KeyFrame*>(mvpOrderedConnectedKeyFrames.begin(), mvpOrderedConnectedKeyFrames.begin()+n);
}
}
/**
* @brief 得到该关键帧与pKF的权重
* @param pKF 关键帧
* @return 权重
*/
int KeyFrame::GetWeight(KeyFrame *pKF)
{
unique_lock<mutex> lock(mMutexConnections);
if(mConnectedKeyFrameWeights.count(pKF))
return mConnectedKeyFrameWeights[pKF];
else
// 没有连接的话权重也就是共视点个数就是0
return 0;
}
/**
* MapPoint observation functions
* @brief Add MapPoint to KeyFrame
* @param pMP MapPoint
* @param idx MapPoint在KeyFrame中的索引
*/
void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx)
{
unique_lock<mutex> lock(mMutexFeatures);
mvpMapPoints[idx]=pMP;
}
/**
* @brief 由于其他的原因,导致当前关键帧观测到的某个地图点被删除(bad==true)了,这里是"通知"当前关键帧这个地图点已经被删除了
* @param[in] idx 被删除的地图点索引
*/
void KeyFrame::EraseMapPointMatch(const size_t &idx)
{
unique_lock<mutex> lock(mMutexFeatures);
// NOTE 使用这种方式表示其中的某个地图点被删除
mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);
}
/**
* @brief 由于其他的原因,导致当前关键帧观测到的某个地图点被删除(bad==true)了,这里是"通知"当前关键帧这个地图点已经被删除了
* @param[in] pMP 被删除的地图点指针
*/
void KeyFrame::EraseMapPointMatch(MapPoint* pMP)
{
// 其实和上面函数的操作差不多,不过是先从指针获取到索引,然后再进行删除罢了
int idx = pMP->GetIndexInKeyFrame(this);
if(idx>=0)
mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);
}
/**
* @brief 地图点的替换
* @param[in] idx 要替换掉的地图点的索引
* @param[in] pMP 新地图点的指针
*/
void KeyFrame::ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP)
{
mvpMapPoints[idx]=pMP;
}
/**
* @brief 获取当前帧中的所有地图点
* @return std::set<MapPoint*> 所有的地图点
*/
set<MapPoint*> KeyFrame::GetMapPoints()
{
unique_lock<mutex> lock(mMutexFeatures);
set<MapPoint*> s;
for(size_t i=0, iend=mvpMapPoints.size(); i<iend; i++)
{
// 判断是否被删除了
if(!mvpMapPoints[i])
continue;
MapPoint* pMP = mvpMapPoints[i];
// 如果是没有来得及删除的坏点也要进行这一步
if(!pMP->isBad())
s.insert(pMP);
}
return s;
}
/**
* 关键帧中，大于等于最少观测数目minObs的MapPoints的数量.这些特征点被认为追踪到了
* @brief 关键帧中，大于等于minObs的MapPoints的数量
* @details minObs就是一个阈值，大于minObs就表示该MapPoint是一个高质量的MapPoint \n
* 一个高质量的MapPoint会被多个KeyFrame观测到.
* @param minObs 最小观测
*/
int KeyFrame::TrackedMapPoints(const int &minObs)
{
unique_lock<mutex> lock(mMutexFeatures);
int nPoints=0;
const bool bCheckObs = minObs>0;
// N是当前帧中特征点的个数
for(int i=0; i<N; i++)
{
MapPoint* pMP = mvpMapPoints[i];
if(pMP) //没有被删除
{
if(!pMP->isBad()) //并且不是坏点
{
// NOTICE 奇怪,那为什么不直接和0比呢?
if(bCheckObs)
{
// 该MapPoint是一个高质量的MapPoint
if(mvpMapPoints[i]->Observations()>=minObs)
nPoints++;
}
else
nPoints++;
}
}
}
return nPoints;
}
/**
* 获取当前关键帧的具体的地图点
* @brief Get MapPoint Matches 获取该关键帧的MapPoints
*/
vector<MapPoint*> KeyFrame::GetMapPointMatches()
{
unique_lock<mutex> lock(mMutexFeatures);
return mvpMapPoints;
}
/**
* @brief 获取获取当前关键帧的具体的某个地图点
* @param[in] idx id
* @return MapPoint* 地图点句柄
*/
MapPoint* KeyFrame::GetMapPoint(const size_t &idx)
{
unique_lock<mutex> lock(mMutexFeatures);
return mvpMapPoints[idx];
}
/*
* 更新图的连接
*
* 1. 首先获得该关键帧的所有MapPoint点，统计观测到这些3d点的每个关键帧与其它所有关键帧之间的共视程度
* 对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数。
* 2. 并且该权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高）
* 3. 对这些连接按照权重从大到小进行排序，以方便将来的处理
* 更新完covisibility图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树
*/
void KeyFrame::UpdateConnections()
{
// 在没有执行这个函数前，关键帧只和MapPoints之间有连接关系，这个函数可以更新关键帧之间的连接关系
//===============1==================================
map<KeyFrame*,int> KFcounter; // 关键帧-权重，权重为其它关键帧与当前关键帧共视3d点的个数
vector<MapPoint*> vpMP;
{
// 获得该关键帧的所有3D点
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
// 通过3D点间接统计可以观测到这些3D点的所有关键帧之间的共视程度
// 即统计每一个地图点都有多少关键帧与当前关键帧存在共视关系，统计结果放在KFcounter
for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
if(!pMP)
continue;
if(pMP->isBad())
continue;
// 对于每一个MapPoint点，observations记录了可以观测到该MapPoint的所有关键帧
map<KeyFrame*,size_t> observations = pMP->GetObservations();
for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
// 除去自身，自己与自己不算共视
if(mit->first->mnId==mnId)
continue;
KFcounter[mit->first]++;
// 所以这里最后得到的是当前关键帧和其他关键帧的共视强度,这个转换有点意思
}
}
// This should not happen 这里为了友好一下就没有使用断言
if(KFcounter.empty())
return;
//===============2==================================
// 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;
// vPairs记录与其它关键帧共视帧数大于th的关键帧
// pair<int,KeyFrame*>将关键帧的权重写在前面，关键帧写在后面方便后面排序
vector<pair<int,KeyFrame*> > vPairs;
vPairs.reserve(KFcounter.size());
// 对于一个和当前关键帧具有共视关系的关键帧
for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
{
// 更新具有最佳共视关系的关键帧信息
if(mit->second>nmax)
{
nmax=mit->second;
// 找到对应权重最大的关键帧（共视程度最高的关键帧）
pKFmax=mit->first;
}
if(mit->second>=th)
{
// 对应权重需要大于阈值，对这些关键帧建立连接
vPairs.push_back(make_pair(mit->second,mit->first));
// 对方关键帧也要添加这个信息
// 更新KFcounter中该关键帧的mConnectedKeyFrameWeights
// 更新其它KeyFrame的mConnectedKeyFrameWeights，更新其它关键帧与当前帧的连接权重
(mit->first)->AddConnection(this,mit->second);
}
}
// 如果没有超过阈值的权重，则对权重最大的关键帧建立连接
if(vPairs.empty())
{
// 如果每个关键帧与它共视的关键帧的个数都少于th，
// 那就只更新与其它关键帧共视程度最高的关键帧的mConnectedKeyFrameWeights
// 这是对之前th这个阈值可能过高的一个补丁
vPairs.push_back(make_pair(nmax,pKFmax));
pKFmax->AddConnection(this,nmax);
}
// vPairs里存的都是相互共视程度比较高的关键帧和共视权重，接下来由大到小进行排序
sort(vPairs.begin(),vPairs.end()); // sort函数默认升序排列
// 将排序后的结果分别组织成为两种数据类型
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);
}
//===============3==================================
{
unique_lock<mutex> lockCon(mMutexConnections);
// mspConnectedKeyFrames = spConnectedKeyFrames;
// 更新图的连接(权重)
mConnectedKeyFrameWeights = KFcounter;//更新该KeyFrame的mConnectedKeyFrameWeights，更新当前帧与其它关键帧的连接权重
mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
// 更新生成树的连接
if(mbFirstConnection && mnId!=0)
{
// 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
mpParent = mvpOrderedConnectedKeyFrames.front();
// 建立双向连接关系
mpParent->AddChild(this);
mbFirstConnection = false;
}
}
}
/**
* Spanning tree functions
* @brief 添加子关键帧（即和子关键帧具有最大共视关系的关键帧就是当前关键帧）
* @param[in] pKF 子关键帧句柄
*/
void KeyFrame::AddChild(KeyFrame *pKF)
{
unique_lock<mutex> lockCon(mMutexConnections);
mspChildrens.insert(pKF);
}
/**
* Spanning tree functions
* @brief 删除某个子关键帧
* @param[in] pKF 子关键帧句柄
*/
void KeyFrame::EraseChild(KeyFrame *pKF)
{
unique_lock<mutex> lockCon(mMutexConnections);
mspChildrens.erase(pKF);
}
/**
* @brief 改变当前关键帧的父关键帧
* @param[in] pKF 父关键帧句柄
*/
void KeyFrame::ChangeParent(KeyFrame *pKF)
{
unique_lock<mutex> lockCon(mMutexConnections);
// 添加双向连接关系
mpParent = pKF;
pKF->AddChild(this);
}
/**
* @brief 获取获取当前关键帧的子关键帧
* @return std::set<KeyFrame*> 子关键帧集合
*/
set<KeyFrame*> KeyFrame::GetChilds()
{
unique_lock<mutex> lockCon(mMutexConnections);
return mspChildrens;
}
/**
* @brief 获取当前关键帧的父关键帧
* @return KeyFrame* 父关键帧句柄
*/
KeyFrame* KeyFrame::GetParent()
{
unique_lock<mutex> lockCon(mMutexConnections);
return mpParent;
}
/**
* @brief 判断某个关键帧是否是当前关键帧的子关键帧
* @param[in] pKF 关键帧句柄
* @return true
* @return false
*/
bool KeyFrame::hasChild(KeyFrame *pKF)
{
unique_lock<mutex> lockCon(mMutexConnections);
return mspChildrens.count(pKF);
}
/**
* @brief Loop Edges 给当前关键帧添加回环边，回环边连接了形成闭环关系的关键帧
* @param[in] pKF 和当前关键帧形成闭环关系的关键帧
*/
void KeyFrame::AddLoopEdge(KeyFrame *pKF)
{
unique_lock<mutex> lockCon(mMutexConnections);
mbNotErase = true;
mspLoopEdges.insert(pKF);
}
// 获取和当前关键帧形成闭环关系的关键帧
set<KeyFrame*> KeyFrame::GetLoopEdges()
{
unique_lock<mutex> lockCon(mMutexConnections);
return mspLoopEdges;
}
// Enable/Disable bad flag changes
// 设置当前关键帧不要在优化的过程中被删除 由回环检测线程调用
void KeyFrame::SetNotErase()
{
unique_lock<mutex> lock(mMutexConnections);
mbNotErase = true;
}
// 删除当前的这个关键帧,表示不进行回环检测过程;由回环检测线程调用
void KeyFrame::SetErase()
{
{
unique_lock<mutex> lock(mMutexConnections);
// 如果当前关键帧和其他的关键帧没有形成回环关系,那么就删吧
if(mspLoopEdges.empty())
{
mbNotErase = false;
}
}
// 这个地方是不是应该：(!mbToBeErased)，(wubo???)
// SetBadFlag函数就是将mbToBeErased置为true，mbToBeErased就表示该KeyFrame被擦除了
if(mbToBeErased)
{
SetBadFlag();
}
}
// 真正地执行删除关键帧的操作
void KeyFrame::SetBadFlag()
{
// 首先处理一下应该删除但是最后删除不了的特殊情况
{
unique_lock<mutex> lock(mMutexConnections);
// 第0关键帧不允许被删除
if(mnId==0)
return;
else if(mbNotErase)// mbNotErase表示不应该擦除该KeyFrame，于是把mbToBeErased置为true，表示已经擦除了，其实没有擦除
{
mbToBeErased = true;
return;
}
}
// 接下来是真正的要进行删除关键帧的操作了 ---- 感觉这里的操作也应该是在互斥锁的保护中进行啊 ---- 不是,这里操作的是其他的关键帧的成员变量
for(map<KeyFrame*,int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
mit->first->EraseConnection(this);// 让其它的KeyFrame删除与自己的联系
for(size_t i=0; i<mvpMapPoints.size(); i++)
if(mvpMapPoints[i])
mvpMapPoints[i]->EraseObservation(this);// 让与自己有联系的MapPoint删除与自己的联系
// 然后对当前关键帧成员变量的操作
{
unique_lock<mutex> lock(mMutexConnections);
unique_lock<mutex> lock1(mMutexFeatures);
//清空自己与其它关键帧之间的联系
mConnectedKeyFrameWeights.clear();
mvpOrderedConnectedKeyFrames.clear();
// Update Spanning Tree 主要是给子关键帧选择父关键帧
set<KeyFrame*> sParentCandidates;
sParentCandidates.insert(mpParent);
// Assign at each iteration one children with a parent (the pair with highest covisibility weight)
// Include that children as new parent candidate for the rest
// 如果这个关键帧有自己的孩子关键帧，告诉这些子关键帧，它们的父关键帧不行了，赶紧找新的父关键帧
while(!mspChildrens.empty())
{
bool bContinue = false;
int max = -1;
KeyFrame* pC;
KeyFrame* pP;
// 遍历每一个子关键帧，让它们更新它们指向的父关键帧
// ? 感觉这边的循环设计有问题呢, 这里是遍历每一个关键帧
for(set<KeyFrame*>::iterator sit=mspChildrens.begin(), send=mspChildrens.end(); sit!=send; sit++)
{
KeyFrame* pKF = *sit;
// 跳过不行了的子关键帧
if(pKF->isBad())
continue;
// Check if a parent candidate is connected to the keyframe
// 子关键帧遍历每一个与它相连的关键帧（共视关键帧）
vector<KeyFrame*> vpConnected = pKF->GetVectorCovisibleKeyFrames();
for(size_t i=0, iend=vpConnected.size(); i<iend; i++)
{
for(set<KeyFrame*>::iterator spcit=sParentCandidates.begin(), spcend=sParentCandidates.end(); spcit!=spcend; spcit++)
{
// 如果该帧的子节点和父节点（祖孙节点）之间存在连接关系（共视）
// 举例：B-->A（B的父节点是A） C-->B（C的父节点是B） D--C（D与C相连） E--C（E与C相连） F--C（F与C相连） D-->A（D的父节点是A） E-->A（E的父节点是A）
// 现在B挂了，于是C在与自己相连的D、E、F节点中找到父节点指向A的D
// 此过程就是为了找到可以替换B的那个节点。
// 上面例子中，B为当前要设置为SetBadFlag的关键帧
// A为spcit，也即sParentCandidates
// C为pKF,pC，也即mspChildrens中的一个
// D、E、F为vpConnected中的变量，由于C与D间的权重 比 C与E间的权重大，因此D为pP
if(vpConnected[i]->mnId == (*spcit)->mnId)
{
int w = pKF->GetWeight(vpConnected[i]);
// 寻找并更新权值最大的那个共视关系
if(w>max)
{
pC = pKF; //子关键帧
pP = vpConnected[i]; //目前和子关键帧具有最大权值的关键帧
max = w; //这个最大的权值
bContinue = true; //说明子节点找到了可以作为其新父关键帧的帧
}
}
}
}
}
// 如果在上面的过程中找到了新的关键帧
// ? 感觉这里的应该是在"遍历每一个子关键帧"的过程中进行的吧
if(bContinue)
{
// 因为父节点死了，并且子节点找到了新的父节点，子节点更新自己的父节点
pC->ChangeParent(pP);
// NOTICE 因为子节点找到了新的父节点并更新了父节点，那么该子节点升级，作为其它子节点的备选父节点
sParentCandidates.insert(pC);
// 该子节点处理完毕
mspChildrens.erase(pC);
}
else
break;
}
// If a children has no covisibility links with any parent candidate, assign to the original parent of this KF
// 如果还有子节点没有找到新的父节点
if(!mspChildrens.empty())
for(set<KeyFrame*>::iterator sit=mspChildrens.begin(); sit!=mspChildrens.end(); sit++)
{
// 直接把父节点的父节点作为自己的父节点 即对于这些子节点来说,他们的新的父节点其实就是自己的爷爷节点
(*sit)->ChangeParent(mpParent);
}
mpParent->EraseChild(this);
// 如果当前的关键帧要被删除的话就要计算这个,表示当前关键帧到原本的父关键帧的位姿变换 (注意在这个删除的过程中,其实并没有将当前关键帧中存储的父关键帧的指针删除掉)
mTcp = Tcw*mpParent->GetPoseInverse();
// 嗯,确定当前关键帧已经完蛋了
mbBad = true;
} //退出互斥锁的保护区域
mpMap->EraseKeyFrame(this);
mpKeyFrameDB->erase(this);
}
// 返回当前关键帧是否已经完蛋了
bool KeyFrame::isBad()
{
unique_lock<mutex> lock(mMutexConnections);
return mbBad;
}
/**
* 删除当前关键帧和指定关键帧之间的共视关系
* @param pKF 要删除的共视关系
*/
void KeyFrame::EraseConnection(KeyFrame* pKF)
{
// 其实这个应该表示是否真的是有共视关系
bool bUpdate = false;
{
unique_lock<mutex> lock(mMutexConnections);
if(mConnectedKeyFrameWeights.count(pKF))
{
mConnectedKeyFrameWeights.erase(pKF);
bUpdate=true;
}
}
// 如果是真的有共视关系,那么删除之后就要更新共视关系
if(bUpdate)
UpdateBestCovisibles();
}
/**
* 获取某个特征点的邻域中的特征点id,其实这个和 Frame.cc 中的那个函数基本上都是一致的; r为边长（半径）
* KeyPoint functions
* @brief 获取某个特征点的邻域中的特征点id
* @param[in] x 特征点坐标
* @param[in] y 特征点坐标
* @param[in] r 邻域大小(半径)
* @return std::vector<size_t> 在这个邻域内找到的特征点索引的集合
*/
vector<size_t> KeyFrame::GetFeaturesInArea(const float &x, const float &y, const float &r) const
{
vector<size_t> vIndices;
vIndices.reserve(N);
// 计算要搜索的cell的范围
// floor向下取整，mfGridElementWidthInv 为每个像素占多少个格子
const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv));
if(nMinCellX>=mnGridCols)
return vIndices;
// ceil向上取整
const int nMaxCellX = min((int)mnGridCols-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv));
if(nMaxCellX<0)
return vIndices;
const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));
if(nMinCellY>=mnGridRows)
return vIndices;
const int nMaxCellY = min((int)mnGridRows-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));
if(nMaxCellY<0)
return vIndices;
// 遍历每个cell,取出其中每个cell中的点,并且每个点都要计算是否在邻域内
for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
{
for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
{
const vector<size_t> vCell = mGrid[ix][iy];
for(size_t j=0, jend=vCell.size(); j<jend; j++)
{
const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];
const float distx = kpUn.pt.x-x;
const float disty = kpUn.pt.y-y;
if(fabs(distx)<r && fabs(disty)<r)
vIndices.push_back(vCell[j]);
}
}
}
return vIndices;
}
/**
* 判断某个点是否在当前关键帧的图像中
* Image
* @brief 判断某个点是否在当前关键帧的图像中
* @param[in] x 点的坐标
* @param[in] y 点的坐标
* @return true
* @return false
*/
bool KeyFrame::IsInImage(const float &x, const float &y) const
{
return (x>=mnMinX && x<mnMaxX && y>=mnMinY && y<mnMaxY);
}
/**
* @brief Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates.
* @param i 第i个keypoint
* @return 3D点（相对于世界坐标系）
* 在双目和RGBD情况下将特征点反投影到空间中
*/
cv::Mat KeyFrame::UnprojectStereo(int i)
{
const float z = mvDepth[i];
if(z>0)
{
// 由2维图像反投影到相机坐标系
// mvDepth是在ComputeStereoMatches函数中求取的
// mvDepth对应的校正前的特征点，因此这里对校正前特征点反投影
// 可在Frame::UnprojectStereo中却是对校正后的特征点mvKeysUn反投影
// 在ComputeStereoMatches函数中应该对校正后的特征点求深度？？ (wubo???)
// 我觉得是作者可能写错了 (guoqing)
const float u = mvKeys[i].pt.x;
const float v = mvKeys[i].pt.y;
const float x = (u-cx)*z*invfx;
const float y = (v-cy)*z*invfy;
cv::Mat x3Dc = (cv::Mat_<float>(3,1) << x, y, z);
unique_lock<mutex> lock(mMutexPose);
// 由相机坐标系转换到世界坐标系
// Twc为相机坐标系到世界坐标系的变换矩阵
// Twc.rosRange(0,3).colRange(0,3)取Twc矩阵的前3行与前3列
return Twc.rowRange(0,3).colRange(0,3)*x3Dc+Twc.rowRange(0,3).col(3);
}
else
return cv::Mat();
}
// Compute Scene Depth (q=2 median). Used in monocular. 评估当前关键帧场景深度，q=2表示中值. 只是在单目情况下才会使用
// 其实过程就是对当前关键帧下所有地图点的深度进行从小到大排序,返回距离头部其中1/q处的深度值作为当前场景的平均深度
/**
* @brief 评估当前关键帧场景深度，q=2表示中值
* @param q q=2
* @return Median Depth
*/
float KeyFrame::ComputeSceneMedianDepth(const int q)
{
vector<MapPoint*> vpMapPoints;
cv::Mat Tcw_;
{
unique_lock<mutex> lock(mMutexFeatures);
unique_lock<mutex> lock2(mMutexPose);
vpMapPoints = mvpMapPoints;
Tcw_ = Tcw.clone();
}
vector<float> vDepths;
vDepths.reserve(N);
cv::Mat Rcw2 = Tcw_.row(2).colRange(0,3);
Rcw2 = Rcw2.t();
float zcw = Tcw_.at<float>(2,3);
// 遍历每一个地图点,计算并保存其在当前关键帧下的深度
for(int i=0; i<N; i++)
{
if(mvpMapPoints[i])
{
MapPoint* pMP = mvpMapPoints[i];
cv::Mat x3Dw = pMP->GetWorldPos();
float z = Rcw2.dot(x3Dw)+zcw; // (R*x3Dw+t)的第三行，即z
vDepths.push_back(z);
}
}
sort(vDepths.begin(),vDepths.end());
return vDepths[(vDepths.size()-1)/q];
}
} //namespace ORB_SLAM
源码解读KeyFrameDatabase.cc
// 关键帧数据库,用于回环检测和重定位
#include "KeyFrameDatabase.h"
#include "KeyFrame.h"
#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
#include <mutex>
#include <vector>
#include <list>
#include <set>
#include "Frame.h"
#include "ORBVocabulary.h"
using namespace std;
namespace ORB_SLAM2
{
// class KeyFrameDatabase 关键帧数据库
/**
* @brief 构造函数
* @param[in] voc 词袋模型的字典
* // Associated vocabulary
const ORBVocabulary* mpVoc; ///< 预先训练好的词典
*/
KeyFrameDatabase::KeyFrameDatabase (const ORBVocabulary &voc):mpVoc(&voc)
{
// 数据库的主要内容了
// Inverted file
//std::vector<list<KeyFrame*> > mvInvertedFile; ///< 倒排索引，mvInvertedFile[i]表示包含了第i个word id的所有关键帧
mvInvertedFile.resize(voc.size()); // number of words
}
// 根据关键帧的词包，更新数据库的倒排索引
void KeyFrameDatabase::add(KeyFrame *pKF)
{
unique_lock<mutex> lock(mMutex);
// 为每一个word添加该KeyFrame
for(DBoW2::BowVector::const_iterator vit= pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit!=vend; vit++)
mvInvertedFile[vit->first].push_back(pKF);
}
// 关键帧被删除后，更新数据库的倒排索引
/**
* @brief 关键帧被删除后，更新数据库的倒排索引
* @param pKF 关键帧
*/
void KeyFrameDatabase::erase(KeyFrame* pKF)
{
unique_lock<mutex> lock(mMutex);
// Erase elements in the Inverse File for the entry
// 每一个KeyFrame包含多个words，遍历mvInvertedFile中的这些words，然后在word中删除该KeyFrame
for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit!=vend; vit++)
{
// List of keyframes that share the word
list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];
// 这个效率有点低啊
for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
{
if(pKF==*lit)
{
lKFs.erase(lit);
break;
}
}
}
}
/** @brief 清空关键帧数据库 */
void KeyFrameDatabase::clear()
{
mvInvertedFile.clear();// mvInvertedFile[i]表示包含了第i个word id的所有关键帧
mvInvertedFile.resize(mpVoc->size());// mpVoc：预先训练好的词典
}
/*
* @brief 在闭环检测中找到与该关键帧可能闭环的关键帧
* 1. 找出和当前帧具有公共单词的所有关键帧（不包括与当前帧相连的关键帧）
* 2. 只和具有共同单词较多的关键帧进行相似度计算
* 3. 将与关键帧相连（权值最高）的前十个关键帧归为一组，计算累计得分
* 4. 只返回累计得分较高的组中分数最高的关键帧
* @param pKF 需要闭环的关键帧
* @param minScore 相似性分数最低要求
* @return 可能闭环的关键帧
* @see III-E Bags of Words Place Recognition
*/
vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)
{
// 提出所有与该pKF相连的KeyFrame，这些相连Keyframe都是局部相连，在闭环检测的时候将被剔除
set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();
list<KeyFrame*> lKFsSharingWords;// 用于保存可能与pKF形成回环的候选帧（只要有相同的word，且不属于局部相连帧）
//这里的局部相连帧,就是和当前关键帧具有共视关系的关键帧
// Search all keyframes that share a word with current keyframes
// Discard keyframes connected to the query keyframe
//. 步骤1：找出和当前帧具有公共单词的所有关键帧（不包括与当前帧链接的关键帧）
{
unique_lock<mutex> lock(mMutex);
// words是检测图像是否匹配的枢纽，遍历该pKF的每一个word
for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)
{
// 提取所有包含该word的KeyFrame
list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];
// 然后对这些关键帧展开遍历
for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
{
KeyFrame* pKFi=*lit;
if(pKFi->mnLoopQuery!=pKF->mnId)// pKFi还没有标记为pKF的候选帧
{
pKFi->mnLoopWords=0;
if(!spConnectedKeyFrames.count(pKFi))// 与pKF局部链接的关键帧不进入闭环候选帧
{
pKFi->mnLoopQuery=pKF->mnId;// pKFi标记为pKF的候选帧，之后直接跳过判断
lKFsSharingWords.push_back(pKFi);
}
}
pKFi->mnLoopWords++;// 记录pKFi与pKF具有相同word的个数
}
}
}
// 如果没有关键帧和这个关键帧具有相同的单词,那么就返回空
if(lKFsSharingWords.empty())
return vector<KeyFrame*>();
list<pair<float,KeyFrame*> > lScoreAndMatch;
// Only compare against those keyframes that share enough words
//. 步骤2：统计所有闭环候选帧中与pKF具有共同单词最多的单词数
int maxCommonWords=0;
for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
{
if((*lit)->mnLoopWords>maxCommonWords)
maxCommonWords=(*lit)->mnLoopWords;
}
int minCommonWords = maxCommonWords*0.8f;
int nscores=0;
// Compute similarity score. Retain the matches whose score is higher than minScore
//. 步骤3：遍历所有闭环候选帧，挑选出共有单词数大于minCommonWords且单词匹配度大于minScore存入lScoreAndMatch
for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
{
KeyFrame* pKFi = *lit;
// pKF只和具有共同单词较多的关键帧进行比较，需要大于minCommonWords
if(pKFi->mnLoopWords>minCommonWords)
{
nscores++;// 这个变量后面没有用到
// 相似度评分就是在这里计算的
float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);
pKFi->mLoopScore = si;
if(si>=minScore)
lScoreAndMatch.push_back(make_pair(si,pKFi));
}
}
// 如果没有超过指定相似度阈值的，那么也就直接跳过去
if(lScoreAndMatch.empty())
return vector<KeyFrame*>();
list<pair<float,KeyFrame*> > lAccScoreAndMatch;
float bestAccScore = minScore;
// Lets now accumulate score by covisibility
// 单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧相连（权值最高，共视程度最高）的前十个关键帧归为一组，计算累计得分
//. 步骤4：具体而言：lScoreAndMatch中每一个KeyFrame都把与自己共视程度较高的帧归为一组，每一组会计算组得分并记录该组分数最高的KeyFrame，记录于lAccScoreAndMatch
for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
{
KeyFrame* pKFi = it->second;
vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
float bestScore = it->first; // 该组最高分数
float accScore = it->first; // 该组累计得分
KeyFrame* pBestKF = pKFi; // 该组最高分数对应的关键帧
for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
{
KeyFrame* pKF2 = *vit;
if(pKF2->mnLoopQuery==pKF->mnId && pKF2->mnLoopWords>minCommonWords)
{
accScore+=pKF2->mLoopScore;// 因为pKF2->mnLoopQuery==pKF->mnId，所以只有pKF2也在闭环候选帧中，才能贡献分数
if(pKF2->mLoopScore>bestScore)// 统计得到组里分数最高的KeyFrame
{
pBestKF=pKF2;
bestScore = pKF2->mLoopScore;
}
}
}
lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
if(accScore>bestAccScore)// 记录所有组中组得分最高的组
bestAccScore=accScore;
}
// Return all those keyframes with a score higher than 0.75*bestScore
float minScoreToRetain = 0.75f*bestAccScore;
set<KeyFrame*> spAlreadyAddedKF;
vector<KeyFrame*> vpLoopCandidates;
vpLoopCandidates.reserve(lAccScoreAndMatch.size());
//. 步骤5：得到组得分大于minScoreToRetain的组，得到组中分数最高的关键帧 0.75*bestScore
for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
{
if(it->first>minScoreToRetain)
{
KeyFrame* pKFi = it->second;
if(!spAlreadyAddedKF.count(pKFi))// 判断该pKFi是否已经在队列中了
{
vpLoopCandidates.push_back(pKFi);
spAlreadyAddedKF.insert(pKFi);
}
}
}
return vpLoopCandidates;
}
// Relocalization
/**
* @brief 在重定位中找到与该帧相似的关键帧
* 1. 找出和当前帧具有公共单词的所有关键帧
* 2. 只和具有共同单词较多的关键帧进行相似度计算
* 3. 将与关键帧相连（权值最高）的前十个关键帧归为一组，计算累计得分
* 4. 只返回累计得分较高的组中分数最高的关键帧
* @param F 需要重定位的帧
* @return 相似的关键帧
* @see III-E Bags of Words Place Recognition
* 基本上的套路和进行回环检测的时候基本上行都是相同的
*/
vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F)
{
// 相对于关键帧的闭环检测DetectLoopCandidates，重定位检测中没法获得相连的关键帧
list<KeyFrame*> lKFsSharingWords;// 用于保存可能与F形成回环的候选帧（只要有相同的word，且不属于局部相连帧(这里其实已经没有了所谓的"局部相连帧"的概念了)）
// Search all keyframes that share a word with current frame
//. 步骤1：找出和当前帧具有公共单词的所有关键帧
{
unique_lock<mutex> lock(mMutex);
// words是检测图像是否匹配的枢纽，遍历该pKF的每一个word
for(DBoW2::BowVector::const_iterator vit=F->mBowVec.begin(), vend=F->mBowVec.end(); vit != vend; vit++)
{
// 提取所有包含该word的KeyFrame
list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];
for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
{
KeyFrame* pKFi=*lit;
if(pKFi->mnRelocQuery!=F->mnId)// pKFi还没有标记为pKF的候选帧
{
pKFi->mnRelocWords=0;
pKFi->mnRelocQuery=F->mnId;
lKFsSharingWords.push_back(pKFi);
}
pKFi->mnRelocWords++;
}
}
}
if(lKFsSharingWords.empty())
return vector<KeyFrame*>();
// Only compare against those keyframes that share enough words
//. 步骤2：统计所有闭环候选帧中与当前帧F具有共同单词最多的单词数，并以此决定阈值
int maxCommonWords=0;
for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
{
if((*lit)->mnRelocWords>maxCommonWords)
maxCommonWords=(*lit)->mnRelocWords;
}
int minCommonWords = maxCommonWords*0.8f;
list<pair<float,KeyFrame*> > lScoreAndMatch;
int nscores=0;
// Compute similarity score.
//. 步骤3：遍历所有闭环候选帧，挑选出共有单词数大于阈值minCommonWords且单词匹配度大于minScore存入lScoreAndMatch
for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
{
KeyFrame* pKFi = *lit;
// 当前帧F只和具有共同单词较多的关键帧进行比较，需要大于minCommonWords
if(pKFi->mnRelocWords>minCommonWords)
{
nscores++;// 这个变量后面没有用到
float si = mpVoc->score(F->mBowVec,pKFi->mBowVec);
pKFi->mRelocScore=si;
lScoreAndMatch.push_back(make_pair(si,pKFi));
}
}
if(lScoreAndMatch.empty())
return vector<KeyFrame*>();
list<pair<float,KeyFrame*> > lAccScoreAndMatch;
float bestAccScore = 0;
// Lets now accumulate score by covisibility
// 步骤4：计算候选帧组得分，得到最高组得分bestAccScore，并以此决定阈值minScoreToRetain
// 单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧相连（权值最高，共视程度最高）的前十个关键帧归为一组，计算累计得分
// 具体而言：lScoreAndMatch中每一个KeyFrame都把与自己共视程度较高的帧归为一组，每一组会计算组得分并记录该组分数最高的KeyFrame，记录于lAccScoreAndMatch
for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
{
KeyFrame* pKFi = it->second;
vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
float bestScore = it->first; // 该组最高分数
float accScore = bestScore; // 该组累计得分
KeyFrame* pBestKF = pKFi; // 该组最高分数对应的关键帧
for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
{
KeyFrame* pKF2 = *vit;
if(pKF2->mnRelocQuery!=F->mnId)
continue;
accScore+=pKF2->mRelocScore;// 只有pKF2也在闭环候选帧中，才能贡献分数
if(pKF2->mRelocScore>bestScore)// 统计得到组里分数最高的KeyFrame
{
pBestKF=pKF2;
bestScore = pKF2->mRelocScore;
}
}
lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
if(accScore>bestAccScore) // 记录所有组中组得分最高的组
bestAccScore=accScore; // 得到所有组中最高的累计得分
}
// Return all those keyframes with a score higher than 0.75*bestScore
// 步骤5：得到组得分大于阈值的，组内得分最高的关键帧
float minScoreToRetain = 0.75f*bestAccScore;
set<KeyFrame*> spAlreadyAddedKF;
vector<KeyFrame*> vpRelocCandidates;
vpRelocCandidates.reserve(lAccScoreAndMatch.size());
for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
{
const float &si = it->first;
// 只返回累计得分大于minScoreToRetain的组中分数最高的关键帧 0.75*bestScore
if(si>minScoreToRetain)
{
KeyFrame* pKFi = it->second;
if(!spAlreadyAddedKF.count(pKFi))// 判断该pKFi是否已经在队列中了
{
vpRelocCandidates.push_back(pKFi);
spAlreadyAddedKF.insert(pKFi);
}
}
}
return vpRelocCandidates;
}
} //namespace ORB_SLAM