三角测量的一些基础理论

三角测量，通俗一点讲就是通过在一个相机在两个不同的位置（或者双目相机）对一个目标点进行观测，依靠两个观测系间的绝对（或相对）位姿变换关系，算出该被观测点在两个观测系中的绝对（或相对）深度。进一步推广，当我们在n个不同的位置对同一个点进行观测，并已知这些观测系相对于一固定系的变换时，便可通过求解超定方程组，求出在该固定系下被观测点坐标的最优解。

构建超定方程组

设待观测点P在世界系的坐标为$P(X,Y,Z)$，我们对它进行多次观测。在产生第n次观测时，相机系与世界系之间的变换表示为$T_{cw\_n}=\{R,t\}$(后面简写作$T_n$)，这个观测表示为该相机系下的其次坐标$p_n(x,y,1)$。此时我们可以得到$$T_n\overline{P}={\lambda }_n\left(\begin{array}{c}x\\ y\\ 1\end{array}\right)$$其中${\lambda }_n$是在当前相机系被观测点的深度,$\overline{P}$是P的齐次坐标。在此基础上，我们可以非常容易得出
$$T_{n1}\overline{P}-x{T}_{n3}\overline{P}=0$$$$T_{n2}\overline{P}-y{T}_{n3}\overline{P}=0$$其中$T_{ni}$代表T矩阵的第i行。
将待求变量分离出来，改写为$$\left(\begin{array}{c}{T}_{n1}-x{T}_{n3}\\ T_{n2}-y{T}_{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)\ne 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 
#include 
#include   
#include 
#include 
#include 
#include 

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

以上代码提供了一个简单的三角化例程，在生成了一个三维点并只用一个相机对他进行观测。可以发现四个奇异值两个都极小，接近于零，这也印证了我们上文的分析，这种状况下待求解的变量空间的不可观维数是2。

（如有需要转发，请注明出处～）