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<MapPoint *> &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<float>(N, -1.0f);mvDepth = vector<float>(N, -1.0f);// step0. 右目图像特征点逐行统计: 将右目图像中每个特征点注册到附近几行上vector<vector<size_t> > vRowIndices(nRows, vector<size_t>()); // 图像每行的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<pair<int, int> > 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<size_t> &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<float>(w, w) * cv::Mat::ones(IL.rows, IL.cols, CV_32F);int bestDist = INT_MAX;int bestincR = 0;const int L = 5;vector<float> 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<float>(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<int, int>(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<KeyFrame *>(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<float>(FRAME_GRID_COLS) / static_cast<float>(mnMaxX - mnMinX);mfGridElementHeightInv = static_cast<float>(FRAME_GRID_ROWS) / static_cast<float>(mnMaxY - mnMinY);fx = K.at<float>(0, 0);fy = K.at<float>(1, 1);cx = K.at<float>(0, 2);cy = K.at<float>(1, 2);invfx = 1.0f / fx;invfy = 1.0f / fy;// 计算完成,标志复位mbInitialComputations = false;}mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL)); // 初始化本帧的地图点mvbOutlier = vector<bool>(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<ORBextractor *>(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<MapPoint *>(N, static_cast<MapPoint *>(NULL));mvbOutlier = vector<bool>(N, false);// step5. 第一次调用构造函数时计算static变量if (mbInitialComputations) {ComputeImageBounds(imLeft);mfGridElementWidthInv = static_cast<float>(FRAME_GRID_COLS) / static_cast<float>(mnMaxX - mnMinX);mfGridElementHeightInv = static_cast<float>(FRAME_GRID_ROWS) / static_cast<float>(mnMaxY - mnMinY);fx = K.at<float>(0, 0);fy = K.at<float>(1, 1);cx = K.at<float>(0, 2);cy = K.at<float>(1, 2);invfx = 1.0f / fx;invfy = 1.0f / fy;// 计算完成,标志复位mbInitialComputations = false;}mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL)); // 初始化本帧的地图点mvbOutlier = vector<bool>(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<MapPoint *> vpMP;{unique_lock<mutex> lockMPs(mMutexFeatures);vpMP = mvpMapPoints;}map<KeyFrame *, int> KFcounter; for (MapPoint *pMP : vpMP) {map<KeyFrame *, size_t> observations = pMP->GetObservations();for (map<KeyFrame *, size_t>::iterator mit = observations.begin(); mit != observations.end(); mit++) {if (mit->first->mnId == mnId) // 与当前关键帧本身不算共视continue;KFcounter[mit->first]++;}}// step2. 找到与当前关键帧共视程度超过15的关键帧,存入变量vPairs中vector<pair<int, KeyFrame *> > vPairs;int th = 15;int nmax = 0;KeyFrame *pKFmax = NULL; 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));(mit->first)->AddConnection(this, mit->second); // 对超过阈值的共视边建立连接}}// step3. 对关键帧按照共视权重降序排序,存入变量mvpOrderedConnectedKeyFrames和mvOrderedWeights中sort(vPairs.begin(), vPairs.end());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);}{unique_lock<mutex> lockCon(mMutexConnections);mConnectedKeyFrameWeights = KFcounter;mvpOrderedConnectedKeyFrames = vector<KeyFrame *>(lKFs.begin(), lKFs.end());mvOrderedWeights = vector<int>(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和父关键帧的成员变量mspChildrensunique_lock<mutex> lockCon(mMutexConnections);if (mbFirstConnection && mnId != 0) {mpParent = mvpOrderedConnectedKeyFrames.front();mpParent->AddChild(this);mbFirstConnection = false;}
}
对KeyFrame
的删除过程也采取先标记再清除的方式,参与回环检测的关键帧具有不被删除的特权
当一个关键帧被删除时,其父关键帧和所有子关键帧的生成树信息也会受到影响,需要为其所有子关键帧寻找新的父关键帧,如果父关键帧找的不好的话,就会产生回环,导致生成树就断开.
采用类似于最小生成树算法中的加边法重新构建生成树结构: 每次循环取权重最高的候选边建立父子连接关系,并将新加入生成树的子节点到加入候选父节点集合sParentCandidates
中
在回环检测中回环矫正函数在调用本质图BA优化前会先添加当前关键帧和闭环匹配关键帧之间的回环关系。