视觉SLAM VIO中的三角化

视觉 SLAM 中的三角化

​ 在单目 SLAM 中,通常在插入关键帧时计算新路标点的三角化。

• 有的 SLAM 系统在关键帧时提取新 Feature(DSO, SVO)，也有的方案对每个帧都提新 Feature(VINS, ORB)。
• 前者节省计算量(非关键帧无需提点,节省 5-10ms 左右);后者效果好(在单目里需要防止三角化 Landmark 数量不够)

三角化的数学描述

​ 考虑某路标点 $\mathbf{y}$ 在若干个关键帧 $k=1$, $\cdots$, $n$中看到。$\mathbf{y}\in {\mathbb{R}}^4$ ，取齐次坐标。每次观测为${\mathbf{x}}_k={\left[u_k,v_k,1\right]}^{\top }$，取归一化平 面坐标（这样可以忽略掉内参)。
​ 记投影矩阵 ${\mathbf{P}}_k=\left[{\mathbf{R}}_k,{\mathbf{t}}_k\right]\in {\mathbb{R}}^{3\times 4}$，为 World 系到 Camera 系 投影关系：
$$\forall k,{\lambda }_k{\mathbf{x}}_k={\mathbf{P}}_k\mathbf{y}$$
​ 其中 ${\lambda }_k$ 为观测点的深度值（末知） 根据上式第三行:

$${\lambda }_k={\mathbf{P}}_{k,3}^{\top }\mathbf{Y}$$
​ 其中 ${\mathbf{P}}_{k,3}^{\top }$ 为 ${\mathbf{P}}_k$的第 3 行。

​ 将 ${\lambda }_k={\mathbf{P}}_{k,3}^{\top }\mathbf{Y}$ 带入 world 到 camera 的投影关系可以得到：
$$\begin{array}{l}{u}_k{\mathbf{P}}_{k,3}^{\top }\mathbf{y}={\mathbf{P}}_{k,1}^{\top }\mathbf{y}\\ v_k{\mathbf{P}}_{k,3}^{\top }\mathbf{y}={\mathbf{P}}_{k,2}^{\top }\mathbf{y}\end{array}$$
​ 每次观测将提供两个这样的方程,视 $\mathbf{y}$ 为未知量,并将 $\mathbf{y}$ 移到等式一侧:
$$\left[\begin{array}{c}{u}_1{\mathbf{P}}_{1,3}^{\top }-{\mathbf{P}}_{1,1}^{\top }\\ v_1{\mathbf{P}}_{1,3}^{\top }-{\mathbf{P}}_{1,2}^{\top }\\ ⋮\\ u_n{\mathbf{P}}_{n,3}^{\top }-{\mathbf{P}}_{n,1}^{\top }\\ v_n{\mathbf{P}}_{n,3}^{\top }-{\mathbf{P}}_{n,2}^{\top }\end{array}\right]\mathbf{y}=0\to \mathbf{Dy}=0$$
​ 于是, $\mathbf{y}$ 为 $\mathbf{D}$ 零空间中的一个非零元素。

​ 由于 $\mathbf{D}\in {\mathbb{R}}^{2n\times 4}$，在观测次于大于等于两次时，很可能 $\mathbf{D}$ 满秩， 无零空间。
寻找最小二乘解:
$$\underset{\mathbf{y}}{\min }\Vert \mathbf{Dy}{\Vert }_2^2,\text{ s.t. }\Vert \mathbf{y}\Vert =1$$
解法：对 ${\mathbf{D}}^{\top }\mathbf{D}$ 进行 $SVD$：

$${\mathbf{D}}^{\top }\mathbf{D}=\sum _{i=1}^4{\sigma }_i^2{\mathbf{u}}_i{\mathbf{u}}_j^{\top }$$

其中 ${\sigma }_i$ 为奇异值，且由大到小排列， ${\mathbf{u}}_i,{\mathbf{u}}_j$ 正交。

​ 此时，取 $\mathbf{y}={\mathbf{u}}_4$ (为什么?)，那么该问题的目标函数值为 ${\sigma }_4$。 判断该解有效性的条件: ${\sigma }_4\ll {\sigma }_3$。若该条件成立，认为三角化有 效，否则认为三角化无效。

​ Rescale: 在某此场合 (比如我们使用 UTM 坐标的平移)，$D$ 的数 值大小差异明显，导致解在数值上不稳定。此时对 $D$ 做一个缩放：
$$\mathrm{Dy}=\underset{\tilde{\mathrm{D}}}{\underbrace{\mathbf{DS}}}\underset{\tilde{\mathrm{y}}}{\underbrace{{\mathbf{S}}^{-1}\mathbf{y}}}=0$$
​ 其中 $S$ 为一个对角阵，取 $D$ 最大元素之逆即可。实用当中，还需要加上 $\mathbf{y}$ 投影正负号的判定作为依据

通过 SVD 进行三角化代码

#include <iostream>
#include <vector>
#include <random>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
struct Pose
{
    Pose(Eigen::Matrix3d R, Eigen::Vector3d t) : Rwc(R), qwc(R), twc(t){};
    Eigen::Matrix3d Rwc;
    Eigen::Quaterniond qwc;
    Eigen::Vector3d twc;
    Eigen::Vector2d uv; // 这帧图像观测到的特征坐标
};
int main()
{
    int poseNums = 10;
    double radius = 8;
    double fx = 1.;
    double fy = 1.;
    std::vector<Pose> camera_pose;
    // 随机生成 pose 的位置
    for (int n = 0; n < poseNums; ++n)
    {
        double theta = n * 2 * M_PI / (poseNums * 4); // 1/4 圆弧
        // 绕 z轴 旋转
        Eigen::Matrix3d R;
        R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
        Eigen::Vector3d t = Eigen::Vector3d(radius * cos(theta) - radius,
                                            radius * sin(theta), 1 * sin(2 * theta));
        camera_pose.push_back(Pose(R, t));
    }
    // 随机数生成 1 个 三维特征点32.
    // 只是为了生成landmark,实际是不知道landmark的pose的,只有landmark在uv下
    // 的坐标和一个深度值未知的量
    std::default_random_engine generator;
    std::uniform_real_distribution<double> xy_rand(-4, 4.0);
    std::uniform_real_distribution<double> z_rand(8., 10.);
    double tx = xy_rand(generator);
    double ty = xy_rand(generator);
    double tz = z_rand(generator);
    Eigen::Vector3d Pw(tx, ty, tz);
    std::cout << "Ground truth:" << Pw.transpose() << std::endl;
    // 这个特征从第三帧相机开始被观测,i=3
    int start_frame_id = 3;
    int end_frame_id = poseNums;
    //从第三帧开始,计算这一个特征点在每一帧图像里的归一化坐标
    for (int i = start_frame_id; i < end_frame_id; ++i)
    {
        Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose(); //原本是wc,transpose变成cw
        Eigen::Vector3d Pc = Rcw * (Pw - camera_pose[i].twc);
        double x = Pc.x();
        double y = Pc.y();
        double z = Pc.z();
        camera_pose[i].uv = Eigen::Vector2d(x / z, y / z);
    }
    /// TODO:三角化估计深度的代码
    // 遍历所有的观测数据,并三角化
    Eigen::Vector3d P_est;
    // 结果保存到这个变量
    P_est.setZero();
    /* code begin */
    auto loop_times = camera_pose.size() - start_frame_id;
    Eigen::MatrixXd D((loop_times)*2, 4);
    for (int j = 0; j < loop_times; ++j)
    {
        Eigen::MatrixXd T_tmp(3, 4);
        T_tmp.block<3, 3>(0, 0) = camera_pose[j + 3].Rwc.transpose();
        T_tmp.block<3, 1>(0, 3) = -camera_pose[j + 3].Rwc.transpose() *
                                  camera_pose[j + 3].twc;
        auto P_k1 = T_tmp.block<1, 4>(0, 0);
        auto P_k2 = T_tmp.block<1, 4>(1, 0);
        auto P_k3 = T_tmp.block<1, 4>(2, 0);
        D.block<1, 4>(2 * j, 0) = camera_pose[j + 3].uv[0] * P_k3 - P_k1;
        D.block<1, 4>(2 * j + 1, 0) = camera_pose[j + 3].uv[1] * P_k3 -
                                      P_k2;
    }
    Eigen::Matrix4d D_res = D.transpose() * D;
    Eigen::JacobiSVD<Eigen::Matrix4d> svd(D_res, Eigen::ComputeFullU |
                                                     Eigen::ComputeFullV);
    auto res_U = svd.matrixU();
    auto res_V = svd.matrixV();

    P_est << res_U(0, 3) / res_U(3, 3), res_U(1, 3) / res_U(3, 3), res_U(2, 3) / res_U(3, 3);

    // std::cout << "Trans=" << Trans.rows() << " " << Trans.cols() << std::endl;
    std::cout << "U=" << res_U << std::endl;
    auto tmp = res_U.rightCols(1);
    std::cout << "res=" << (tmp / tmp(3)).transpose() << std::endl;
    /* code end */
    std::cout << "D: \n"
              << D.transpose() * D << std::endl;
    std::cout << "ground truth: \n"
              << Pw.transpose() << std::endl;
    std::cout << "your result: \n"
              << P_est.transpose() << std::endl;
    return 0;
}

• 输出

Ground truth:  -2.9477 -0.330799   8.43792
U= 0.0530721   0.846878    0.41558  -0.327528
 -0.103079   0.431629  -0.895388 -0.0367562
 -0.102585   0.309021   0.122288   0.937565
  0.987945  0.0316285  -0.103049   0.111113
res=  -2.9477 -0.330799   8.43792         1
D: 
        7         0 -0.486169   24.7361
        0         7   5.90714  -47.5284
-0.486169   5.90714    5.6799  -47.4055
  24.7361  -47.5284  -47.4055   457.196
ground truth: 
  -2.9477 -0.330799   8.43792
your result: 
  -2.9477 -0.330799   8.43792