vSLAM学习笔记——特征点三角化

记录最近学习vSLAM的一些理论和代码实现。

---

三角化的数学描述

1. 考虑某路标点 y 在若干个关键帧 k = 1,…,n 中看到。
   y∈$R^4$，取齐次坐标。每次观测为$x_k$=[$u_k$, $v_k$,1]${}^T$，取归一化平面坐标（这样可以忽略掉内参）。

2. 从World系到Camera系的投影矩阵$P_k=[R_K,t_k]\in R^{3\times 4}$。

3. 投影关系：$$\forall k,{\lambda }_k{x}_k=P_{k}y....(1)$$其中${\lambda }_k$为观测点的深度值（未知）

4. 根据上式第三行：
   $${\lambda }_k=P_{k,3}^{T}y....(2)$$ 其中$P_{k,3}^T$为$P_k$的第3行

5. 将(2)代入(1)的前两行：
   $$u_k{P}_{k,3}^{T}y=P_{k,1}^{T}y$$ $$v_k{P}_{k,3}^{T}y=P_{k,2}^{T}y$$

6. 每次观测将提供两个这样的方程，视$y$为未知量，并将$y$移到等式一侧：
   $$\left[\begin{array}{c}{u}_1{P}_{1,3}^T-P_{1,1}^T\\ v_1{P}_{1,3}^T-P_{1,2}^T\\ ⋮\\ u_n{P}_{n,3}^T-P_{n,1}^T\\ u_n{P}_{n,3}^T-P_{n,2}^T\end{array}\right]y=0\to Dy=0$$于是，$y$为$D$零空间中的一个非零元素。

7. 由于$D\in R^{2n\times 4}$，在观测次数大于等于两次时，很可能$D$满秩（因为容易得到4个线性无关组），无零空间。

8. 寻找最小二乘解：$$\underset{y}{\min }||Dy|{|}_2^2$$$$st.||y||=1$$解法：对$D^{T}D$进行SVD分解：$$D^{T}D=V\Sigma V^T$$其中V为4×4的酉矩阵。或者还可以表示为：$$D^{T}D=\sum _{i=1}^4{\sigma }_i^2{u}_i{u}_j^T$$
   其中${\sigma }_i$为奇异值，且由大到小排列，$u_i$,$u_j$正交。

9. 此时取$y=u_4$，那么该问题的目标函数值为${\sigma }_4$。
   10.判断该解有效性的条件：${\sigma }_4<<{\sigma }_3$。若该条件成立，则认为三角化有效，否则认为三角化无效。

代码实现

//
// Created by hyj on 18-11-11.
//
#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;
    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 个 三维特征点
    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);
    // 这个特征从第三帧相机开始被观测，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();
        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::homework; 请完成三角化估计深度的代码
    // 遍历所有的观测数据，并三角化
    Eigen::Vector3d P_est;           // 结果保存到这个变量
    P_est.setZero();
    /* 对应上述公式部分 */
    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 + start_frame_id].Rwc.transpose();
        T_tmp.block<3, 1>(0, 3) = -camera_pose[j + start_frame_id].Rwc.transpose() 
                                        * camera_pose[j + start_frame_id].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 + start_frame_id].uv[0] * P_k3 - P_k1;
        D.block<1, 4>(2 * j + 1, 0) = camera_pose[j + start_frame_id].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();
    auto tmp = res_U.rightCols(1);
    std::cout << "res=" << tmp / tmp(3) << std::endl;
    /* 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;
}