SLAM基础——滑动窗口中的三角测量

前言：简单的三角化

  理想情况下，两张图像中关联的同一个三维特征点$P$ 满足下述方程：

$$\begin{gathered} \{\begin{array}{r}{s}_2{x}_2=P\\ s_1{R}_{21}{x}_1+t_{21}=P\end{array} \\ {s}_2{x}_2=s_1{R}_1{x}_1+t_1 \end{gathered}$$
其中 $s_1、s_2$ 为深度信息，$x_1、x_2$对应归一化坐标，同时$R_{21}、t_{21}$ 十分精准,可以通过等式两边同时叉乘$x_2$直接解出 $s_1、s_2$ :
$$s_2{x}_2\times x_2=0=s_1{x}_2\times R_{21}{x}_1+x_2\times t_{21}$$
《SLAM十四讲》(第二版)p177中已经讲得较为详细。

最小二乘法求解

推广形式

  实际情况中， 上述公式中的$R_{21}、t_{21}$ 并不精确，且存在多帧图像“tracking”到同一个特征点，基于上述“存在误差、多组数据”可以很自然地想到最小二乘法。
首先将公式拓展成一般形式：
$$los{s}_i=s_i{x}_i\times x_i={\delta }_i=s_1{x}_i\times R_{i1}{x}_1+x_i\times t_{i1}$$
为方便表示，令$X_1=s_1{x}_1$，$X_1$表示在图像1坐标系下$P$点坐标：
$$\begin{gathered} los{s}_i=x_i\times R_{i1}{X}_1+x_2\times t_{i1}={\delta }_i \\ Loss=\frac{1}{2}\sum _{i=2}^{n}loss{i}^2 \end{gathered}$$
因为子函数$los{s}_i$ 全部为线性，可以转换成求$A{X}_1=0$ 最小二乘问题，采用SVD求解$A$ 的零空间向量（近似的零空间向量，认为特征值最小的特征向量近似于零空间向量），求出的零空间向量则为$X_1$的具体数值。

进一步简化

  $X_1$ 扩展成齐次坐标后，（注意之后$X_1$ 可能为齐次坐标也可能非齐次）公式如下：
$$los{s}_i=x_i\times T_{i1}{X}_1$$
$$los{s}_i=\left[\begin{array}{ccc}0& -1& v_i\\ 1& 0& -u_i\\ -v_i& u_i& 0\end{array}\right]T_{i1}{X}_1$$
$$los{s}_i=\left[\begin{array}{c}-T_{i1row2}+v_i{T}_{i1row3}\\ T_{i1row1}-u_i{T}_{i1row3}\\ -v_i{T}_{i1row1}+u_i{T}_{i1row2}\end{array}\right]X_1$$
第三行可以由上两行线性变换所得，可以简化成：
$$los{s}_i=\left[\begin{array}{c}-T_{i1row2}+v_i{T}_{i1row3}\\ T_{i1row1}-u_i{T}_{i1row3}\end{array}\right]X_1$$
整个$Loss$为：
$$Los{s}_i=\left[\begin{array}{c}-T_{21row2}+v_2{T}_{21row3}\\ T_{21row1}-u_2{T}_{21row3}\\ ....\\ -T_{n1row2}+v_2{T}_{n1row3}\\ T_{n1row1}-u_2{T}_{n2row3}\end{array}\right]X_1$$
利用线性代数性质，整齐一点的写法:(主要是针对写代码时候看得舒服)
$$Los{s}_i=\left[\begin{array}{c}{u}_2{T}_{21row3}-T_{21row1}\\ v_2{T}_{21row3}-T_{21row2}\\ ....\\ u_n{T}_{n1row3}-T_{n1row1}\\ v_n{T}_{n1row3}-T_{n1row2}\end{array}\right]X_1$$

滑动窗口中的三角化——VINS为例

VINS中滑动窗口优化的逆深度为当前滑动窗口中第一次看到该特征点的图像下的逆深度。同时又加入相机外参，显得稍微复杂些。
$$\{\begin{array}{r}{T}_{w{b}_0}{T}_{bc}{X}_1=P\\ T_{w{b}_i}{T}_{bc}{X}_i=P\end{array}$$
公式展开，并进行简单的等式变换后也可得如下的形式：
$$s_i{x}_i=s_1{R}_i^{{}^{\prime }}{x}_1+t_i^{{}^{\prime }}$$
只不过$R_i^{{}^{\prime }}、t_i^{{}^{\prime }}$是通过数学变换，努力向之前已有的公式上“蹭”。
Latex敲得头大，直接手写。

对应VINS中的代码：

void FeatureManager::triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[])
{
    for (auto &it_per_id : feature)
    {
        it_per_id.used_num = it_per_id.feature_per_frame.size();
        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
            continue;

        if (it_per_id.estimated_depth > 0)
            continue;
        int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;

        ROS_ASSERT(NUM_OF_CAM == 1);
        Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4);
        int svd_idx = 0;

        Eigen::Matrix<double, 3, 4> P0;
        Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
        Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];
        P0.leftCols<3>() = Eigen::Matrix3d::Identity();
        P0.rightCols<1>() = Eigen::Vector3d::Zero();

        for (auto &it_per_frame : it_per_id.feature_per_frame)
        {
            imu_j++;

            Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0];
            Eigen::Matrix3d R1 = Rs[imu_j] * ric[0];
            Eigen::Vector3d t = R0.transpose() * (t1 - t0);
            Eigen::Matrix3d R = R0.transpose() * R1;
            Eigen::Matrix<double, 3, 4> P;

			//对应公式中最后要求的R' t'
			
            P.leftCols<3>() = R.transpose();
            P.rightCols<1>() = -R.transpose() * t;
            Eigen::Vector3d f = it_per_frame.point.normalized();
            svd_A.row(svd_idx++) = f[0] * P.row(2) - f[2] * P.row(0);
            svd_A.row(svd_idx++) = f[1] * P.row(2) - f[2] * P.row(1);

            if (imu_i == imu_j)
                continue;
        }
        ROS_ASSERT(svd_idx == svd_A.rows());
        Eigen::Vector4d svd_V = Eigen::JacobiSVD<Eigen::MatrixXd>(svd_A, Eigen::ComputeThinV).matrixV().rightCols<1>();
        double svd_method = svd_V[2] / svd_V[3];
        //it_per_id->estimated_depth = -b / A;
        //it_per_id->estimated_depth = svd_V[2] / svd_V[3];

        it_per_id.estimated_depth = svd_method;
        //it_per_id->estimated_depth = INIT_DEPTH;

        if (it_per_id.estimated_depth < 0.1)
        {
            it_per_id.estimated_depth = INIT_DEPTH;
        }

    }
}