单目SLAM尺度问题

双目通过基线（单位mm）和特征点在两个像素平面上坐标的视差来确定空间点坐标（x,y,z），单目就不行了。

以ORB_SLAM2的单目初始化为例。

流程：(只介绍初始化时的跟踪)

在system.ccTracking::Tracking()创建一个tracking线程，传入词典向量、传感器类型、相机设置文件，以及绑定地图、关键帧数据库、地图绘制、帧绘制。

然后Tracking::GrabImageMonocular()将图片转为灰度图，根据系统状态mState是未被初始化还是别的，创建当前帧（初始帧的特征点提取数量比一般帧的数量多一倍）。

然后调用Tracking::Track()：未初始化，则调用Tracking::MonocularInitialization()，初始化成功后mState=OK.

Tracking::MonocularInitialization():单目初始化需要连续的两帧，这两帧提取的特征点都要>100，且匹配的点数要>100，否则就接着读取下一帧。然后调用Initialize::Initialize()，判断能否初始化以及计算R,t（创建两个线程并行计算H和F矩阵，根据得分判断用哪个模型，然后用该模型进行三角化，验证模型是否合适）。经过这些操作，终于到靠题的一个函数了：Tracking::CreateInitialMapMonocular()。

Tracking::CreateInitialMapMonocular()创建初始关键帧pKFini和当前关键帧pKFcur，创建地图点并进行局部BA。然后就可以认为初始化结果是正确的，这个时候，尺度怎样确定呢？根据高博的SLAM14讲第152页的内容，单目尺度不确定性可以通过对初始两帧的平移向量t归一化作为后续的单位；也可以通过令初始化时所有特征点的平均深度为1固定一个尺度，这种方式可以控制场景规模大小，使计算在数值上更稳定。那么ORB-SLAM2单目初始化使用的是第二种固定尺度的方法（与书上略有不同）：

    // step1：获取初始关键帧的地图点的深度中值Z
    float medianDepth = pKFini->ComputeSceneMedianDepth(2);
    float invMedianDepth = 1.0f/medianDepth;

    if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
    {
        cout << "Wrong initialization, reseting..." << endl;
        Reset();
        return;
    }

    //step2:相机位姿归一化x'/z,y'/z,z'/z
    cv::Mat Tc2w = pKFcur->GetPose();
    Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
    pKFcur->SetPose(Tc2w);

    //step3:地图点归一化
    vector<MapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches();
    for(size_t iMP=0; iMP<vpAllMapPoints.size(); iMP++)
    {
        if(vpAllMapPoints[iMP])
        {
            MapPoint* pMP = vpAllMapPoints[iMP];
            pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth);
        }
    }

综上，尺度固定是对于平移向量来说的，即一个形象表达式为c*t（c是尺度变换因子，即代码段中的invMedianDepth），和旋转矩阵R没有关系（常识也是这样，沿着方向，走的距离不同（1m还是1cm），造成的轨迹尺度也不一样，但是形状是一样的）。用ORB_SLAM2单目跑标准数据集，与gt比较，需要先进行尺度对齐，在SLAM评估软件evo中，通过指令--align --correct-scale实现，对应的代码段的原理论文《Least-SSquares Estimation of Transmation Parameters Between Two Point Patterns》中：

X,Y是两组对应的点（两组尺度、方向、不一致的轨迹），c、R、t分别对应尺度、旋转、平移变换参数。

以一个简单的2维，3点对齐问题为例，用该论文提出的最小化上述方差方法得到的变换参数，进行变换，效果如下：-[

——>