【三角测量法】slam小白看懂的三角测量法

1. 单目

1.1. 问题背景

我们现在有着的数据是

1. 第$i$帧相机位姿$R_i$,$t_i$,$T_i$
2. 第$i-1$帧相机位姿$R_{i-1}$,$t_{i-1}$,$T_{i-1}$
3. 这两帧中已经匹配好的某个特征点在两帧之中的归一化坐标$P_i$,$P_{i-1}$

我们需要求解的是这个特征点在世界坐标系的坐标$P_w$以及深度$s_i$,$s_{i-1}$

1.2. 推导过程

1.2.1. 世界坐标的推导

首先我们考虑$P_w$的齐次坐标,方便维度对齐
$$P_w=(X_w,Y_w,Z_w,1)\text{\hspace{1em}(1)}$$
考虑相机模型,其中$P_{c1}$,$P_{c2}$为第$i$帧和第$i-1$帧特征点在相机坐标系的坐标
$$P_{c1}=\left[\begin{array}{ccc}{R}_i& |& t_i\end{array}\right]P_w\text{\hspace{1em}(2)}$$

将$P_{c1}$,$P_{c2}$转换为归一化坐标

$$\begin{gathered} Z_{c1}{P}_i=\left[\begin{array}{ccc}{R}_i& |& t_i\end{array}\right]P_w={\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]}_{3\times 1}=T_{i_{3\times 4}}{\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]}_{4\times 1} \\ \to P_i=T{P}_w\text{\hspace{1em}(3)} \end{gathered}$$

根据$(3)$这个等式,$P_i$和$T{P}_w$共线,所以它们的叉积为0
$$P_i\times T{P}_w=0\text{\hspace{1em}(4)}$$

$$P_i^{\wedge }T{P}_w=0\text{\hspace{1em}(5)}$$

$$P_i^{\wedge }=\left[\begin{array}{ccc}0& -1& v_i\\ 1& 0& -u_i\\ -v_i& u_i& 0\end{array}\right]\text{\hspace{1em}(6)}$$

为了方便展开,我们将$T$用一种简单的方式展开
$$T=\left[\begin{array}{c}{r}_1\\ r_2\\ r_3\end{array}\right]\text{\hspace{1em}(7)}$$
那么(5)可以表示为
$$\left[\begin{array}{ccc}0& -1& v_i\\ 1& 0& -u_i\\ -v_i& u_i& 0\end{array}\right]\left[\begin{array}{c}{r}_1\\ r_2\\ r_3\end{array}\right]P_w=\left[\begin{array}{c}-r_2+v_i{r}_3\\ r_1-u_i{r}_3\end{array}\right]P_w=0\text{\hspace{1em}(8)}$$
同理,对于第$i-1$帧
$$\left[\begin{array}{c}-r_2+v_{i-1}{r}_3\\ r_1-u_{i-1}{r}_3\end{array}\right]P_w=0\text{\hspace{1em}(9)}$$

如今我们有四个约束式子,且形式都是$Ax=b$的形式,那么我们用SVD方法求解这两个方程,通过SVD解得了四个数$x_0$,$x_1$,$x_2$,$x_3$,那么
特征点在世界坐标系的的三维坐标为：
$$P_w=\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=\left[\begin{array}{c}\frac{x_0}{x_3}\\ \frac{x_1}{x_3}\\ \frac{x_2}{x_3}\\ 1\end{array}\right]0\text{\hspace{1em}(1)}$$

1.2.2. 深度计算

如今我们有了特征点的世界坐标和变换矩阵,那么我们求解特征点在相机坐标系中的坐标就可以解出了
$$Z_{c1}{P}_{c1}=\left[\begin{array}{ccc}{R}_i& |& t_i\end{array}\right]P_{w}1\text{\hspace{1em}(1)}$$
$$Z_{c2}{P}_{c2}=\left[\begin{array}{ccc}{R}_{i-1}& |& t_{i-1}\end{array}\right]P_{w}2\text{\hspace{1em}(1)}$$
其中$Z_{c1}$和$Z_{c2}$就是我们需要的深度了

2.双目

• $O_L$,$O_R$:左右光圈中心
• 蓝色框为成像平面，$f$为焦距
• $u_R$,$u_R$:成像平面的坐标
  因为$\Delta P{P}_L{P}_R\sim \Delta P{O}_L{O}_R$
  $$\frac{z-f}{z}=\frac{b-u_L+u_R}{b}\text{\hspace{1em}(13)}$$
  所以
  $$z=\frac{fb}{d}$$
  其中
  $z$:深度
  $d$:左右图像横坐标之差，$u_L-u_R$

3. 代码实践

#include <iostream>
#include <vector>
#include <random>
#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

using namespace Eigen;
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(); // to camera from world 旋转
        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();
    /* your 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();
    // 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) << std::endl;
    /* your code end */
    std::cout << "ground truth: \n"
              << Pw.transpose() << std::endl;
    std::cout << "your result: \n"
              << P_est.transpose() << std::endl;
    return 0;
}