三角測量的一些基礎理論

三角測量，通俗一點講就是通過在一個相機在兩個不同的位置（或者雙目相機）對一個目標點進行觀測，依靠兩個觀測系間的絕對（或相對）位姿變換關係，算出該被觀測點在兩個觀測系中的絕對（或相對）深度。進一步推廣，當我們在n個不同的位置對同一個點進行觀測，並已知這些觀測系相對於一固定系的變換時，便可通過求解超定方程組，求出在該固定系下被觀測點座標的最優解。

構建超定方程組

設待觀測點P在世界系的座標爲 $P(X,Y,Z)$ ，我們對它進行多次觀測。在產生第n次觀測時，相機系與世界系之間的變換表示爲 $T_{cw\_n}=\{R,t\}$ (後面簡寫作 $T_{n}$ )，這個觀測表示爲該相機系下的其次座標 $p_{n}(x,y,1)$ 。此時我們可以得到 $T_{n} \overline{P} =λ_{n} \left( \begin{array}{c} x \\ y \\ 1 \end{array} \right)$ 其中 $λ_{n}$ 是在當前相機系被觀測點的深度, $\overline{P}$ 是P的齊次座標。在此基礎上，我們可以非常容易得出
$T_{n1}\overline{P}-xT_{n3}\overline{P}=0$ $T_{n2}\overline{P}-yT_{n3}\overline{P}=0$ 其中 $T_{ni}$ 代表T矩陣的第i行。
將待求變量分離出來，改寫爲 $\left( \begin{array}{c} T_{n1}-xT_{n3} \\ T_{n2}-yT_{n3} \end{array} \right) \overline{P}=0$ 括號內是一個2x4的矩陣，當我們有了多個觀測時，便可以構造出一個2n*4的方程組。

解的分析

下面我們來分析一下我們能夠得到什麼樣的解，首先我們從幾何角度上看：當我們若只有一個觀測時，因爲深度未知，所以P可能出現在沿當前觀測系下深度方向上的任意一點；當有兩個或兩個以上不同位置的觀測（這決定了方程組有效方程的個數）時，該點在給定系下的絕對位置便可以確定了，但由於觀測誤差的存在，我們這裏一般通過兩個以上的觀測求得一個最優解。
從齊次方程組的性質上分析：對於Ax=0（其中A是m*n的矩陣），我們通常先構造 $A^{T}Ax=0$ 。若 $det\left(A^{T}A\right)\not=0$ ，也就是說矩陣 $A^{T}A$ 滿秩，此時只有零解；當 $det\left(A^{T}A\right)=0$ 時，有無數多組線性相關的解。因爲待求解的向量空間只有三自由度（齊次座標最後一維是1），所以我們只取一組即可，通常求取 $\left|x\right|=1$ 的解再進行歸一化。
接下來我們談一談這兩個角度的一些聯繫，但在往下繼續講之前，我們先再簡單說一下零空間的問題：矩陣 $A^{T}A$ 的零空間，對應了方程 $A^{T}Ax=0$ 的所有解的集合。因爲前文已經提到了，齊次座標的自由度是3，所以首先， $A^{T}A$ 的零空間維數至少爲1，此時方程不會只有零解。爲什麼說至少爲1呢？ 此時對應我們前面在幾何角度的分析，如果我們只擁有一個觀測的話，那麼在空間中便無法有多條射線形成一個交點，在其對應觀測系下的深度我們是可以任意取值的。這就造成了系統狀態空間不可觀的維度（即零空間的維度）又加了1，唯一解退化成了無數個線性相關（與觀測系下深度線性相關）的解。那麼對於兩個及以上的觀測，零空間爲1，求解齊次座標可通過對 $A^{T}A$ 進行SVD，找出最小的那個奇異值（往往特別特別小和次小相比可忽略不計），所對應的特徵向量便是我們所需要的解。

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

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); 
        // 繞 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);
    std::cout << tx << " " << ty << " " << tz << std::endl;


    std::default_random_engine noise_generator;

    double variance;
    variance = 0.0;

    std::normal_distribution<double> noise_pdf(0, variance);
    double noise;


    Eigen::Vector3d Pw(tx, ty, tz);
    std::cout << "generate 3D point finished" << std::endl;

    int start_frame_id = 0;
    int end_frame_id = 1;

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

        noise = noise_pdf(noise_generator);
        std::cout << noise << std::endl;
        double x = Pc.x() + noise;
        noise = noise_pdf(noise_generator);
        std::cout << noise << std::endl;
        double y = Pc.y() + noise;
        noise = noise_pdf(noise_generator);
        std::cout << noise << std::endl << std::endl;
        double z = Pc.z() + noise;

        camera_pose[i].uv = Eigen::Vector2d(x/z,y/z);
    }
    std::cout << "measurement generated" << std::endl;
    
    Eigen::Vector3d P_est;         
    int D_row = 2 * (end_frame_id - start_frame_id);
    Eigen::MatrixXd D;
    D.resize(D_row, 4);
    for(int i = 0; i < D_row/2; i++)
    {
        //對應camera_pose[start_frame+i]
        Eigen::MatrixXd T(3,4);
        Eigen::Matrix3d rotation = camera_pose[i+start_frame_id].Rwc.transpose();
        Eigen::Vector3d trans = -1 * rotation * camera_pose[i+start_frame_id].twc;
        for(int j = 0; j < 3; j++)
                T.block(j, 0, 1, 4) << rotation(j,0), rotation(j,1), rotation(j,2), trans(j);
        std::cout << "T:" << std::endl << T << std::endl;
            
        D.block(i*2, 0, 1, 4) = camera_pose[i+start_frame_id].uv(0) * T.block(2, 0, 1, 4) - T.block(0, 0, 1, 4);
        D.block(i*2+1,0,1, 4) = camera_pose[i+start_frame_id].uv(1) * T.block(2, 0, 1, 4) - T.block(1, 0, 1, 4);
    }
    std::cout << "D:" << std::endl << D << std::endl;
    Eigen::MatrixXd square(4,4);
    square = D.transpose() * D;
   
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(square, Eigen::ComputeThinU | Eigen::ComputeThinV);
    
    std::cout << "Singular values: " << std::endl << svd.singularValues() << std::endl;
    std::cout << "matrix U: " << std::endl << svd.matrixU() << std::endl;
    std::cout << "matrix v: " << std::endl << svd.matrixV() << std::endl;
    std::cout <<"ground truth: \n"<< Pw.transpose() <<std::endl;

    double ddd = svd.matrixU()(1,3);
    P_est << svd.matrixU()(0,3)/svd.matrixU()(3,3), svd.matrixU()(1,3)/svd.matrixU()(3,3), svd.matrixU()(2,3)/svd.matrixU()(3,3); 
    std::cout <<"my result: \n"<< P_est.transpose() <<std::endl;

    return 0;
}