Staskaer

Hi, this is Staskaer

0%

ORBSLAM2源码小记(1)--地图点MapPoint和地图Map

Posted on Edited on In Valine:

ORBSLAM2的地图点MapPoint和地图Map

主要是记录下相对容易忘的内容，不会写的很细

MapPoint

MapPoint是一个三维点，即为空间中的特征点，在单目SLAM中通过对多帧图像的特征点进行三角化得到。

在ORBSLAM2中，MapPoint类还增加一些额外信息负责维护地图点和加速匹配特征点。

地图点描述子

每帧图像会计算出特征点，当不同帧特征点匹配后可以三角化出地图点，由于后续图像帧也可能观测到同样的地图点，所以需要为每个地图点计算出地图点描述子，以供后续帧匹配。

在ORBSLAM2中，通过计算匹配到该地图点的特征点描述子的的中值作为地图点的描述子。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58

/**
* @brief 计算地图点最具代表性的描述子
*
* 由于一个地图点会被许多相机观测到，因此在插入关键帧后，需要判断是否更新代表当前点的描述子
* 先获得当前点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值
*/
void MapPoint::ComputeDistinctiveDescriptors()
{

    ...

    const size_t N = vDescriptors.size();

    // 将Distances表述成一个对称的矩阵
    // float Distances[N][N];
    std::vector<std::vector<float>> Distances;
    Distances.resize(N, vector<float>(N, 0));
    for (size_t i = 0; i < N; i++)
    {
        // 和自己的距离当然是0
        Distances[i][i] = 0;
        // 计算并记录不同描述子距离
        for (size_t j = i + 1; j < N; j++)
        {
            int distij = ORBmatcher::DescriptorDistance(vDescriptors[i], vDescriptors[j]);
            Distances[i][j] = distij;
            Distances[j][i] = distij;
        }
    }

    // 选择最有代表性的描述子，它与其他描述子应该具有最小的距离中值
    int BestMedian = INT_MAX; // 记录最小的中值
    int BestIdx = 0;          // 最小中值对应的索引
    for (size_t i = 0; i < N; i++)
    {
        // 第i个描述子到其它所有描述子之间的距离
        // vector<int> vDists(Distances[i],Distances[i]+N);
        vector<int> vDists(Distances[i].begin(), Distances[i].end());
        sort(vDists.begin(), vDists.end());

        // 获得中值
        int median = vDists[0.5 * (N - 1)];

        // 寻找最小的中值
        if (median < BestMedian)
        {
            BestMedian = median;
            BestIdx = i;
        }
    }

    {
        unique_lock<mutex> lock(mMutexFeatures);
        mDescriptor = vDescriptors[BestIdx].clone();
    }
}

观测距离范围与反投影金字塔层级

观测距离范围

由于ORBSLAM2使用了尺度不变的描述子，而尺度不变性通常由图像金字塔来处理，当一个特征只有在放大时才能被检测到(即在图像金字塔下层)，说明该特征点对应的地图点较远。

由于每个地图点存在一个参考关键帧，ORBSLAM2通过这个参考关键帧来计算这个地图点大致会在什么深度范围内被观测到。地图点到参考帧相机中心距离，乘上参考帧中描述子获取金字塔放大尺度得到最大距离mfMaxDistance，而最大距离除以整个金字塔最高层的放大尺度得到最小距离mfMinDistance。

1
2
3
4
5
6
7
8
9
10
11
void MapPoint::UpdateNormalAndDepth()
{
    ...

    {
        // 观测到该点的距离上限
        mfMaxDistance = dist * levelScaleFactor;
        // 观测到该点的距离下限
        mfMinDistance = mfMaxDistance / pRefKF->mvScaleFactors[nLevels - 1]; 
    }
}

最终的范围是。通过这两个距离可以粗略的判断一个地图点可不可以被某一帧检测到，对应于bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)函数。

这里计算出来的深度本质是一个等效，因为图像金字塔层级是有限的(规定第0层最小，第7层最大)，所以放大倍率也是有限的。

比如，某个地图点距离参考帧距离为1m，在第6层金字塔被检测到，那么最大距离为，就是说，相机在距离该地图点大约2.98m时拍摄的图像帧，在原始图像（第0层金字塔）提取出的该地图点对应的特征点，与相机距离该地图点1米时在最深层金字塔图像帧（第7层金字塔）提取的该地图点对应的特征点相似。而一旦超过这个距离，该地图点就会被投影到第7层以上的金字塔，而ORBSLAM2最大只有第7层金字塔，所以这个地图点就极大概率不会被提取到。

最小距离也是类似的，不存在第-1层金字塔，当距离过近时也同样无法在金字塔里被提取出来。

反投影金字塔层级

由于ORBSLAM2会通过对地图点反投影来扩大匹配，可以通过计算地图点--帧光心的距离来估计这个地图点大致会在那一层被检测到来缩小搜索范围。计算公式如下：


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
/**
 * @brief 预测地图点对应特征点所在的图像金字塔尺度层数
 *
 * @param[in] currentDist   相机光心距离地图点距离
 * @param[in] pKF           关键帧
 * @return int              预测的金字塔尺度
 */
int MapPoint::PredictScale(const float &currentDist, KeyFrame *pKF)
{
    float ratio;
    {
        unique_lock<mutex> lock(mMutexPos);
        // mfMaxDistance = ref_dist*levelScaleFactor 为参考帧考虑上尺度后的距离
        // ratio = mfMaxDistance/currentDist = ref_dist/cur_dist
        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;
}

平均观测方向

观测到所有该地图点的关键帧的相机光心到地图点的方向向量的平均值，主要用于判断地图点是否在视野内，当地图点方向与相机光心方向夹角小于一定阈值时，认为地图点在视野内，对应于bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)函数。

其物理意义描述了特征点的朝向，比如桌角的特征点，只有面对桌角时才能看到，所以这个方向向量就是桌角的方向，而背对桌角时就看不到这个特征点。

被观测到次数与应该被观测到次数

这两个数据主要用于判断一个地图点的质量，用于地图点的剔除。

  • 被观测到次数mnFound即所有帧（不止关键帧）存在特征点匹配到该地图点的次数
  • 应该被观测到次数mnVisible即所有帧（不止关键帧）应该能观测到该地图点的次数

mnFound很好理解，每匹配成功就增加一次计数即可；而mnVisible则是在tracking线程中调用bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)来判断，这个函数会校验地图点投影深度、投影区域、地图点距离和夹角。具体调用时机与校验的地图点范围等留在tracking线程中细说。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
/**
 * @brief 判断地图点是否在视野中
 * @param[in] pMP                       当前地图点
 * @param[in] viewingCosLimit           当前相机指向地图点向量和地图点的平均观测方向夹角余弦阈值
 * @return true                         地图点合格，且在视野内
 * @return false                        地图点不合格，抛弃
 */
bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)
{
    ...

    // 根据当前帧(粗糙)位姿转化到当前相机坐标系下的三维点Pc
    const cv::Mat Pc = mRcw * P + mtcw;
    const float &PcX = Pc.at<float>(0);
    const float &PcY = Pc.at<float>(1);
    const float &PcZ = Pc.at<float>(2);

    // 将这个地图点变换到当前帧的相机坐标系下，如果深度值为正才能继续下一步。
    if (PcZ < 0.0f)
        return false;

    // 将地图点投影到当前帧的像素坐标，如果在图像有效范围内才能继续下一步。
    const float invz = 1.0f / PcZ;
    const float u = fx * PcX * invz + cx;
    const float v = fy * PcY * invz + cy;
    if (u < mnMinX || u > mnMaxX)
        return false;
    if (v < mnMinY || v > mnMaxY)
        return false;

    // 计算地图点到相机中心的距离，如果在有效距离范围内才能继续下一步。
    // 可靠距离范围:[0.8f*mfMinDistance, 1.2f*mfMaxDistance]
    const float maxDistance = pMP->GetMaxDistanceInvariance();
    const float minDistance = pMP->GetMinDistanceInvariance();
    // 得到当前地图点距离当前帧相机光心的距离
    const cv::Mat PO = P - mOw;
    const float dist = cv::norm(PO);
    if (dist < minDistance || dist > maxDistance)
        return false;

    // 计算当前相机指向地图点向量和地图点的平均观测方向夹角，小于阈值才能进入下一步。
    cv::Mat Pn = pMP->GetNormal();
    // 计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值，注意平均观测方向为单位向量
    const float viewCos = PO.dot(Pn) / dist;
    if (viewCos < viewingCosLimit)
        return false;

    ...
    return true;
}

Map

Map即地图，维护了所有的地图点和关键帧，由于ORBSLAM2仅支持一张地图，所以每次只有会一个Map对象。

其本身非常简单，主要是一些增删查的接口，不再赘述。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
class Map
{
public:
    // 保存了最初始的关键帧
    vector<KeyFrame *> mvpKeyFrameOrigins;

    // 当更新地图时的互斥量 回环检测中和局部BA后更新全局地图的时候会用到这个
    std::mutex mMutexMapUpdate;

    // 为了避免地图点id冲突设计的互斥量
    std::mutex mMutexPointCreation;

protected:
    // 存储所有的地图点
    std::set<MapPoint *> mspMapPoints;

    // 存储所有的关键帧
    std::set<KeyFrame *> mspKeyFrames;

    // 用于绘制当前观测到的地图点
    std::vector<MapPoint *> mvpReferenceMapPoints;

    // 当前地图中具有最大ID的关键帧
    long unsigned int mnMaxKFid;

    /// 类的成员函数在对类成员变量进行操作的时候,防止冲突的互斥量
    std::mutex mMutexMap;
}