03 地图点 MapPoint

文章目录

    • [03 地图点 MapPoint](#03 地图点 MapPoint)
      • [3.1 各成员/变量](#3.1 各成员/变量)
      • [3.2 观测尺度](#3.2 观测尺度)
      • [3.2.1 平均观测距离 `mfMinDistance` 和 `mfMaxDistance`](#3.2.1 平均观测距离 mfMinDistancemfMaxDistance)
      • [3.2.2 更新平均观测方向和距离: `UpdateNormalAndDepth()`](#3.2.2 更新平均观测方向和距离: UpdateNormalAndDepth())
      • [3.3 特征描述子](#3.3 特征描述子)
      • [3.4 地图点的删除与替换](#3.4 地图点的删除与替换)
        • [3.4.1 地图点的删除 `SetBadFlag()`](#3.4.1 地图点的删除 SetBadFlag())
        • [3.4.2 地图点的替换 `Replace()`](#3.4.2 地图点的替换 Replace())
      • [3.5 MapPoint 类的用途](#3.5 MapPoint 类的用途)
        • [3.5.1 MapPoint 的生命周期](#3.5.1 MapPoint 的生命周期)

03 地图点 MapPoint

3.1 各成员/变量

(1)地图点的世界坐标 mWorldPos

成员函数/变量 访问控制 意义
cv::Mat mWorldPos protected 地图点在世界坐标系下的坐标
cv::Mat GetWorldPos() public 获取 mWorldPos 的值
void SetWorldPos(const cv::Mat &Pos) public 设置 mWorldPos 的值
std::mutex mMutexFeatures protected mWorldPos 的锁

(2)与关键帧的观测关系 mObservations

成员函数/变量 访问控制 意义
std::map<KeyFrame*,size_t> mObservations protected 观测到该 MapPoint 的关键帧和该 MapPoint 在关键帧中的索引
std::map<KeyFrame*,size_t> GetObservations() public mObservations 的get方法
void AddObservation(KeyFrame* pKF,size_t idx) public 增加地图点的观测关系
void EraseObservation(KeyFrame* pKF) public 删除观测关系
bool IsInKeyFrame(KeyFrame* pKF) protected 查询当前地图点是否在某 KeyFrame
int GetIndexInKeyFrame(KeyFrame* pKF) public 查询当前地图点在某 KeyFrame 中的索引
int nObs public 记录当前地图点被多少相机观测到单目帧每次观测加1,双目帧每次观测加2
int Observations() public nObs 的 get 方法

std::map<KeyFrame*,size_t> mObservations 保存了当前地图点对关键帧 KeyFrame 的观测关系,key 为某个关键帧,value 为当期地图点在该关键帧中的索引。

函数 AddObservation()EraseObservation() 同时维护 mObservationsnObs

cpp 复制代码
/**
 * @brief 添加观测
 *
 * 记录哪些KeyFrame的那个特征点能观测到该MapPoint \n
 * 并增加观测的相机数目nObs,单目+1,双目或者grbd+2
 * 这个函数是建立关键帧共视关系的核心函数,能共同观测到某些MapPoints的关键帧是共视关键帧
 * @param pKF KeyFrame
 * @param idx MapPoint在KeyFrame中的索引
 */
void MapPoint::AddObservation(KeyFrame* pKF, size_t idx)
{
    unique_lock<mutex> lock(mMutexFeatures);
    if(mObservations.count(pKF))   // 若已经存在,不做处理
        return;
    // 记录下能观测到该MapPoint的KF和该MapPoint在KF中的索引
    mObservations[pKF]=idx;

    if(pKF->mvuRight[idx]>=0)
        nObs+=2; // 双目或者grbd
    else
        nObs++; // 单目
}

3.2 观测尺度

成员变量/函数 访问控制 意义
cv::Mat mNormalVector protected 平均观测方向
float mfMinDistance protected 平均观测距离的下限
float mfMaxDistance protected 平均观测距离的上限
cv::Mat GetWorldPos() public mNormalVector 的 get 方法
float GetMinDistanceInvariance() public mfMinDistance 的 get 方法
float GetMaxDistanceInvariance() public mfMaxDistance 的 get 方法
void UpdateNormalAndDepth() public 更新平均观测距离和方向
int PredictScale(const float &currentDist, KeyFrame*pKF) int PredictScale(const float &currentDist, Frame* pF) public 估计当前地图点在某 Frame 中对应特征点的金字塔层级
KeyFrame* mpRefKF protected 当前地图点的参考关键帧
KeyFrame* GetReferenceKeyFrame() public mpRefKF 的 get 方法

3.2.1 平均观测距离 mfMinDistancemfMaxDistance

特征点的观测距离与其在图像金字塔中的图层呈线性关系。图层越低,分辨率越高,越能看见远处的物体。反过来说,如果一个图像区域被放大后才能被识别出来,则说明该区域的观测深度较深。

距离较近的地图点,将在金字塔层数较高的地方提取出,距离较远的地图点,在金字塔层数较低的地方提取出。

特征点的平均观测距离的上下限由成员变量 mfMinDistancemfMaxDistance 表示:

  • mfMaxDistance 地图点匹配在图像金字塔第 0 层时的距离

  • mfMinDistance 地图点匹配在图像金字塔第 7 层时的距离

这两个变量是基于地图点在其参考关键帧上的观测得到的。

根据层级算深度

cpp 复制代码
// pFrame是当前MapPoint的参考帧
const int level = pFrame->mvKeysUn[idxF].octave;
const float levelScaleFactor = pFrame->mvScaleFactors[level];
const int nLevels = pFrame->mnScaleLevels;
mfMaxDistance = dist*levelScaleFactor;                              
mfMinDistance = mfMaxDistance/pFrame->mvScaleFactors[nLevels-1];    

函数 int PredictScale(const float &currentDist, KeyFrame* pKF)int PredictScale(const float &currentDist, Frame* pF) 根据某地图点到某帧的观测深度估计其在该帧图片上的层级,是上述过程的逆运算。

mfMaxDistance currentDist = 1. 2 level level = ⌈ log ⁡ 1.2 ( mfMaxDistance currentDist ) ⌉ \begin{gathered} \frac{\text { mfMaxDistance }}{\text { currentDist }}=1.2^{\text {level }} \\ \text { level }=\left\lceil\log _{1.2}\left(\frac{\text { mfMaxDistance }}{\text { currentDist }}\right)\right\rceil \end{gathered} currentDist mfMaxDistance =1.2level level =⌈log1.2( currentDist mfMaxDistance )⌉

cpp 复制代码
int MapPoint::PredictScale(const float &currentDist, KeyFrame* pKF) {
    float ratio;
    {
        unique_lock<mutex> lock(mMutexPos);
        ratio = mfMaxDistance/currentDist;
    }

    int nScale = ceil(log(ratio)/pKF->mfLogScaleFactor);
    if(nScale<0)
        nScale = 0;
    else if(nScale>=pKF->mnScaleLevels)
        nScale = pKF->mnScaleLevels-1;

    return nScale;
}

3.2.2 更新平均观测方向和距离: UpdateNormalAndDepth()

函数 UpdateNormalAndDepth() 更新当前地图点的平均观测方向和距离,其中平均观测方向是根据 mObservations 中所有观测到本地图的关键帧取平均得到,平均观测距离是根据参考关键帧得到的。

cpp 复制代码
/**
 * @brief 更新平均观测方向以及观测距离范围
 *
 * 由于一个MapPoint会被许多相机观测到,因此在插入关键帧后,需要更新相应变量
 * mNormalVector:3D点被观测的平均方向
 * mfMaxDistance:观测到该3D点的最大距离
 * mfMinDistance:观测到该3D点的最小距离
 * @see III - C2.2 c2.4
 */
void MapPoint::UpdateNormalAndDepth()
{
    map<KeyFrame*,size_t> observations;
    KeyFrame* pRefKF;
    cv::Mat Pos;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        if(mbBad)
            return;

        observations=mObservations; // 获得观测到该3d点的所有关键帧
        pRefKF=mpRefKF;             // 观测到该点的参考关键帧
        Pos = mWorldPos.clone();    // 3d点在世界坐标系中的位置
    }

    if(observations.empty())
        return;

    cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
    int n=0;
    for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;
        cv::Mat Owi = pKF->GetCameraCenter();
        cv::Mat normali = mWorldPos - Owi;
        normal = normal + normali/cv::norm(normali); // 对所有关键帧对该点的观测方向归一化为单位向量进行求和
        n++;
    } 

    cv::Mat PC = Pos - pRefKF->GetCameraCenter(); // 参考关键帧相机指向3D点的向量(在世界坐标系下的表示)
    const float dist = cv::norm(PC); // 该点到参考关键帧相机的距离
    const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;
    const float levelScaleFactor =  pRefKF->mvScaleFactors[level];
    const int nLevels = pRefKF->mnScaleLevels; // 金字塔层数

    {
        unique_lock<mutex> lock3(mMutexPos);
        // 另见PredictScale函数前的注释
        mfMaxDistance = dist*levelScaleFactor;                           // 观测到该点的距离最大值
        mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1]; // 观测到该点的距离最小值
        mNormalVector = normal/n;                                        // 获得平均的观测方向
    }
}

地图点的平均观测距离是根据其参考关键帧计算的,那么参考关键帧 KeyFrame* mpRefKF 是如何指定的呢?

  • 构造函数中,创建该地图点的参考帧被设为参考关键帧.

  • 若当前地图点对参考关键帧的观测被删除,则取第一个观测到当前地图点的关键帧做参考关键帧。

函数 UpdateNormalAndDepth() 的调用时机:

(1)创建地图点时调用 UpdateNormalAndDepth() 初始化其观测信息;

(2)地图点对关键帧的观测 mObservations 更新时(跟踪局部地图添加或删除对关键帧的观测时、LocalMapping 线程删除冗余关键帧时或 LoopClosing 线程闭环矫正时),调用 UpdateNormalAndDepth() 初始化其观测信息.

(3)地图点世界坐标 mWorldPos 发生变化时(BA 优化之后),调用 UpdateNormalAndDepth() 初始化其观测信息。

总而言之,只要地图点本身或关键帧对该地图点的观测发生变化,就应该调用函数 MapPoint::UpdateNormalAndDepth() 更新其观测尺度和方向信息。

3.3 特征描述子

成员变量/函数 访问控制 意义
cv::Mat mDescriptor protected 当前关键点的特征描述子
cv::Mat GetDescriptor() public mDescriptor 的get方法
void ComputeDistinctiveDescriptors() public 计算 mDescriptor

一个地图点在不同关键帧中对应不同特征点和描述子,其特征描述子 mDescriptor 是其在所有观测关键帧中描述子的中位数(准确的说,该描述子与其它所有描述子的中值距离最小)

(1)特征点描述子的更新时机:

一旦某地图点对关键帧的观测 mObservations 发生改变,就调用函数 void ComputeDistinctiveDescriptors() 更新该地图点的特征描述子。

(2)特征描述子的用途:

在函数 ORBmatcher::SearchByProjection()ORBmatcher::Fuse() 中,通过比较地图点的特征描述子与图片特征点描述子,实现将 地图点与图像特征点的匹配 (3D-2D 匹配)。

3.4 地图点的删除与替换

成员变量/函数 访问控制 意义
bool mbBad protected 坏点标记
bool isBad() public 查询当前地图点是否被删除(本质上是查询 mbBad
void SetBadFlag() public 删除当前地图点
MapPoint* mpReplaced protected 用来替换当前地图点的新地图点
void Replace(MapPoint* pMP) public 使用地图点 pMp 替换当前地图点
3.4.1 地图点的删除 SetBadFlag()

变量 mbBad 用来表征当前地图点是否被删除。

删除地图点的各成员变量是一个比较耗时的过程,因此函数 SetBadFlag() 删除关键点时采取 先标记再清除 的方式,具体过程为:

  • 先将坏点标记 mbBad 置为 true,逻辑上删除该地图点。

  • 再依次清空当前地图点的各成员变量,物理上删除该地图点。

这样只有在设置坏点标记 mbBad 时需要加锁,之后的操作就不需要加锁了。

3.4.2 地图点的替换 Replace()

函数 Replace(MapPoint* pMP) 将当前地图点的成员变量叠加到新地图点 pMP 上。

3.5 MapPoint 类的用途

3.5.1 MapPoint 的生命周期

(1)创建MapPoint的时机:

  • Tracking 线程中初始化过程(Tracking::MonocularInitialization()Tracking::StereoInitialization()

    Tracking线程中创建新的关键帧(Tracking::CreateNewKeyFrame())

  • Tracking 线程中恒速运动模型跟踪(Tracking::TrackWithMotionModel())也会产生临时地图点,但这些临时地图点在跟踪成功后会被马上删除(那跟踪失败怎么办?跟踪失败的话不会产生关键帧,这些地图点也不会被注册进地图)。

  • LocalMapping 线程中创建新地图点的步骤(LocalMapping::CreateNewMapPoints())会将当前关键帧与前一关键帧进行匹配,生成新地图点。

(2)删除 MapPoint 的时机:

  • LocalMapping线程中删除恶劣地图点的步骤(LocalMapping::MapPointCulling())。

  • 删除关键帧的函数 KeyFrame::SetBadFlag() 会调用函数 MapPoint::EraseObservation() 删除地图点对关键帧的观测,若地图点对关键帧的观测少于 2,则地图点无法被三角化,就删除该地图点。

(3)替换 MapPoint 的时机:

  • LoopClosing 线程中闭环矫正(LoopClosing::CorrectLoop())时当前关键帧和闭环关键帧上的地图点发生冲突时,会使用闭环关键帧的地图点替换当前关键帧的地图点。

  • LoopClosing 线程中闭环矫正函数 LoopClosing::CorrectLoop() 会调用 LoopClosing::SearchAndFuse() 将闭环关键帧的共视关键帧组中所有地图点投影到当前关键帧的共视关键帧组中,发生冲突时就会替换。

相关推荐
Lusix194911 天前
Realsense相机驱动在使用imu数据时出现Qos问题
数码相机·slam·realsense
1037号森林里一段干木头14 天前
相机外参与相机位姿深度理解
数学·计算机视觉·slam·相机标定
xiaoyaolangwj14 天前
高翔【自动驾驶与机器人中的SLAM技术】学习笔记(十二)拓展图优化库g2o(一)框架
自动驾驶·slam·g2o
LPhilo21 天前
ROS理论与实践学习笔记——6 ROS机器人导航(仿真)之导航实现
linux·ubuntu·机器人·ros·slam·navigation·gmapping
请不要叫我菜鸡1 个月前
无环SLAM系统集成后端回环检测模块(loop):SC-A-LOAM以及FAST_LIO_SLAM
目标检测·slam·fast-lio·a_loam·sc-pgo·回环检测
小虎哥哥爱学习1 个月前
《机器人SLAM导航核心技术与实战》第1季:第9章_视觉SLAM系统
图像处理·人工智能·算法·计算机视觉·机器人·自动驾驶·slam
我搞slam2 个月前
Cartographer源码理解
算法·slam·cartographer
杀生丸学AI2 个月前
【三维重建】近期进展(完善中)
3d·aigc·slam·三维重建·nerf·视觉大模型
WHAT8162 个月前
【Orb-Slam3学习】 特征匹配函数的目的与分类
c++·人工智能·算法·slam
Chris·Bosh2 个月前
intel RealSense D435i自制数据集跑SLAM
linux·slam