從零手寫vio-作業6-三角化

2完成三角化程式碼

原始碼如下：

//
// Created by nnz on 18-11-11.
//
#include <iostream>
#include <vector>
#include <random>  
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
using namespace std;
//
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();
    /* your code begin */



    // step 1 construct D
    int D_rows=2*(end_frame_id-start_frame_id);//D的行數
    Eigen::MatrixXd D=Eigen::MatrixXd::Zero(D_rows,4);//初始化一下D
    for(int i=start_frame_id;i<end_frame_id;++i)
    {
        Eigen::Matrix<double,3,4> Tcw;
        Eigen::Matrix3d Rcw=camera_pose[i].Rwc.transpose();
        Eigen::Vector3d tcw=-Rcw*camera_pose[i].twc;
        Tcw<<Rcw,tcw;
        D.block(2*(i-start_frame_id),0,1,4)=camera_pose[i].uv(0)*Tcw.row(2)
                                                                                    -Tcw.row(0);
        D.block(2*(i-start_frame_id)+1,0,1,4)=camera_pose[i].uv(1)*Tcw.row(2)
                                                                                      -Tcw.row(1);
    }
    //step 2 放縮 max取D的最大的元素
    double max=D.maxCoeff();
    cout<<"D = "<< endl<<D <<endl;
    cout<<"The max num of D ="<< max <<endl;
    Eigen::MatrixXd DTD=(D/max).transpose()*(D/max);

    clock_t time_stt = clock();

   Eigen::JacobiSVD<Eigen::MatrixXd> svd_h(DTD,Eigen::ComputeThinU | Eigen::ComputeThinV);

    clock_t time_stt1 = clock();
    std::cout<<"SVD分解，耗時:\n"<<(clock()-time_stt1)/(double) CLOCKS_PER_SEC<<"ms"<<std::endl;

    Eigen::MatrixXd U=svd_h.matrixU();
    Eigen::MatrixXd V=svd_h.matrixV();
    Eigen::MatrixXd S(svd_h.singularValues());
    cout<<"U = "<<endl<<U<<endl;
    cout<<"V = "<<endl<<V<<endl;
    cout<<" singularValues "<< endl<<S <<endl;

    //step 3 判斷是否有效

    if(abs(svd_h.singularValues()[3]/svd_h.singularValues()[2])<1e-2)
    {
        Eigen::Vector4d u4=max*U.block(0,3,4,1);
        P_est=(u4/u4[3]).head(3);//歸一化
    }
    else
    {
        cout<<"無效!"<<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;
}

執行結果如下：

D = 
-0.891007  -0.45399 -0.478253   1.25886
  0.45399 -0.891007 -0.339294   3.90642
-0.809017 -0.587785 -0.548566   2.04958
 0.587785 -0.809017 -0.432396   5.11352
-0.707107 -0.707107 -0.626707   2.96985
 0.707107 -0.707107  -0.51176   6.16861
-0.587785 -0.809017 -0.707633   3.97072
 0.809017 -0.587785 -0.571913   7.01606
 -0.45399 -0.891007  -0.78662   5.00446
 0.891007  -0.45399 -0.609762   7.62136
-0.309017 -0.951057 -0.860286   6.03353
 0.951057 -0.309017 -0.625116   7.97589
-0.156434 -0.987688 -0.927108   7.03502
 0.987688 -0.156434 -0.620237   8.09317
The max num of D =8.09317
SVD分解，耗時:
1e-06ms
U = 
0.0530721  0.846878   0.41558  0.327528
-0.103079  0.431629 -0.895388 0.0367562
-0.102585  0.309021  0.122288 -0.937565
 0.987945 0.0316285 -0.103049 -0.111113
V = 
0.0530721  0.846878   0.41558  0.327528
-0.103079  0.431629 -0.895388 0.0367562
-0.102585  0.309021  0.122288 -0.937565
 0.987945 0.0316285 -0.103049 -0.111113
 singularValues 
     7.1513
   0.118267
  0.0110422
1.88518e-17
ground truth: 
  -2.9477 -0.330799   8.43792
your result: 
  -2.9477 -0.330799   8.43792