首先会看到long unsigned int
类型的数据:
long占四个字节;
int的尺寸和平台有关系:
①在16位的系统中,int 占据2个字节
②在32位系统中,占用4个字节
关键帧——初始化函数
初始化:关键帧数据库,地图,帧ID,网格用于快速匹配,位姿。
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)
{
mnId=nNextId++;
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);
}
关键帧——计算词袋(BoW bag of words)
首先将二维矩阵表述的描述子矩阵,按照行排列,逐行变为描述子向量;
其次通过词袋模型,将所有描述子分散到树的第四层上,便于查找。
/**
* @brief Bag of Words Representation
*
* 计算mBowVec,并且将描述子分散在第4层上,即mFeatVec记录了属于第i个node的ni个描述子
* @see ProcessNewKeyFrame()
*/
void KeyFrame::ComputeBoW()
{
// 此处判断在Frame.cpp中是否已经计算了帧的词袋
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);
}
}
关键帧(位姿)——设置位姿
此处涉及到同一个变量,在多线程中的读写,因此此函数内部需要加锁。
计算世界坐标系到相机坐标系的平移向量:
R
c
w
⋅
P
w
+
t
c
w
=
P
c
R_{cw}\cdot P_{w} + t_{cw} = P_{c}
Rcw⋅Pw+tcw=Pc
P
w
=
R
c
w
−
1
⋅
P
c
−
R
c
w
−
1
⋅
t
c
w
P_{w} = R_{cw}^{-1} \cdot P_{c} -R_{cw}^{-1} \cdot t_{cw}
Pw=Rcw−1⋅Pc−Rcw−1⋅tcw
P
w
=
T
w
c
⋅
P
c
=
R
w
c
⋅
P
c
+
t
w
c
P_{w} = T_{wc} \cdot P_{c} = R_{wc} \cdot P_{c} + t_{wc}
Pw=Twc⋅Pc=Rwc⋅Pc+twc
当
P
c
=
[
0
,
0
,
0
]
T
P_{c} = [0, 0, 0]_{}^{T}
Pc=[0,0,0]T时,上下两式相等,可以等到世界坐标到相机坐标的平移向量
t
w
c
=
−
R
c
w
−
1
⋅
t
c
w
t_{wc} = -R_{cw}^{-1} \cdot t_{cw}
twc=−Rcw−1⋅tcw 同时由于旋转矩阵
R
c
w
R_{cw}
Rcw与
R
w
c
R_{wc}
Rwc是反对称矩阵,因此其转置就是求逆。
T w c T_{wc} Twc与T_{cw}的换算关系:
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;
}
关键帧(位姿)——得到位姿
位姿:相机坐标系到世界坐标系的变换矩阵
T
c
w
T_{cw}
Tcw:
T
c
w
⋅
P
w
=
P
c
T_{cw} \cdot P_{w} = P_{c}
Tcw⋅Pw=Pc
cv::Mat KeyFrame::GetPose()
{
unique_lock<mutex> lock(mMutexPose);
return Tcw.clone();
}
关键帧(位姿)——得到位姿的逆
其实并不是 T c w T_{cw} Tcw进行 T c w − 1 T_{cw}^{-1} Tcw−1运算,而是为了得到世界坐标到相机坐标的变换矩阵 T w c T_{wc} Twc。
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();
}
关键帧(位姿)——得到旋转矩阵
指: T c w T_{cw} Tcw的前三维构成的3X3矩阵。
cv::Mat KeyFrame::GetRotation()
{
unique_lock<mutex> lock(mMutexPose);
return Tcw.rowRange(0,3).colRange(0,3).clone();
}
关键帧(位姿)——得到平移矩阵
指: T c w T_{cw} Tcw第3列形成的1X3的向量。
cv::Mat KeyFrame::GetTranslation()
{
unique_lock<mutex> lock(mMutexPose);
return Tcw.rowRange(0,3).col(3).clone();
}
关键帧(共视图)——为关键帧添加连接
需要理解std::map<KeyFrame*,int> mConnectedKeyFrameWeights; ///< 与该关键帧连接的关键帧与权重
其中std::map<>
形成键/值对(关键帧/权重)。哈希表是不可重复的,链表是可重复的。
/**
* @brief 为关键帧之间添加连接
*
* 更新了mConnectedKeyFrameWeights
* @param pKF 关键帧
* @param weight 权重,该关键帧与pKF共同观测到的3d点数量
*/
void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight)
{
{
unique_lock<mutex> lock(mMutexConnections);
// std::map::count函数只可能返回0或1两种情况
if(!mConnectedKeyFrameWeights.count(pKF)) // count函数返回0,mConnectedKeyFrameWeights中没有pKF,之前没有连接
mConnectedKeyFrameWeights[pKF]=weight;
else if(mConnectedKeyFrameWeights[pKF]!=weight) // 之前连接的权重不一样,更新键值(weight)
mConnectedKeyFrameWeights[pKF]=weight;
else
return;
}
// 按照权重对连接的关键帧进行排序
UpdateBestCovisibles();
}
关键帧(共视图)——跟新连接的关键帧排序
利用sort函数对pair数据进行排序;针对vector<pair<int,KeyFrame*> >
使用sort函数,默认对first进行升序排列,从小到大。得到排序后的权重序列,以及根据权重序列排列的关键帧序列。
由于map结构没有sort函数,需要将元素取出放入一个pair组成的vector中,排序后放入这两个向量中mvpOrderedConnectedKeyFrames 和 mvOrderedWeights 。
/**
* @brief 按照权重对连接的关键帧进行排序
*
* 更新后的变量存储在mvpOrderedConnectedKeyFrames和mvOrderedWeights中
*/
void KeyFrame::UpdateBestCovisibles()
{
unique_lock<mutex> lock(mMutexConnections);
// http://stackoverflow.com/questions/3389648/difference-between-stdliststdpair-and-stdmap-in-c-stl
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());
}
关键帧(共视图)——得到与此关键帧连接的关键帧集合
从已经存储好的连接关键帧的键值对中,将关键帧取出来,放入集合中。
/**
* @brief 得到与该关键帧连接的关键帧
* @return 连接的关键帧
*/
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;
}
关键帧(共视图)——得到共视关键帧(已经排序的关键帧序列)
跟新连接的关键帧排序函数中已经将此序列的关键帧提取出来;
/**
* @brief 得到与该关键帧连接的关键帧(已按权值排序)
* @return 连接的关键帧
*/
vector<KeyFrame*> KeyFrame::GetVectorCovisibleKeyFrames()
{
unique_lock<mutex> lock(mMutexConnections);
return mvpOrderedConnectedKeyFrames;
}
关键帧(共视图)——得到与该关键帧连接的前N个关键帧
这个是否不应该使用权值排序后的关键帧序列,排序将打乱其时间顺序,怎么确定前N个关键帧哪?
如果按照越靠近该关键帧,其权重越大的思路考虑,使用排序后的关键帧序列,取前N个关键帧,仍然有效。
/**
* @brief 得到与该关键帧连接的前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);
}
关键帧(共视图)——得到连接的关键帧中权值大于w的关键帧
需要理解#include <algorithm>
中的upper_bound
函数。此函数使用条件:数组有序,并且是非降序列。如果是降序,则需要使用其扩展用法lower_bound(迭代器_1, 迭代器_2, x, compare)
或者upper_bound(迭代器_1, 迭代器_2, x, compare)
理解vector的反向迭代器(rbegin, rend)与(begin, end)的区别。
/**
* @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只能返回第一个大于的(这个解释是错误的)upper_bound在扩展应用情况下返回的是第一个小于w的迭代器":::::此处原始用法是正确的。
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);
}
}
关键帧(共视图)——得到该关键帧与pKF的权重
首先确定pKF关键帧是否存在连接关系;
其次从map的键值对中取出权重值。
/**
* @brief 得到该关键帧与pKF的权重
* @param pKF 关键帧
* @return 权重
*/
int KeyFrame::GetWeight(KeyFrame *pKF)
{
unique_lock<mutex> lock(mMutexConnections);
if(mConnectedKeyFrameWeights.count(pKF))
return mConnectedKeyFrameWeights[pKF];
else
return 0;
}
关键帧(地图点)——增加地图点
讲mappoint.cpp中相关函数计算的地图点,作为关键帧的属性添加进来。
/**
* @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;
}
关键帧(地图点)——根据ID擦除地图点与关键帧的匹配
其中用到了static_cast
mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);
此ID下的地图点重置为空。
void KeyFrame::EraseMapPointMatch(const size_t &idx)
{
unique_lock<mutex> lock(mMutexFeatures);
mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);
}
关键帧(地图点)——根据地图点删除与关键帧的匹配
首先利用地图与关键帧的映射关系(在mappoint.cpp中的函数),找到对应的ID,然后将此ID下的地图点重置为空。
void KeyFrame::EraseMapPointMatch(MapPoint* pMP)
{
int idx = pMP->GetIndexInKeyFrame(this);
if(idx>=0)
mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);
}
关键帧(地图点)——根据关键帧ID匹配地图点
void KeyFrame::ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP)
{
mvpMapPoints[idx]=pMP;
}
关键帧(地图点)——获取地图点
// MapPoints associated to keypoints
std::vector<MapPoint*> mvpMapPoints;
从上述变量中取出所有的地图点。
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代表最少被观测到MapPoint的关键帧数量。返回一共多少个关键帧观测到此MapPoint。
/**
* @brief 关键帧中,大于等于minObs的MapPoints的数量
* minObs就是一个阈值,大于minObs就表示该MapPoint是一个高质量的MapPoint
* 一个高质量的MapPoint会被多个KeyFrame观测到,
* @param minObs 最小观测
*/
int KeyFrame::TrackedMapPoints(const int &minObs)
{
unique_lock<mutex> lock(mMutexFeatures);
int nPoints=0;
const bool bCheckObs = minObs>0;
for(int i=0; i<N; i++)
{
MapPoint* pMP = mvpMapPoints[i];
if(pMP)
{
if(!pMP->isBad())
{
if(bCheckObs)
{
// 该MapPoint是一个高质量的MapPoint
if(mvpMapPoints[i]->Observations()>=minObs)
nPoints++;
}
else
nPoints++;
}
}
}
return nPoints;
}
关键帧(地图点)——获取关键帧的地图点
mvpMapPoints地图点是跟踪、局部地图等输入;
返回的是地图点向量。
/**
* @brief Get MapPoint Matches
*
* 获取该关键帧的MapPoints
*/
vector<MapPoint*> KeyFrame::GetMapPointMatches()
{
unique_lock<mutex> lock(mMutexFeatures);
return mvpMapPoints;
}
关键帧(地图点)——获取指定ID的地图点
仅仅返回对应的地图点。
MapPoint* KeyFrame::GetMapPoint(const size_t &idx)
{
unique_lock<mutex> lock(mMutexFeatures);
return mvpMapPoints[idx];
}
关键帧(生长树)——增加子树
将当前帧插入到子树集合中
void KeyFrame::AddChild(KeyFrame *pKF)
{
unique_lock<mutex> lockCon(mMutexConnections);
mspChildrens.insert(pKF);
}
关键帧(生长树)——删除子树
void KeyFrame::EraseChild(KeyFrame *pKF)
{
unique_lock<mutex> lockCon(mMutexConnections);
mspChildrens.erase(pKF);
}
关键帧(生长树)——更改父节点
void KeyFrame::ChangeParent(KeyFrame *pKF)
{
unique_lock<mutex> lockCon(mMutexConnections);
mpParent = pKF;
pKF->AddChild(this);
}
关键帧(生长树)——获得所有子树节点set
set<KeyFrame*> KeyFrame::GetChilds()
{
unique_lock<mutex> lockCon(mMutexConnections);
return mspChildrens;
}
关键帧(生长树)——得到该帧父节点
KeyFrame* KeyFrame::GetParent()
{
unique_lock<mutex> lockCon(mMutexConnections);
return mpParent;
}
关键帧(生长树)——关键帧是否存在在子树中
判断某一帧是否含有某一个关键帧子节点
bool KeyFrame::hasChild(KeyFrame *pKF)
{
unique_lock<mutex> lockCon(mMutexConnections);
return mspChildrens.count(pKF);
}