ORB-SLAM2重要函数总结(1)
ORBFeature
IC_Angle
通过遍历特征点周围的像素,利用灰度质心法来计算特征点的角度朝向。具体地,它根据像素值和像素坐标的加权和来计算方向角度,使特征点具有旋转不变性。computeOrientation
函数通过遍历一组特征点的方向角度,并为每个特征点调用IC_Angle
函数来计算其方向角度,以实现特征点的旋转不变性,从而提高特征匹配的鲁棒性和性能。keypoints
中的每个特征点的angle
属性被更新。computeOrbDescriptor
用于计算ORB特征点的描述子。它采用了Steer BRIEF算法,遍历描述子的32个字节,每个字节根据预定义的采样模板进行大小比较,将0或1的结果保存到描述子中。最终得到一个256位的ORB描述子。ORBextractor::DistributeOctTree
函数使用四叉树法对图像金字塔图层中的特征点进行平均分配,将输入的特征点均匀分布到图像的不同区域,并保留每个区域中响应值最大的一个特征点,以实现特征点的均匀分布和响应值最大化,适用于特征点提取的优化和均匀分布需求。ExtractorNode::DivideNode
函数用于将ORB 特征提取器节点分成四个子节点,将当前节点分成四个子节点,并将图像上的特征点分配给这些子节点。函数完成图像区域的划分、特征点归属的划分,从而实现更好的特征点分布和性能。ORBextractor::ORBextractor
ORB特征点提取器的构造函数,初始化ORB特征点提取器,设置各种参数和数据结构包括参数、金字塔构建、特征点数量分配、随机采样点的准备等,为后续在图像上执行ORB特征点提取做好了准备工作。ORBextractor::ComputeKeyPointsOctTree
用于在图像金字塔的不同层级上提取特征点,并确保这些特征点在图像中均匀分布。此外,它还计算了每个特征点的方向信息,以便在后续的特征匹配中使用。函数的核心思想是使用图像金字塔和四叉树结构来有效地提取和管理特征点。调用OpenCV在图像块中检测FAST角点,DistributeOctTree
函数和computeOrientation
函数。
ORBmatcher
- ``ORBmatcher::DescriptorDistance`函数用于计算两个ORB特征描述子之间的汉明距离,数值越小表示两个描述子越相似,用于度量两个特征点相似度。通过在两个特征点的ORB描述子之间计算异或然后统计不同位而实现,用来确定最佳匹配。
ORBmatcher::ComputeThreeMaxima
从一组直方图中识别出最大的三个峰值,这用于在分析和筛选匹配特征点时保证旋转一致性。ORBmatcher::SearchForInitialization
单目初始化,用于在两个初始帧之间找到特征点匹配,主要是参考帧和当前帧的特征点匹配,包括特征点逐一匹配、正反一致性检查、旋转一致性检查、剔除误匹配等步骤。初始化以便估计初始相机位姿和构建初始地图。ORBmatcher::SearchByProjection
通过将地图点投影到帧中来寻找匹配点对关系。共有4个重载版本,可以在不同的场合下使用,包括重定位、跟踪、初始化、局部地图更新和闭环检测等,闭环版本输入还有旋转矩阵。通常包含如下步骤:1.遍历地图点; 2.将每个地图点投影到当前帧上,计算图像坐标; 3.定义在当前帧中搜索匹配特征点的区域;4.在投影位置附近,根据ORB描述子距离搜索候选匹配点;5.根据描述子相似度确定最佳特征点匹配;6.筛选匹配点,如旋转和尺度一致性检查;7.更新匹配信息。重定位版本的函数还涉及地图点是否在当前帧视野内的可视性检查,计算预测的尺度等。ORBmatcher::SearchByBoW
用于在两个关键帧(重载版本1)或用于在一个关键帧和一个普通帧之间(重载版本2)进行特征点匹配。前者用于关键帧之间的追踪或闭环检测,后者用于重定位和追踪过程中。通常包含如下步骤:1.利用BoW模型找到候选匹配点;2.计算匹配点对在两个关键帧中的旋转角度差;3. 构建并统计旋转差直方图;4.直方图旋转一致性筛选;5.匹配验证和确定。此函数利用词袋模型快速匹配特征点,可以显著提高匹配的速度和效率。ORBmatcher::SearchBySim3
用于在pKF1
和pKF2
之间进行特征点匹配,利用Sim3变换(尺度、旋转和平移)来寻找更多的匹配点对。通常在闭环检测过程中调用,用于寻找闭环候选关键帧与当前帧之间的Sim3变换。通常包含如下步骤:1. 计算Sim3的逆矩阵;2.获取两个关键帧中的地图点;3.通过Sim3变换,地图点投影到图像并在合适的深度范围内搜索候选匹配点;4.在合适的金字塔层级搜索半径内查找匹配;5.检查两次匹配的一致性。此函数避免了闭环使用BoW匹配方法可能导致的匹配遗漏。ORBmatcher::SearchForTriangulation
用于在两个关键帧之间找到匹配的特征点,这些匹配点随后可以用于三角化新的地图点。主要在双目和RGB-D SLAM中三角化创建新地图点。通常包含如下步骤:1. 计算KF1的相机中心投影到KF2的图像坐标;2.利用BoW快速找到候选的匹配特征点;3.使用基础矩阵F12
对匹配点进行极线约束检查;4.视觉金字塔筛选地图点;5.构建并统计旋转差异直方图;6.剔除旋转差异较大的错误匹配;7.成功的匹配点对用于后续的三角化。ORBmatcher::Fuse
用于将地图点投影到指定的关键帧上进行匹配和融合。有2个重载版本,用于地图点的重用和更新,特别是在关键帧之间共享视觉信息时。一般包括以下步骤:1.遍历待投影的地图点;2.地图点的投影和匹配;3.通过描述子距离验证匹配有效性,4.通过多个条件判定,包括:是否在图像坐标范围内、深度值检查、检查地图点到关键帧相机光心的连线与地图点的平均观测向量之间的夹角、通过预测金字塔层级和候选窗口半径查找,最终检查描述子距离是否小于阈值; 5. 合并重复的地图点或更新匹配信息。函数通过识别和合并重复的地图点,帮助系统减少冗余,并于维护和优化地图。
MapPoint
map<KeyFrame*, size_t> MapPoint::GetObservations()
返回mObservations,记录了可以观测到当前地图点的所有关键帧,以及该地图点在对应关键帧中的索引;void MapPoint::AddObservation(KeyFrame* pKF, size_t idx)
:向地图点添加一个观测,用于建立地图点与关键帧的连接。记录哪些 KeyFrame 的那个特征点能观测到该地图点,并增加观测的相机数目nObs。MapPoint::SetWorldPos(const cv::Mat &Pos)
设置地图点在世界坐标系下的坐标。MapPoint::ComputeDistinctiveDescriptors()
用于计算地图点最具代表性的描述子,最好的描述子与其他描述子应该具有最小的距离中值。在地图点被创建或更新时调用。cv::Mat MapPoint::GetDescriptor()
获取当前地图点的描述子,也就是地图点最具代表性的描述子mDescriptor。- ``MapPoint::UpdateNormalAndDepth()`:根据观测关键帧的姿态计算地图点的平均观测方向以及观测距离范围,更新地图点的法向量和深度。这个函数用于在地图点被观测到或更新时调用,地图点从而能够更好地与其他关键帧和地图点进行匹配和校正。
MapPoint::SetBadFlag()
标记当前地图点为“坏点”,通知所有观测到该地图点的关键帧删除观测。步骤为:获取地图点的观测记录,按照观测到该MapPoint的KF和该MapPoint在KF中的索引,遍历关键帧释放观测记录。该函数通常在定位或关键帧插入阶段被调用。MapPoint::EraseObservation(KeyFrame *pKF)
:删除某个关键帧对当前地图点的观测,通常在关键帧被删除时调用。根据单目和双目类型从当前地图点的被观测次数中减去nObs,当观测到该点的相机数目少于2时,丢弃该点。
KeyFrame
KeyFrame::UpdateConnections
更新关键帧之间的共视关系和连接关系。该函数通常在关键帧生成和地图点变化时被调用。运行步骤包括: 1. 统计该关键帧每个地图点有多少个关键帧观测,获得地图点的共视关系;2.筛选连接关系,>=15个共视地图点则与关键帧建立连接关系;3.排序共视关键帧的权重,创建与该关键帧连接的前N个最强共视关键;4.最大共视关键帧为父关键帧,更新父子关系。该函数通过统计关键帧之间的共视程度,构建关键帧之间的连接关系图,用于后续的图优化和回环检测中。KeyFrame::AddConnection(KeyFrame *pKF, const int &weight)
为当前关键帧新建、或更新,和其他关键帧pKF的连接权重。连接关系变化后,更新最佳共视权重,重新进行排序。KeyFrame::UpdateBestCovisibles()
:按照权重从大到小对连接(共视)的关键帧进行排序。这个函数用于维护关键帧之间的共视关系,并在关键帧之间建立拓扑关系。KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx)
:将一个地图点与关键帧关联起来,记录地图点在关键帧中的观测。这个函数在关键帧生成时以及重定位、地图点添加等情况下调用,用于建立地图点与关键帧的连接。set<MapPoint*> KeyFrame::GetMapPoints()
获取当前关键帧中的所有地图点,遍历mvpMapPoints返回set.KeyFrame::EraseMapPointMatch(const size_t &idx)
:删除关键帧中某个观测的地图点。KeyFrame::EraseConnection(KeyFrame* pKF)
删除当前关键帧和指定关键帧之间的共视关系。删除则会标记bUpdate
为true
,以便后续更新最佳共视关系。KeyFrame::SetBadFlag()
:删除关键帧及其和其他所有关键帧、地图点之间的连接关系,其中连接关系的删除涉及为其所有子关键帧寻找新的父关键帧。这一步通过迭代子关键帧查找有连接关系的候选父关键帧,同时在迭代候选父时寻找最高共视程度的父关键帧,然后ChangeParent。这个函数在闭环检测和关键帧剔除时调用。
Initializer
Initializer::Normalize
特征点坐标归一化,以特征点X,Y坐标离均值的平均偏离程度为缩放尺度,乘以坐标完成缩放。函数目的是为了让噪声对于图像的影响在一个数量级上。cv::Mat Initializer::ComputeH21
用DLT方法求解单应矩阵H。函数输入参考帧和当前帧中归一化后的特征点,利用了SVD分解。函数用于平面场景的位姿估计。Initializer::CheckHomography
检验单应矩阵的好坏。输入参考帧与当前帧的单应矩阵F12和F21,以及匹配好的特征点对,利用卡方检验对内点的双向重投影误差进行计算。cv::Mat Initializer::ComputeF21
利用8对特征点求解基础矩阵。用于一般非平面场景的位姿估计,需要用到2次奇异值分解。Initializer::CheckFundamental
检验基础矩阵好坏。利用F12和匹配点对,对内点的点对到对应极线的距离来计算评分。Initializer::Triangulate
特征点对三角化。输入两帧位姿的投影矩阵P1,P2和图像上的匹配特征点kp1,kp2,利用SVD分解计算三维点坐标。Initializer::CheckRT
对特征匹配点进行三角化,筛选出合格的三维点。输入参考帧的特征点集合及匹配关系、帧之间的旋转和平移,运行步骤如下:1. 特征点对三角化,得到三维点坐标;2. 检查它们的深度与光心的视差角;3.检查重投影误差不大;4.统计三维点的视差角并排序。函数运行筛查后初始化可产生地图点。Frame::ComputeStereoMatches
执行双目稀疏立体匹配,将左图中的每一个特征点与右图中的匹配点进行匹配。首先,根据基线和描述子距离进行粗匹配,然后使用块匹配进行精确匹配,再使用抛物线极值进行亚像素插值,最终选择最佳匹配点并删除离群点。函数涉及使用图像块滑动窗口内用差的绝对和SAD进行精确匹配。该函数是双目初始化的重要一步。
Tracking
MonocularInitialization
单目摄像头设置中初始化跟踪和地图构建过程。有以下步骤:1.特征提取与匹配:在连续的视频帧中提取特征点,并进行特征匹配。2.运动估计:使用单应矩阵(H模型)或基础矩阵(F模型)估计两帧间的相对运动。3.三角化:基于估计的运动和匹配的特征点进行三角化,生成3D点。StereoInitialization
函数负责在双目或RGB-D摄像头设置中初始化跟踪过程。函数利用深度信息从观测到的特征中生成3D点。然后在世界坐标系中创建初始的地图点集合,并建立第一个关键帧。Tracking::Track
Tracking::TrackReferenceKeyFrame
Tracking::UpdateLastFrame
Tracking::TrackWithMotionModel
Tracking::Relocalization
Tracking::UpdateLocalKeyFrames
Tracking::UpdateLocalPoints
Tracking::SearchLocalPoints
Tracking::TrackLocalMap
Tracking::TrackReferenceKeyFrame
Tracking::NeedNewKeyFrame
Tracking::CreateNewKeyFrame
LocalMapping
LocalMapping::Run()
LocalMapping::ProcessNewKeyFrame()
LocalMapping::MapPointCulling()
LocalMapping::CreateNewMapPoints()
LocalMapping::SearchInNeighbors()
ORBmatcher::Fuse(pKF, vpMapPoints, th)
LocalMapping::KeyFrameCulling()
LoopClosing
LoopClosing::Run()
LoopClosing::DetectLoop()
KeyFrameDatabase::DetectLoopCandidates()
LoopClosing::ComputeSim3()
LoopClosing::CorrectLoop()
Optimizer
Optimizer::PoseOptimization
Optimizer::LocalBundleAdjustment
Optimizer::OptimizeSim3
- ``Optimizer::OptimizeEssentialGraph`
Optimizer::BundleAdjustment