单目初始化及尺度不确定

单目里程计：
问题：通过单目相机图像序列，计算得到图像序列对应的相机位姿$(R,t)$

两个特点：

1、需要足够平移才能完成初始化
2、尺度不确定及尺度漂移

1、单目初始化需要相机足够的平移，才能够完成初始化。
使用2D-2D的方式来估计两帧相机之间的位姿时，可以通过求解**H(单应矩阵homography)或F(基础矩阵fundamental)以及BA(bundle adjustment)**等方式来解析出相机的位姿。

在近纯旋转(低偏移)的情况下，F阵解算失效，通过H和BA的方式可以解析R，此时解析出的t是不可靠的，并且由于$t$小，因此也无法使用三角测距的方法来恢复地图点的深度。

由于单目恢复地图点三维坐标的方式，通过对极几何(三角化)的方式，因此必须需要两帧图像之间有足够的偏移(t)运动。此在三角化特征点(triangulation)方法有所阐述。

因此，只有t足够大时，才能被解析，才能通过对极几何的方式，恢复地图点深度。

此外，地图点的深度解析与偏移t的解析值存在相关性。因此单目中存在尺度不确定。

2、尺度不确定及尺度漂移
两帧图像中有一对匹配图像特征点：$p_i^1和p_i^2$，设两帧图像之间对应相机的运动为$(R,t)$，则：
$$d_1{p}_i^1=d_{2}R{p}_i^2+t$$$$d_1\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=d_{2}R\left[\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right]+t$$
两侧同乘以常数$c$，得：
$$c{d}_1{p}_i^1=c{d}_{2}R{p}_i^2+ct$$
如下示意图，地图点深度($d$)与偏移($t$)相关，地图点不同深度，则对应一个$t$值。同样的观测可以计算出无穷组$d$和$t$。

可知：使用单目来实现SLAM时，存在尺度不确定。

因此，单目VIO需要初始化过程，初始化过程中，检测足够大的平移运动，解析出t及地图点深度，并确定尺度。

尺度的确定有两种方法：
1）对初始两帧的平移向量t归一化作为后续的单位；也可以通过
2）令初始化时所有特征点进行归一化，这里的归一化令特征点平均深度为1，这种方式可以控制场景规模大小，使计算在数值上更稳定。

以上来自[视觉十四讲-第二版]（176页）及https://blog.csdn.net/weixin_42269064/article/details/115691673

在VINS-Mono中，使用本质矩阵(E)解算出相机运动(R,t)，这里的t并未进行尺度评估，在视觉与IMU对齐时，利用IMU的积分数据来估计尺度值。
在ORB-Mono中，使用基础矩阵(F)和单应矩阵(H)来解析(R,t)，将地图点归一化，即尺度使用的是(invMedianDepth),
<ORB_SLAM/src/ Tracking.cc > 中的Tracking::CreateInitialMapMonocular()中：

    // 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);
        }

因此ORB-Mono估计的轨迹需要对齐尺度，若用evo时，需加上-s参数。

SVO单目使用直接法来初始化相机位移，前几帧使用真实值来得到尺度参数。

单目在确定尺度后，随着计算，尺度会出现漂移。当初始帧固定尺度后，由于后期计算的误差，如t的解析误差，会同步带来地图点深度的误差，地图点深度误差，同步会得到t的解析误差，从而累积，形成尺度漂移。