本文参考:泡泡机器人提供的带注释的ORB_SLAM2源代码
转载本文请注明出处:
https://blog.csdn.net/RobotLife/article/details/87194017
在orb_slam2中,为了实现特征尺度不变性采用了图像金字塔,金字塔的缩放因子为1.2,。其思路就是对原始图形(第0层)依次进行1/1.2缩放比例进行降采样得到共计8张图片(包括原始图像),然后分别对得到的图像进行特征提取,并记录特征所在金字塔的第几层,这样得到一帧图像的特征点,如图1所示。
现在假设在第二层中有一特征点F,为了避免缩放带来特征点F在纵向的移动,为简化叙述,选择的特征点F位于图像中心,如图2所示。根据相机成像“物近像大,物远像小”的原理,如图2所示为相机成像的示意图。假设图1中摄像机原始图像即金字塔第0层对应图2中成像视野I0 ,则图1中图像金字塔第2层图像可以相应对应于图2中成像视野I2 。
其中特征点F所在patch的相应关系如图3所示。根据图2可以得到 结论1: d2 / d0 = 1.22 。
有了以上铺垫现在,来说说,尺度不变性,这里不直接说明,而是看看对于第m层上的一个特征点,其对应尺度不变时相机与特征点对应空间位置之间距离(简称物距)的范围。
假设第m层上有一特征点Fm,其空间位置与拍摄时相机中心的位置为dm ,显然这是原始图像缩放1/1.2m 倍后得到的特征点patch,考虑“物远像小”的成像特点,要使得该第m层特征点对应patch变为图像金字塔第0层中同样大小的patch,其相机与空间点的距离d=dm * 1.2m ,即尺度不变的最大物距dmax = dm*1.2m 。
要求尺度不变的最小物距则这样考虑:根据“物近像大”的成像特点,使得当前第m层的特征点移到第7层上则,真实相机成像图像得放大1.27-m倍,故对应最小物距dmin=dm *1.2m-7 。
orb_slam2中原始代码如下:
注意:上面变量与代码中变量的对应关系:
7 <–> nLevels-1
m <–> level
1.2m <–> levelScaleFactor
dmax <–> mfMaxDistance
dmin <–> mfMinDistance
void MapPoint::UpdateNormalAndDepth()
{
map<KeyFrame*,size_t> observations;
KeyFrame* pRefKF;
cv::Mat Pos;
{
unique_lock<mutex> lock1(mMutexFeatures);
unique_lock<mutex> lock2(mMutexPos);
if(mbBad)
return;
observations=mObservations; // 获得观测到该3d点的所有关键帧
pRefKF=mpRefKF; // 观测到该点的参考关键帧
Pos = mWorldPos.clone(); // 3d点在世界坐标系中的位置
}
if(observations.empty())
return;
cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
int n=0;
for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
KeyFrame* pKF = mit->first;
cv::Mat Owi = pKF->GetCameraCenter();
cv::Mat normali = mWorldPos - Owi;
normal = normal + normali/cv::norm(normali); // 对所有关键帧对该点的观测方向归一化为单位向量进行求和
n++;
}
cv::Mat PC = Pos - pRefKF->GetCameraCenter(); // 参考关键帧相机指向3D点的向量(在世界坐标系下的表示)
const float dist = cv::norm(PC); // 该点到参考关键帧相机的距离
const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;
const float levelScaleFactor = pRefKF->mvScaleFactors[level];
const int nLevels = pRefKF->mnScaleLevels; // 金字塔层数
{
unique_lock<mutex> lock3(mMutexPos);
// 另见PredictScale函数前的注释
mfMaxDistance = dist*levelScaleFactor; // 观测到该点的距离下限
mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1]; // 观测到该点的距离上限
mNormalVector = normal/n; // 获得平均的观测方向
}
}