ORB-SLAM2源码笔记（4）——帧Frame和关键帧KeyFrame
2024-05-12 09:45:10
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优化前会先添加当前关键帧和闭环匹配关键帧之间的回环关系。
ORB-SLAM2源码笔记（4）——帧Frame和关键帧KeyFrame相关推荐
- ORB SLAM2源码解读(三)：Frame类
文章目录 前言 构造函数 双目相机 RGBD相机 单目相机 ExtractORB:提取特征点 ComputeBoW:计算词袋数据 SetPose:设置相机外参 isInFrustum:判断一个MapP ...
- Kernel源码笔记之调度：4.Fork系统调用
Kernel源码笔记目录 简介 fork系统调用是用户空间进程的起点,fork的流程中涉及了内核中大多数核心的基础组件,比如:文件系统,内存管理,namespace等等,execve用来执行一个程序, ...
- 《〈XNova/OGame〉源码笔记》（3－4）
<〈XNova/OGame〉源码笔记> 作者:赖勇浩(http://blog.csdn.net/lanphaday) 一. 用户验证 ogame/login.php 用户在登陆页面输入用户 ...
- angularjs源码笔记(3)--injector
2019独角兽企业重金招聘Python工程师标准>>> 简介 injector是用来做参数自动注入的,例如 function fn ($http, $scope, aService) ...
- spring aop原理_Spring知识点总结！已整理成142页离线文档（源码笔记+思维导图）...
写在前面 由于Spring家族的东西很多,一次性写完也不太现实.所以这一次先更新Spring[最核心]的知识点:AOP和IOC 无论是入门还是面试,理解AOP和IOC都是非常重要的.在面试的时候,我没 ...
- 数据结构源码笔记（C语言描述）汇总
数据结构源码笔记(C语言):英文单词按字典序排序的基数排序 数据结构源码笔记(C语言):直接插入排序 数据结构源码笔记(C语言):直接选择排序 数据结构源码笔记(C语言):置换-选择算法 数据结构源码 ...
- 数据结构源码笔记（C语言）：英文单词按字典序排序的基数排序
//实现英文单词按字典序排序的基数排序算法#include<stdio.h> #include<malloc.h> #include<string.h>#defin ...
- 数据结构源码笔记（C语言）：索引文件建立和查找
//实现索引文件建立和查找算法#include<stdio.h> #include<malloc.h> #include<string.h> #include< ...
- 数据结构源码笔记（C语言）：快速排序
//实现快速排序算法 #include<stdio.h> #include<malloc.h> #define MAXE 20typedef int KeyType; type ...
最新文章
- android2.2桌面,手机桌面课表软件
- 沉痛哀悼在汶川特大地震中的遇难者!!!!!
- 背包问题——01背包问题——饭卡
- SQL语句将表中数据导入到另一个数据库表中
- [汇编学习笔记][第十六章直接定址表]
- clickhouse语句_Siem落地方案：初识clickhouse
- 图像处理-HSL彩色图像均衡化
- mysql gis 高德_基于高德自定义地图数据的GIS矢量地图制作
- 如何使用PAUP4、MrBayes、TNT构建系统发育树
- 樱桃键盘驱动linux,樱桃键盘mx board9.0驱动
- 小样本算法库LibFewShot
- python相关性分析模型,相关性分析原理及Python实战
- 我的电脑被谁远程登陆过？怎么查看电脑远程登陆日志？
- 深入理解设计模式-责任链模式（职责链模式）
- 社保照片PS修改教程
- 祥云杯-re复现 （未完待续）
- 计算机组成原理第二章例题解析（下）
- python 椭圆曲线_Python、Sympy和椭圆曲线
- C++数据结构，三万字详解（强烈建议收藏）
- 爷青回！用原生 Audio API 实现一个千千静听