03 地图点 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() 同时维护 mObservations 和 nObs。

/**
 * @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 平均观测距离 mfMinDistance 和 mfMaxDistance

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

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

特征点的平均观测距离的上下限由成员变量 mfMinDistance 和 mfMaxDistance 表示：

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

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

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

根据层级算深度

// 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) 根据某地图点到某帧的观测深度估计其在该帧图片上的层级,是上述过程的逆运算。

$$\begin{array}{c}\frac{\text{ mfMaxDistance }}{\text{ currentDist }}=1.2^{\text{level }}\\ \text{ level }=\lceil {\log }_{1.2}\left(\frac{\text{ mfMaxDistance }}{\text{ currentDist }}\right)\rceil \end{array}$$

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 中所有观测到本地图的关键帧取平均得到，平均观测距离是根据参考关键帧得到的。

/**
 * @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() 将闭环关键帧的共视关键帧组中所有地图点投影到当前关键帧的共视关键帧组中，发生冲突时就会替换。