ORB-SLAM2局部建图LcalMapping函数说明

发布时间:2024年01月01日

LocalMapping::Run()函数说明

概述

LocalMapping::Run() 是ORB-SLAM2中的线程主函数,负责维护和优化地图以确保地图的一致性和质量。

输入输出:

无输入参数。无输出。

核心算法

LocalMapping线程的主要工作流程,包括处理新关键帧、剔除地图点、生成新地图点、融合重复点、局部捆绑调整、冗余关键帧剔除、闭环检测等任务,以维护ORB-SLAM2地图的一致性和质量。

  1. mpCurrentKeyFrame->ComputeBoW():将关键帧的特征点描述子转换为词袋模型(BoW),并将关键帧插入到地图中。
  2. MapPointCulling:根据地图点的观测情况,剔除质量不好的地图点。
  3. CreateNewMapPoints:通过当前关键帧与相邻关键帧的三角化操作生成新的地图点。
  4. SearchInNeighbors:检查并融合当前关键帧与相邻关键帧中重复的地图点。
  5. Local Bundle Adjustment:当局部地图中的关键帧数量大于2个时,执行局部捆绑调整(BA)。
  6. KeyFrameCulling:检测并剔除当前帧相邻的冗余关键帧。
  7. InsertKeyFrame:将当前关键帧加入闭环检测队列中。
运行步骤
  1. (Step 1)设置LocalMapping为繁忙,告知Tracking线程不接受新的关键帧。
  2. 主线程大循环(MainLoop):检查是否有新的关键帧(CheckKeyFrames):
    • 如果有新关键帧,则执行以下子步骤:
      • Step 2 处理新关键帧(ProcessKeyFrame),包括计算BoW、更新观测、描述子、共视图等。
      • (Step 3)。根据地图点的观测情况,剔除不合格的地图点(MapPointCulling)。
      • (Step 4)。创建新地图点(CreateNewMapPoints),当前关键帧与相邻关键帧通过三角化产生新的地图点,使跟踪更稳定。
      • (Step 5)。搜索邻近关键帧并融合重复点(SearchNeighbors):检查并融合当前关键帧与相邻关键帧中的重复地图点。
    • 如果没有新的关键帧,进入下一步。
  3. 检查是否有请求停止LocalMapping线程:
    • 如果有请求停止:
      • (Step 6)。进行局部BA调整 Optimizer::LocalBundleAdjustment,优化地图和相机位姿。
      • (Step 7)冗余关键帧剔除(KeyFrameCulling):检测并剔除当前帧相邻的关键帧中的冗余关键帧。
      • (Step 8)如果没有请求停止,将当前关键帧加入闭环检测队列(InsertKeyFrame)。
  4. 设置LocalMapping为非繁忙,允许接受新的关键帧。
  5. 检查是否已完成,如果未完成,则返回第2步,继续处理新的关键帧。
  6. 线程执行结束。
调用场合

本函数用于ORB-SLAM2的地图维护和优化,作为LocalMapping线程的主要工作函数,在LocalMapping线程启动时被调用。

总结

LocalMapping::Run()负责处理新关键帧、地图点的剔除和生成、局部捆绑调整、冗余关键帧的剔除以及闭环检测等任务,以维护ORB-SLAM2地图的一致性和质量。该函数通过多个步骤循环执行,直到满足终止条件,同时与Tracking线程进行协同工作,确保地图的连贯性。

LocalMapping::ProcessNewKeyFrame() 函数说明

概述

ProcessNewKeyFrame() 函数主要功能是处理新关键帧,包括关联地图点、更新地图点信息,以及将新关键帧插入到地图中。

输入: 无输入参数。

核心算法

ProcessNewKeyFrame() 调用了以下重要的函数,也就对应着主要执行步骤:

  1. mpCurrentKeyFrame->ComputeBoW():计算当前关键帧的词袋(Bag of Words)向量。
  2. mpCurrentKeyFrame->GetMapPointMatches():获取当前关键帧中的与地图点匹配的特征点。
  3. pMP->AddObservation(mpCurrentKeyFrame, i):为地图点添加关键帧的观测信息。
  4. pMP->UpdateNormalAndDepth():更新地图点的平均观测方向和观测距离。
  5. pMP->ComputeDistinctiveDescriptors():计算地图点的最佳描述子。
  6. mpCurrentKeyFrame->UpdateConnections():更新当前关键帧与其他关键帧之间的连接关系(共视图)。
  7. mpMap->AddKeyFrame(mpCurrentKeyFrame):将当前关键帧插入到地图中。
运行步骤

ProcessNewKeyFrame() 的执行步骤和功能是处理新的关键帧,包括计算词袋向量、更新观测、描述子、更新共视图连接关系,并将关键帧插入到地图中。

具体步骤如下:

  1. 从跟踪线程给定的关键帧队列中,取出最前面的关键帧,pop front(),作为当前要处理的关键帧。
  2. 计算当前关键帧特征点的词袋(BoW)表示ComputeBoW()。
  3. 遍历关键帧中所有的有效地图点,GetMapPointMatches,对地图点进行更新:
    • 如果地图点不是来自当前帧的观测(比如来自局部地图点),为当前地图点添加观测,并更新法线、深度和描述子(pMP->AddObservation(mpCurrentKeyFrame, i)pMP->UpdateNormalAndDepth()pMP->ComputeDistinctiveDescriptors())。
    • 如果地图点已经是当前关键帧的观测,将其添加到最近添加的地图点列表(mlpRecentAddedMapPoints.push_back(pMP)),等待后续MapPointCulling函数的检验。这些地图点可能来自双目或RGBD在创建关键帧中新生成的地图点,或者是CreateNewMapPoints 中通过三角化产生的。
  4. 更新当前关键帧与其他关键帧之间的连接关系,构建共视图(mpCurrentKeyFrame->UpdateConnections())。
  5. 将当前关键帧插入到ORB-SLAM2地图中(mpMap->AddKeyFrame(mpCurrentKeyFrame))。
调用场合

局部建图线程第一步操作,来自跟踪线程,等待处理的关键帧列表不为空,这些关键帧会进入一个队列中,等待局部建图线程的处理。

总结

ProcessNewKeyFrame()函数负责处理新的关键帧,它包括计算BoW表示、更新地图点信息、构建共视图以及将关键帧插入地图等多个步骤,以确保ORB-SLAM2地图的一致性和质量。

LocalMapping::MapPointCulling() 函数说明

概述

MapPointCulling() 函数用于检查新增的地图点,根据地图点的观测情况剔除质量不好的新增地图点,确保地图的质量和一致性。

输入输出
  • 输入:无输入参数。
  • 输出:无输出。
核心调用函数
  • pMP->GetFoundRatio(): 获取地图点的观测比例。具体来说,这个比例是通过以下方式计算的:
    • mnFound:地图点 pMP 被观测到的总帧数,包括所有观测。
    • mnVisible:根据地图点的预测,它应该在相机视野中出现的总帧数。
    • 返回 (mnFound ÷ mnVisible) 的值。如果这个比例很低,意味着这个地图点很少被观测到
  • lit = mlpRecentAddedMapPoints.erase(lit); 的作用是删除列表中指向的地图点,并将 lit 迭代器更新为指向删除后的下一个元素。用于将新增地图点从列表中删除。
  • MapPoint::SetBadFlag(): 将地图点标记为坏点。
  • MapPoint::isBad(): 用于检查地图点是否被标记为坏点。
运行步骤
  1. 设置观测阈值: 根据相机类型(单目或双目),设置观测阈值 nThObs
  2. 遍历检查新添加的地图点: 遍历最近添加的地图点列表 mlpRecentAddedMapPoints
  3. 检查地图点质量: 对于每个地图点,按照以下条件进行检查并可能剔除:
    • 如果地图点已被标记为坏点(isBad() 返回 true),则直接从列表中删除。
    • 如果地图点的观测比例(GetFoundRatio())小于 25%,则将其标记为坏点并从列表中删除。"发现比例"通常用来衡量一个地图点在相机的视野中有多少时间被观测到。
    • 如果从该地图点建立开始,到当前关键帧的时间间隔大于等于 2 个关键帧,并且观测到该地图点的相机数不超过观测阈值 cnThObs,则将其标记为坏点并从列表中删除。
    • 高质量的地图点。如果从该地图点建立开始,到当前关键帧的时间间隔大于等于 3 个关键帧,说明该地图点是质量较高的点,不标记为坏点。
调用场合

处理列表中的关键帧ProcessNewKeyFrame之后,CreateNewMapPoints也就是当前关键帧与相邻关键帧通过三角化产生新的地图点之前。

总结

MapPointCulling() 函数检查mlpRecentAddedMapPoints并剔除不合格的新增地图点。通过遍历最近添加的地图点列表,根据观测比例和相机数量等因素进行筛选和标记,确保地图中的地图点质量和一致性。

LocalMapping::CreateNewMapPoints() 函数说明

概述

CreateNewMapPoints() 函数用于根据当前关键帧与相邻关键帧之间的特征点匹配,通过三角化产生新的地图点,以提高地图的稳定性和密度。在局部建图线程中,会在共识关键帧之间重新进行特征匹配、三角化,生成新的地图点。这对于稳定的跟踪非常重要。

输入输出
  • 输入:无输入参数,但函数内部使用了当前关键帧(mpCurrentKeyFrame)以及地图(mpMap)等数据结构。
  • 输出:生成并添加新的地图点到地图(mpMap)中,同时将它们添加到检测队列(mlpRecentAddedMapPoints)中,以供后续检查。
核心调用函数
  • KeyFrame::GetBestCovisibilityKeyFrames(): 获取与当前关键帧具有最佳共视关系的相邻关键帧列表。
  • ORBmatcher::SearchForTriangulation(): 在相邻关键帧间搜索匹配的特征点对,并进行极线约束剔除误匹配。
  • KeyFrame::ComputeSceneMedianDepth(): 计算关键帧场景深度的中值。
  • ComputeF12(): 根据两个关键帧的位姿计算它们之间的基础矩阵。
  • cosParallaxRays < cosParallaxStereo 两次观测的匹配点射线夹角余弦值、双目相机观察这个点的时候的视差角余弦、
  • x3D = pKF2->UnprojectStereo(idx2)用双目信息恢复3D点。将图像上的一个特征点通过双目立体视觉的方法反投影到三维空间中,从而估算出该特征点在三维空间中的坐标。
  • MapPoint 类的成员函数被调用,如 AddObservation(), ComputeDistinctiveDescriptors(), UpdateNormalAndDepth()
简化步骤

执行步骤如下:

  1. 获取与当前关键帧具有最佳共视关系的相邻关键帧列表。
  2. 遍历相邻关键帧列表,对每一对相邻关键帧执行以下操作:
    • 检查相机运动基线长度,跳过基线过短的关键帧。
    • 计算基础矩阵和搜索匹配点对。
    • 对每对匹配点对进行三角化生成3D点,并进行多重条件筛选。
    • 将新生成的地图点添加到地图中。
  3. 完成相邻关键帧的遍历。
具体运行步骤
  1. Step 1:获取相邻关键帧列表:根据相机类型和设定的参数获取与当前关键帧具有最佳共视关系的相邻关键帧列表。分为3步:用 ORBmatcher进行特征点匹配,得到当前关键帧(左目)光心在世界坐标系中的坐标和内参,取出当前帧从世界坐标系到相机坐标系的变换矩阵。
  2. Step 2:循环遍历相邻关键帧:遍历相邻关键帧列表,对每一对相邻关键帧进行以下操作:
    • Step 3:检查相机运动基线长度:检查相邻关键帧之间的相机基线长度,如果基线过短(小于单目设定的阈值),则跳过这对关键帧,不生成3D点。单目三角化可行性的标准是,两个关键帧间的相机位移,除以场景相邻关键帧的场景深度中值。如果基线与景深的比例特别小,基线太短恢复3D点不准,那么跳过当前邻接的关键帧,不生成3D点。Step 3分为2步:1. 相减当前关键帧(左目)光心在世界坐标系中的坐标,与相邻的关键帧光心在世界坐标系中的坐标。获得两个关键帧间的相机位移 2. 相除两个关键帧间的相机基线长度,与相邻关键帧的场景深度中值。
    • Step 4:根据两个关键帧的位姿计算它们之间的基础矩阵。F12 = ComputeF12(mpCurrentKeyFrame, pKF2)
    • Step 5:搜索匹配点对:通过词袋法对两个关键帧未匹配的特征点,进行快速匹配,应用极线约束以剔除误匹配,生成新的匹配点对。matcher.SearchForTriangulation
    • Step 6:对每对匹配通过三角化生成3D点:和 Triangulate函数差不多。对每一对匹配点对进行以下子步骤:
      • Step 6.1: 获取匹配点在两个关键帧中的索引以及它们的特征点。当前匹配对在当前关键帧和邻接关键帧中的索引,当前匹配在当前关键帧中、和在邻接关键帧中的特征点。
      • Step 6.2: 利用匹配点反投影得到视差角余弦值。分为3步:1. 特征点反投影,得到的是在各自相机坐标系下的一个非归一化的方向向量。2. 由相机坐标系转到世界坐标系 3. 点乘向量匹配点射线夹角余弦值。
      • Step 6.3: 判断双目用哪对信息来三角化。对于同一个地图点,若左目匹配的射线夹角大于左右目匹配的射线夹角,用三角法恢复3D点。 否则,用双目相机自身的观测,用视差角更大的那个双目信息来恢复,直接用已知3D点反投影。
      • Step 6.4:用三角法恢复3D点的x3D。通过线性三角法或双目信息恢复3D点,如果匹配点对夹角大,用线性三角法。如果是双目相机,且视差角正常,则使用双目信息生成3D点,调用UnprojectStereo。
      • Step 6.5: 检查生成的3D点是否在相机前方,如果不在前方则放弃这个点,continue。
      • Step 6.6: 计算3D点在当前关键帧下的重投影误差、以及在另一个关键帧下的重投影误差。分为4步,1. 获取当前帧的sigmaSquare1,2. Tcw1乘以x3Dt,再计算相机坐标 3. 单目情况下假设测量有一个像素的偏差,2自由度卡方检验;双目情况下,自由度为3的卡方检验,来判断是否投影误差太大。如果误差太大,则放弃这个点,continue。4. 计算3D点在另一个关键帧下的重投影误差,操作同上。
      • Step 6.7: 检查尺度连续性,确保距离比例与图像金字塔的比例接近。分为2步,1. 减去x3D当前帧坐标,与当前关键帧光心在世界坐标系中的坐标,获得dist1;减去x3D当前帧坐标,与相邻关键帧光心在世界坐标系中的坐标,获得dist2;2. 比较x3D在两个关键帧中光心距离的比例,和图像金字塔的比例不应该差太多,否则放弃这个点,continue。
      • **Step 6.8:**确定是合格的地图点。三角化生成3D点成功后,构造成 MapPoint 对象。pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);
      • Step 6.9: 为地图点和关键帧、地图点的关系添加属性,包括观测到该点的关键帧、描述子等信息。地图点的操作为pMP ->AddObservation、pMP->ComputeDistinctiveDescriptors()、pMP->UpdateNormalAndDepth(); 当前帧的操作为pKF2->AddMapPoint;地图的操作为mpMap->AddMapPoint(pMP);
      • Step 6.10: 将新生成的地图点添加到检测队列。
    • Step 6 n matches for循环结束:对每对匹配通过三角化生成3D点
  3. Step 2 vp Neighbour KFs for循环结束:遍历相邻关键帧
调用场合

CreateNewMapPoints() 函数用于根据当前关键帧与相邻关键帧之间的匹配点,生成新的地图点。主要在LocalMapping::Run()线程中被调用,SearchInNeighbors之前,MapPointCulling之后,新建立的地图点会在后续通过 MapPointCulling() 函数进行检验。这有助于增加地图的稳定性和密度,提高系统的性能。

总结

CreateNewMapPoints() 函数通过三角化恢复3D点,同时进行匹配点质量检查和尺度连续性验证,以确保生成的地图点质量高且位于相机前方、重投影误差小、尺度范围合理。这些新地图点将被添加到地图中,供后续的定位和地图维护使用。

LocalMapping::SearchInNeighbors() 函数说明

函数概述:

SearchInNeighbors() 函数用于检查并融合当前关键帧与相邻帧(两级相邻)重复的地图点。这个函数主要处理地图点之间的匹配和融合,以维护和更新地图的一致性。具体地,它会在当前关键帧和一级、二级相邻关键帧之间进行投影匹配,并根据匹配结果融合地图点。

输入:
  • 无(函数内部使用当前关键帧和地图的数据)
输出:
  • 更新地图点的属性,如描述子、深度、平均观测方向等。
  • 更新关键帧之间的共视连接关系。
核心调用函数:
  • ORBmatcher::Fuse(): 用于地图点的投影匹配和融合操作。这个函数在 SearchInNeighbor() 函数中调用2次。正向匹配和融合: 第一次调用ORBmatcher::Fuse函数是将当前关键帧的地图点投影到一级相邻关键帧上进行匹配和融合。反向匹配和融合: 第二次调用ORBmatcher::Fuse函数是将一级相邻关键帧的地图点投影到当前关键帧上进行匹配和融合。
  • mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn); 获得和当前关键帧相邻的关键帧,也就是一级相邻关键帧
  • KeyFrame::GetMapPointMatches(): 获取与关键帧相关的地图点。
  • MapPoint::ComputeDistinctiveDescriptors(): 计算地图点的描述子。
  • MapPoint::UpdateNormalAndDepth(): 更新地图点的平均观测方向和深度。
  • KeyFrame::UpdateConnections(): 更新关键帧之间的共视连接关系。
运行步骤:

主要步骤有以下4步。1. 获取一级和二级相邻关键帧;2. 正向投影融合:将当前关键帧的地图点投影到相邻关键帧上,查找匹配并融合;3.反向投影融合:将相邻关键帧的地图点投影到当前关键帧上,查找匹配并投影融合。4.更新地图点属性:对当前关键帧的地图点进行属性更新。具体而言:

  1. Step 1 获得与当前关键帧相邻的一级相邻关键帧,这些关键帧被称为邻居关键帧,数量取决于相机类型(单目或双目/RGB-D)。对于单目情况,获取前20个邻居;对于双目或RGB-D情况,获取前10个邻居。
  2. Step 2 对于每个一级相邻关键帧,获取它的二级相邻关键帧,这些关键帧被认为是邻居的邻居。加入到相邻关键帧集合 vpTargetKFs 集合中。
  3. Step 3 使用特征点匹配器(ORBmatcher)进行正向投影融合。获取当前关键帧的具体的地图点,将当前关键帧的地图点投影到一级和二级相邻关键帧中,调用matcher.Fuse(pKFi,vpMapPointMatches)。Fuse函数寻找匹配的地图点,如果找到匹配的地图点,它们可能会被融合。包括替换地图点或添加新地图点,规则按照1. 如果地图点能匹配关键帧的特征点,并且该点有对应的地图点,那么选择观测数目多的替换两个地图点。规则2;如果地图点能匹配关键帧的特征点,并且该点没有对应的地图点,那么为该点添加该投影地图点。
  4. Step 4.1 收集需要进行反向投影融合的一级和二级邻接关键帧的地图点,并存储在候选融合点集合 vpFuseCandidates 中。如果地图点是坏点,或者已经加进集合候选融合点集合,跳过。
  5. Step 4.2:使用特征点匹配器(ORBmatcher)进行反向投影融合。将所有的候选融合的地图点投影到当前关键帧中,调用matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates)。Fuse函数寻找匹配的地图点,如果找到匹配的地图点,它们也可能会被融合。同样包括替换或添加地图点。
  6. Step 5 更新当前关键帧地图点的描述子、深度、平均观测方向等属性。
  7. Step 6 更新当前关键帧与其它关键帧之间的共视连接关系(covisibility graph)。
调用场合:

SearchInNeighbors() 函数主要在LocalMapping::Run()线程中被调用,在已经处理完队列中的最后的一个关键帧之后(CreateNewMapPoints以及not CheckNewKeyFrames())之后,局部地图BA之前(LocalBundleAdjustment之前)。

局部建图线程中,地图点产生了较大的变动。比如前面讲过的,在共事关键帧之间重新进行特征匹配三角化。生成新的地图点。又如,前面根据一定的规则剔除不合格的地图点,这时就需要对已有的地图点进行整理。包括合并重复的地图点。用更准确的地图点替换旧的地图点,最后统一更新地图点的描述子、深度、平均观测方向等属性。

总结:

SearchInNeighbors() 通过地图点投影匹配,寻找并融合当前关键帧与其相邻帧之间的重复地图点,然后更新地图点属性和关键帧之间的共视关系。主要步骤是当前关键帧与其相邻帧之间地图点的正向投影融合与反向投影融合,前者检查当前关键帧的地图点与一级相邻关键帧上的地图点是否一致,以及是否有新的地图点可以在一级相邻关键帧上生成;后者检查一级相邻关键帧的地图点是否与当前关键帧上的地图点一致,以及是否有新的地图点可以在当前关键帧上生成。函数确保地图点的一致性,减少重复地图点的数量,以保持地图的稳定性和准确性。

ORBmatcher::Fuse(pKF, vpMapPoints, th)函数说明

概述:

ORBmatcher::Fuse函数用于将地图点投影到关键帧中进行匹配和融合,实现地图点的更新和扩充。该函数的核心目标是在给定关键帧和一组待投影的地图点之间建立关联,然后根据匹配的情况对地图点进行融合或添加新的观测信息。

输入输出:
  • 输入参数包括关键帧(pKF)、待投影的地图点(vpMapPoints)、搜索窗口的阈值(th)。
  • 输出是更新地图点的数量(nFused)。
核心算法:

ORBmatcher::Fuse函数调用了以下重要的函数:

  1. pKF->GetRotation()和GetTranslation(): 用于获取关键帧的旋转和平移信息。
  2. pMP->GetWorldPos() 获得地图点在世界坐标系下的位置
  3. pMP->GetDescriptor(); 获得地图点描述子
  4. pKF->IsInImage(): 输入坐标,用于检查投影点是否在图像范围内。
  5. pMP->PredictScale(dist3D,pKF): 根据地图点到相机光心的距离来预测匹配点所在的金字塔尺度。
  6. GetFeaturesInArea(): 根据投影点和搜索半径,在关键帧图像中找到候选匹配点的索引。
  7. DescriptorDistance(): 用于计算两个描述子之间的距离,用于匹配。
  8. pMP->AddObservation(pKF,best Index); 关键帧中的最佳匹配的地图点,向地图点添加对应的关键帧信息
  9. pKF->AddMapPoint(pMP,best Index); 向关键帧中,添加最佳地图点的信息
运行步骤:
  1. 获取当前关键帧的位姿、内参、光心坐标以及其他参数。
  2. 遍历待投影的地图点(vpMapPoints列表),对每一个待投影的地图点进行以下操作:
    • a. 检查地图点的有效性,排除无效地图点或已经是当前关键帧的地图点的情况。
    • b. 获得地图点在当前关键帧的相机坐标系下的3D位置。 检查深度值是否为正,若为负则跳过。
    • c. 计算地图点在当前关键帧上的投影图像坐标。检查投影点是否在当前关键帧的图像范围内。
    • d 检查地图点的观测方向与当前关键帧相机光心到地图点的连线夹角是否小于60度。
    • e 确定地图点在金字塔中的搜索范围。根据地图点到相机光心的距离,预测匹配点所在的金字塔尺度级别。根据金字塔尺度级别和输入的搜索窗口阈值,确定在图像中搜索匹配点的范围。
  3. Step 5 获得地图点在投影点附近搜索窗口内,找到的候选匹配点的索引
  4. Step 6 遍历候选特征点,寻找最佳匹配点
    • a. 金字塔层级要接近(同一层或小一层),否则跳过
    • b. 计算投影点与候选匹配特征点的距离,如果偏差很大,直接跳过。分单目和双目情况进行处理,如果是双目相机,计算投影点在右目图像上的图像坐标。 如果是单目,按自由度为2的卡方检验进行滤除。如果是双目,按自由度为3的卡方检验进行滤除投影点与候选匹配特征点的距离太大的点。
    • c. 查找投影点的描述子最小的距离,记录半径内距离最小的坐标。
  5. Step 7 找到投影点对应的最佳匹配特征点,根据是否存在地图点来融合或新增。如果找到了最佳匹配点且距离小于阈值TH_LOW,则执行以下操作:
    • a. 如果最佳匹配点已经有对应的地图点,选择被观测次数多的地图点来保留,或者创建新的地图点。
    • b. 更新地图点的观测信息,包括在当前关键帧上的观测和描述子。
  6. 更新当前关键帧的地图点列表。
  7. 所有的待投影地图点 vpMapPoints 循环检查完毕,返回融合后地图点的数量。
调用场合:

该函数通常在局部建图模块的地图点更新和扩充过程中被调用,LocalMapping::SearchInNeighbor() 函数中调用2次,用于正向匹配和融合,反向匹配和融合。用于将新的地图点与关键帧进行匹配和融合。

总结:

ORBmatcher::Fuse函数用于将待投影的地图点与给定关键帧进行匹配,如果找到匹配点,则根据描述子距离和阈值来决定是融合已有地图点还是添加新的地图点。这一过程有助于更新和扩充地图,提高建图的精度和稳定性。

LocalMapping::KeyFrameCulling() 函数说明

概述

KeyFrameCulling() 函数用于检测当前关键帧在共视图中的关键帧,并根据地图点在共视图中的冗余程度来剔除冗余的共视关键帧。冗余关键帧是指那些观测到的地图点在其他关键帧中也有足够数量的观测的关键帧。这个操作有助于优化地图的效率和一致性。

输入输出

输入:

  • 无(函数内部使用当前关键帧和地图的数据)

输出:

  • 可能会将某些关键帧标记为"坏帧",即需要剔除的关键帧。

变量:

  • mpCurrentKeyFrame:当前关键帧,本程序就是判断它是否需要删除
  • pKF: mpCurrentKeyFrame的某一个共视关键帧
  • vpMapPoints:pKF对应的所有地图点
  • pMP:vpMapPoints中的某个地图点
  • observations:所有能观测到pMP的关键帧
  • pKF i:observations中的某个关键帧
  • scaleLevel i:pKFi的金字塔尺度
  • scaleLevel:pKF的金字塔尺度
核心调用函数
  • vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames(): 获取当前关键帧在共视图中的所有共视关键帧。
  • vpMapPoints = pKF->GetMapPointMatches();: 获取关键帧观测到的地图点。
  • pMP->isBad(): 检查地图点是否被标记为"坏点"。
  • pMP->Observations(): 获取地图点被观测到的相机数量。
  • map<KeyFrame*, size-t> observations = pMP->GetObservations(): 获取观测到地图点的关键帧列表。
  • pKF->SetBadFlag(): 将关键帧标记为"坏帧",需要剔除。
运行步骤

整体步骤为3层for循环。对所有的共视关键帧进行遍历,遍历该共视关键帧的所有地图点,判定冗余地图点。通过判定观测到该地图点的关键帧是否超过3个,被3个关键帧观测到就是冗余地图点。如果该关键帧90%以上的有效地图点被判断为冗余的,则认为该关键帧是冗余的,需要删除该关键帧。具体步骤如下:

  1. 获取当前关键帧在共视图中的所有共视关键帧(局部关键帧)。
  2. 遍历每个局部共视关键帧,获取其观测到的地图点。对于第一个关键帧(ID为0)跳过,不做处理。
  3. 遍历该共视关键帧的所有地图点,对于有效的地图点,记录其观测到的关键帧数。有效指的是,不包括当前关键帧,非坏点,且满足条件的地图点。对于双目/RGB-D条件,仅考虑近处(不超过基线的40倍 )的地图点。观测到该地图点的相机总数目,即观测次数大于阈值。
  4. 在能看到该地图点的所有关键帧的集合内,(map类型的observations )遍历观测到该地图点的关键帧。要求观测到该地图点的其他关键帧对应二维特征点所在金字塔层级,小于或等于当前金字塔层级。
  5. 如果某地图点在至少3个关键帧中被观测到(满足条件),则将该地图点标记为冗余地图点。
  6. 如果某关键帧的90%以上的地图点都被标记为冗余地图点,则将该关键帧标记为"坏帧",即冗余关键帧。
调用场合

在少数恶劣情况下需要通过加入更多关键帧来提高跟踪成功率。但太多关键帧会使得局部BA变得非常慢,因此需要及时剔除冗余关键帧。该函数通常在局部地图优化的过程中调用,闭环检测队列添加当前关键帧之前,在局部BA之后。

总结

KeyFrameCulling() 是一个用于冗余关键帧检测和剔除的关键函数。它通过分析每个关键帧观测到的地图点,判断它所具有的大部分地图点是否可以被其他关键帧替代?具体而言是是90%的大部分地图点,以及3个其他关键帧。

文章来源:https://blog.csdn.net/NICAI001/article/details/135322456
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。