三角测量的一些基础理论

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

构建超定方程组

设待观测点P在世界系的坐标为 P ( X , Y , Z ) P(X,Y,Z) P(X,Y,Z),我们对它进行多次观测。在产生第n次观测时,相机系与世界系之间的变换表示为 T c w _ n = { R , t } T_{cw\_n}=\{R,t\} Tcw_n={R,t}(后面简写作 T n T_{n} Tn),这个观测表示为该相机系下的其次坐标 p n ( x , y , 1 ) p_{n}(x,y,1) pn(x,y,1)。此时我们可以得到 T n P ‾ = λ n ( x y 1 ) T_{n} \overline{P} =λ_{n} \left( \begin{array}{c} x \\ y \\ 1 \end{array} \right) TnP=λnxy1其中 λ n λ_{n} λn是在当前相机系被观测点的深度, P ‾ \overline{P} P是P的齐次坐标。在此基础上,我们可以非常容易得出
T n 1 P ‾ − x T n 3 P ‾ = 0 T_{n1}\overline{P}-xT_{n3}\overline{P}=0 Tn1PxTn3P=0 T n 2 P ‾ − y T n 3 P ‾ = 0 T_{n2}\overline{P}-yT_{n3}\overline{P}=0 Tn2PyTn3P=0其中 T n i T_{ni} Tni代表T矩阵的第i行。
将待求变量分离出来,改写为 ( T n 1 − x T n 3 T n 2 − y T n 3 ) P ‾ = 0 \left( \begin{array}{c} T_{n1}-xT_{n3} \\ T_{n2}-yT_{n3} \end{array} \right) \overline{P}=0 (Tn1xTn3Tn2yTn3)P=0括号内是一个2x4的矩阵,当我们有了多个观测时,便可以构造出一个2n*4的方程组。

解的分析

下面我们来分析一下我们能够得到什么样的解,首先我们从几何角度上看:当我们若只有一个观测时,因为深度未知,所以P可能出现在沿当前观测系下深度方向上的任意一点;当有两个或两个以上不同位置的观测(这决定了方程组有效方程的个数)时,该点在给定系下的绝对位置便可以确定了,但由于观测误差的存在,我们这里一般通过两个以上的观测求得一个最优解。
从齐次方程组的性质上分析:对于Ax=0(其中A是m*n的矩阵),我们通常先构造 A T A x = 0 A^{T}Ax=0 ATAx=0。若 d e t ( A T A ) ≠ 0 det\left(A^{T}A\right)\not=0 det(ATA)=0,也就是说矩阵 A T A A^{T}A ATA满秩,此时只有零解;当 d e t ( A T A ) = 0 det\left(A^{T}A\right)=0 det(ATA)=0时,有无数多组线性相关的解。因为待求解的向量空间只有三自由度(齐次坐标最后一维是1),所以我们只取一组即可,通常求取 ∣ x ∣ = 1 \left|x\right|=1 x=1的解再进行归一化。
接下来我们谈一谈这两个角度的一些联系,但在往下继续讲之前,我们先再简单说一下零空间的问题:矩阵 A T A A^{T}A ATA的零空间,对应了方程 A T A x = 0 A^{T}Ax=0 ATAx=0的所有解的集合。因为前文已经提到了,齐次坐标的自由度是3,所以首先, A T A A^{T}A ATA的零空间维数至少为1,此时方程不会只有零解。为什么说至少为1呢? 此时对应我们前面在几何角度的分析,如果我们只拥有一个观测的话,那么在空间中便无法有多条射线形成一个交点,在其对应观测系下的深度我们是可以任意取值的。这就造成了系统状态空间不可观的维度(即零空间的维度)又加了1,唯一解退化成了无数个线性相关(与观测系下深度线性相关)的解。那么对于两个及以上的观测,零空间为1,求解齐次坐标可通过对 A T A A^{T}A ATA进行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;
}

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

(如有需要转发,请注明出处~)

已标记关键词 清除标记
©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页