ORB-SLAM2源码笔记（4）——帧Frame和关键帧KeyFrame
Frame类中的相机参数为static类型，表示所有Frame对象共享一份相机参数
特征点提取ExtractORB
在Frame类构造函数中调用成员变量mpORBextractorLeft和mpORBextractorRight的()运算符进行特征点提取.
畸变矫正前的左目特征点是
mvKeys[i].畸变矫正后的左目特征点是
mvKeysUn[i].在右目图片中对应特征点的横坐标为
mvuRight[i],纵坐标与mvKeys[i]的纵坐标相同.特征点的深度是
mvDepth[i].
void Frame::ExtractORB(int flag, const cv::Mat &im) {
if (flag == 0) // flag==0, 表示对左图提取ORB特征点
(*mpORBextractorLeft)(im, cv::Mat(), mvKeys, mDescriptors);
else // flag==1, 表示对右图提取ORB特征点
(*mpORBextractorRight)(im, cv::Mat(), mvKeysRight, mDescriptorsRight);
}
定义函数ExtractORB，调用通过ORBextractor创建出来的对象mpORBextractorLeft和mpORBextractorRight进行特征提取。
- 对于双目相机,通过双目特征点匹配关系计算特征点的深度值.
- 对于RGBD相机,根据特征点深度构造虚拟的右目图像特征点.
通过这种预处理,在后面SLAM系统中不再区分双目特征点和RGBD特征点,它们以双目特征点的形式被处理.(仅通过判断mvuRight[idx]判断某特征点是否有深度)
int ORBmatcher::SearchByProjection(Frame &F, const vector &vpMapPoints, const float th) {
// ...
for (size_t idx : vIndices) {
if (F.mvuRight[idx] > 0) { // 通过判断 mvuRight[idx] 判断该特征点是否有深度
// 针对有深度的特征点特殊处理
} else {
// 针对单目特征点的特殊处理
}
}
// ...
}
双目特征点匹配
粗匹配: 根据特征点描述子距离和金字塔层级判断匹配.粗匹配关系是按行寻找的,对于左目图像中每个特征点,在右目图像对应行上寻找匹配特征点.
精匹配: 根据特征点周围窗口内容相似度判断匹配.
亚像素插值: 将特征点相似度与匹配坐标之间拟合成二次曲线,寻找最佳匹配位置(得到的是一个小数).
记录右目匹配mvuRight和深度mvDepth信息.
离群点筛选: 以平均相似度的2.1倍为标准,筛选离群点.
void Frame::ComputeStereoMatches() {
mvuRight = vector(N, -1.0f);
mvDepth = vector(N, -1.0f);
// step0. 右目图像特征点逐行统计: 将右目图像中每个特征点注册到附近几行上
vector > vRowIndices(nRows, vector()); // 图像每行的1右目特征点索引
for (int iR = 0; iR < mvKeysRight.size(); iR++) {
const cv::KeyPoint &kp = mvKeysRight[iR];
const float &kpY = kp.pt.y;
const int maxr = ceil(kpY + 2.0f * mvScaleFactors[mvKeysRight[iR].octave]);
const int minr = floor(kpY - 2.0f * mvScaleFactors[mvKeysRight[iR].octave]);
for (int yi = minr; yi <= maxr; yi++)
vRowIndices[yi].push_back(iR);
}
// step1. + 2. 粗匹配+精匹配
const float minZ = mb, minD = 0, maxD = mbf / minZ; // 根据视差公式计算两个特征点匹配搜索的范围
const int thOrbDist = (ORBmatcher::TH_HIGH + ORBmatcher::TH_LOW) / 2;
vector > vDistIdx; // 保存特征点匹配
for (int iL = 0; iL < N; iL++) {
const cv::KeyPoint &kpL = mvKeys[iL];
const int &levelL = kpL.octave;
const float &vL = kpL.pt.y, &uL = kpL.pt.x;
const vector &vCandidates = vRowIndices[vL];
if (vCandidates.empty()) continue;
// step1. 粗匹配,根据特征点描述子和金字塔层级进行粗匹配
int bestDist = ORBmatcher::TH_HIGH;
size_t bestIdxR = 0;
const cv::Mat &dL = mDescriptors.row(iL);
for (size_t iC = 0; iC < vCandidates.size(); iC++) {
const size_t iR = vCandidates[iC];
const cv::KeyPoint &kpR = mvKeysRight[iR];
if (kpR.octave < levelL - 1 || kpR.octave > levelL + 1)
continue;
const float &uR = kpR.pt.x;
if (uR >= minU && uR <= maxU) {
const cv::Mat &dR = mDescriptorsRight.row(iR);
const int dist = ORBmatcher::DescriptorDistance(dL, dR);
if (dist < bestDist) {
bestDist = dist;
bestIdxR = iR;
}
}
}
// step2. 精匹配: 滑动窗口匹配,根据匹配点周围5✖5窗口寻找精确匹配
if (bestDist < thOrbDist) {
const float uR0 = mvKeysRight[bestIdxR].pt.x;
const float scaleFactor = mvInvScaleFactors[kpL.octave];
const float scaleduL = round(kpL.pt.x * scaleFactor);
const float scaledvL = round(kpL.pt.y * scaleFactor);
const float scaleduR0 = round(uR0 * scaleFactor);
const int w = 5;
cv::Mat IL = mpORBextractorLeft->mvImagePyramid[kpL.octave].rowRange(scaledvL - w, scaledvL + w + 1).colRange(scaleduL - w, scaleduL + w + 1);
IL.convertTo(IL, CV_32F);
IL = IL - IL.at(w, w) * cv::Mat::ones(IL.rows, IL.cols, CV_32F);
int bestDist = INT_MAX;
int bestincR = 0;
const int L = 5;
vector vDists;
vDists.resize(2 * L + 1);
const float iniu = scaleduR0 + L - w;
const float endu = scaleduR0 + L + w + 1;
for (int incR = -L; incR <= +L; incR++) {
cv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL - w, scaledvL + w + 1).colRange(scaleduR0 + incR - w, scaleduR0 + incR + w + 1);
IR.convertTo(IR, CV_32F);
IR = IR - IR.at(w, w) * cv::Mat::ones(IR.rows, IR.cols, CV_32F);
float dist = cv::norm(IL, IR, cv::NORM_L1);
if (dist < bestDist) {
bestDist = dist;
bestincR = incR;
}
vDists[L + incR] = dist;
}
// step3. 亚像素插值: 将特征点匹配距离拟合成二次曲线,寻找二次曲线最低点(是一个小数)作为最优匹配点坐标
const float dist1 = vDists[L + bestincR - 1];
const float dist2 = vDists[L + bestincR];
const float dist3 = vDists[L + bestincR + 1];
const float deltaR = (dist1 - dist3) / (2.0f * (dist1 + dist3 - 2.0f * dist2));
// step4. 记录特征点的右目和深度信息
float bestuR = mvScaleFactors[kpL.octave] * ((float) scaleduR0 + (float) bestincR + deltaR);
float disparity = (uL - bestuR);
if (disparity >= minD && disparity < maxD) {
mvDepth[iL] = mbf / disparity;
mvuRight[iL] = bestuR;
vDistIdx.push_back(pair(bestDist, iL));
}
}
}
// step5. 删除离群点: 匹配距离大于平均匹配距离2.1倍的视为误匹配
sort(vDistIdx.begin(), vDistIdx.end());
const float median = vDistIdx[vDistIdx.size() / 2].first;
const float thDist = 1.5f * 1.4f * median;
for (int i = vDistIdx.size() - 1; i >= 0; i--) {
if (vDistIdx[i].first < thDist)
break;
else {
mvuRight[vDistIdx[i].second] = -1;
mvDepth[vDistIdx[i].second] = -1;
}
}
}
对于RGBD特征，根据深度信息构造虚拟右目图像。
特征点分配AssigenFeaturesToGrid（）
在对特征点进行预处理后,将特征点分配到48行64列的网格中以加速匹配
对于双目和RGBD相机，分别执行以下Frame构造函数
// 双目相机Frame构造函数
Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor *extractorLeft, ORBextractor *extractorRight, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
: mpORBvocabulary(voc), mpORBextractorLeft(extractorLeft), mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mpReferenceKF(static_cast(NULL)) {
// step0. 帧ID自增
mnId = nNextId++;
// step1. 计算金字塔参数
mnScaleLevels = mpORBextractorLeft->GetLevels();
mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
mfLogScaleFactor = log(mfScaleFactor);
mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
// step2. 提取双目图像特征点
thread threadLeft(&Frame::ExtractORB, this, 0, imLeft);
thread threadRight(&Frame::ExtractORB, this, 1, imRight);
threadLeft.join();
threadRight.join();
N = mvKeys.size();
if (mvKeys.empty())
return;
// step3. 畸变矫正,实际上UndistortKeyPoints()不对双目图像进行矫正
UndistortKeyPoints();
// step4. 双目图像特征点匹配
ComputeStereoMatches();
// step5. 第一次调用构造函数时计算static变量
if (mbInitialComputations) {
ComputeImageBounds(imLeft);
mfGridElementWidthInv = static_cast(FRAME_GRID_COLS) / static_cast(mnMaxX - mnMinX);
mfGridElementHeightInv = static_cast(FRAME_GRID_ROWS) / static_cast(mnMaxY - mnMinY);
fx = K.at(0, 0);
fy = K.at(1, 1);
cx = K.at(0, 2);
cy = K.at(1, 2);
invfx = 1.0f / fx;
invfy = 1.0f / fy;
// 计算完成,标志复位
mbInitialComputations = false;
}
mvpMapPoints = vector(N, static_cast(NULL)); // 初始化本帧的地图点
mvbOutlier = vector(N, false); // 标记当前帧的地图点不是外点
mb = mbf / fx; // 计算双目基线长度
// step6. 将特征点分配到网格中
AssignFeaturesToGrid();
}
// RGBD相机Frame构造函数
Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor *extractor, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
: mpORBvocabulary(voc), mpORBextractorLeft(extractor), mpORBextractorRight(static_cast(NULL)), mTimeStamp(timeStamp), mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) {
// step0. 帧ID自增
mnId = nNextId++;
// step1. 计算金字塔参数
mnScaleLevels = mpORBextractorLeft->GetLevels();
mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
mfLogScaleFactor = log(mfScaleFactor);
mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
// step2. 提取左目图像特征点
ExtractORB(0, imGray);
N = mvKeys.size();
if (mvKeys.empty())
return;
// step3. 畸变矫正
UndistortKeyPoints();
// step4. 根据深度信息构造虚拟右目图像
ComputeStereoFromRGBD(imDepth);
mvpMapPoints = vector(N, static_cast(NULL));
mvbOutlier = vector(N, false);
// step5. 第一次调用构造函数时计算static变量
if (mbInitialComputations) {
ComputeImageBounds(imLeft);
mfGridElementWidthInv = static_cast(FRAME_GRID_COLS) / static_cast(mnMaxX - mnMinX);
mfGridElementHeightInv = static_cast(FRAME_GRID_ROWS) / static_cast(mnMaxY - mnMinY);
fx = K.at(0, 0);
fy = K.at(1, 1);
cx = K.at(0, 2);
cy = K.at(1, 2);
invfx = 1.0f / fx;
invfy = 1.0f / fy;
// 计算完成,标志复位
mbInitialComputations = false;
}
mvpMapPoints = vector(N, static_cast(NULL)); // 初始化本帧的地图点
mvbOutlier = vector(N, false); // 标记当前帧的地图点不是外点
mb = mbf / fx; // 计算双目基线长度
// step6. 将特征点分配到网格中
AssignFeaturesToGrid();
}
Tracking线程每收到一帧图像,就调用函数Tracking::GrabImageMonocular()、Tracking::GrabImageStereo()或Tracking::GrabImageRGBD()创建一个Frame对象,赋值给mCurrentFrame.函数跟踪结束后,会将
mCurrentFrame赋值给mLastFrame
关键帧KeyFrame
ORB-SLAM2代码详解05: 关键帧KeyFrame_ncepu_Chen的博客-CSDN博客_slam 关键帧
能看到同一地图点的两关键帧之间存在共视关系,共视地图点的数量被称为权重mConnectedKeyFrameWeights记录了当前关键帧的共视关键帧及权重
mvpOrderedConnectedKeyFrames记录了所有共视关键帧,按权重从大到小排序
mvOrderedWeights记录了所有共视权重,按从大到小排序
基于当前关键帧对地图点的观测构造共视图UpdateConnections()
void KeyFrame::UpdateConnections() {
// 1. 通过遍历当前帧地图点获取其与其它关键帧的共视程度,存入变量KFcounter中
vector vpMP;
{
unique_lock lockMPs(mMutexFeatures);
vpMP = mvpMapPoints;
}
map KFcounter;
for (MapPoint *pMP : vpMP) {
map observations = pMP->GetObservations();
for (map::iterator mit = observations.begin(); mit != observations.end(); mit++) {
if (mit->first->mnId == mnId) // 与当前关键帧本身不算共视
continue;
KFcounter[mit->first]++;
}
}
// step2. 找到与当前关键帧共视程度超过15的关键帧,存入变量vPairs中
vector > vPairs;
int th = 15;
int nmax = 0;
KeyFrame *pKFmax = NULL;
for (map::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));
(mit->first)->AddConnection(this, mit->second); // 对超过阈值的共视边建立连接
}
}
// step3. 对关键帧按照共视权重降序排序,存入变量mvpOrderedConnectedKeyFrames和mvOrderedWeights中
sort(vPairs.begin(), vPairs.end());
list lKFs;
list lWs;
for (size_t i = 0; i < vPairs.size(); i++) {
lKFs.push_front(vPairs[i].second);
lWs.push_front(vPairs[i].first);
}
{
unique_lock lockCon(mMutexConnections);
mConnectedKeyFrameWeights = KFcounter;
mvpOrderedConnectedKeyFrames = vector(lKFs.begin(), lKFs.end());
mvOrderedWeights = vector(lWs.begin(), lWs.end());
// step4. 对于第一次加入生成树的关键帧,取共视程度最高的关键帧为父关键帧
if (mbFirstConnection && mnId != 0) {
mpParent = mvpOrderedConnectedKeyFrames.front();
mpParent->AddChild(this);
mbFirstConnection = false;
}
}
}
当关键帧和地图点的连接关系发生变化，UpdateConnections()就会被调用
生成树mpParent、mspChildrens
在ORB-SLAM2中,保存所有关键帧构成的最小生成树(优先选择权重大的边作为生成树的边),在回环闭合时只需对最小生成树做BA优化就能以最小代价优化所有关键帧和地图点的位姿,相比于优化共视图大大减少了计算量
新关键帧的父关键帧会被设为其共视程度最高的共视关键帧
void KeyFrame::UpdateConnections() {
// 更新共视图信息
// ...
// 更新关键帧信息: 对于第一次加入生成树的关键帧,取共视程度最高的关键帧为父关键帧
// 该操作会改变当前关键帧的成员变量mpParent和父关键帧的成员变量mspChildrens
unique_lock lockCon(mMutexConnections);
if (mbFirstConnection && mnId != 0) {
mpParent = mvpOrderedConnectedKeyFrames.front();
mpParent->AddChild(this);
mbFirstConnection = false;
}
}
对KeyFrame的删除过程也采取先标记再清除的方式，参与回环检测的关键帧具有不被删除的特权
当一个关键帧被删除时,其父关键帧和所有子关键帧的生成树信息也会受到影响,需要为其所有子关键帧寻找新的父关键帧,如果父关键帧找的不好的话,就会产生回环,导致生成树就断开.
采用类似于最小生成树算法中的加边法重新构建生成树结构: 每次循环取权重最高的候选边建立父子连接关系,并将新加入生成树的子节点到加入候选父节点集合sParentCandidates中
在回环检测中回环矫正函数在调用本质图BA优化前会先添加当前关键帧和闭环匹配关键帧之间的回环关系。