手写VIO作业总结（六）

手写VIO作业总结（六）

1、作业

2.1 证明

2.2 代码实现

CmakeLists

cmake_minimum_required(VERSION 3.5.1)
project(Triangulate)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")

set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)

find_package(Eigen3 REQUIRED)

include_directories(
        ${EIGEN3_INCLUDE_DIR}
)

add_executable(estimate_depth triangulate.cpp)
#target_link_libraries(estimate_depth  ${Sophus_LIBRARIES})

triangulation.cpp

//
// Created by hyj on 18-11-11.
//
#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); // 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);   //landmark
    // 这个特征从第三帧相机开始被观测，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 */
    //设置稀疏矩阵D
    int D_rows = 2 * (end_frame_id - start_frame_id);
    Eigen::MatrixXd D = Eigen::MatrixXd::Zero(D_rows, 4);
    for (int i = start_frame_id; i < end_frame_id;++i)
    {
        //每轮生成2×4的矩阵
        Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
        Eigen::Vector3d tcw = -Rcw * camera_pose[i].twc;
        D.block(2 * (i - start_frame_id), 0, 1, 3)
            .noalias()  //声明.noalias()这里不存在矩阵混淆问题ref:https://blog.csdn.net/weixin_30550081/article/details/95276173
            = camera_pose[i].uv(0) * Rcw.block(2, 0, 1, 3)
            - Rcw.block(0, 0, 1, 3);  // D()
        D.block(2 * (i - start_frame_id), 3, 1, 1).noalias() =
        camera_pose[i].uv[0] * tcw.segment(2, 1) - tcw.segment(0, 1);
        D.block(2 * (i - start_frame_id) + 1, 0, 1, 3).noalias()=
        camera_pose[i].uv(1) * Rcw.block(2, 0, 1, 3) - Rcw.block(1, 0, 1, 3);
        D.block(2 * (i - start_frame_id) + 1, 3, 1, 1).noalias()
        = camera_pose[i].uv(1) * tcw.segment(2, 1) - tcw.segment(1, 1);
        // std::cout << "D matrix" << D.matrix() << std::endl;
    }
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(
        D.transpose() * D, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::Vector4d lamda = svd.singularValues();
    std::cout << "singularValues = " << lamda.transpose() << std::endl;
    if(lamda(2)/lamda(3)<1e-3){
        std::cout << "The parallax is not enough! " << std::endl;
        return -1;
    }
    // std::cout << "D is Size: " << (D.transpose()*D).size() << std::endl;
    // std::cout << "U " << svd.matrixU() << std::endl;
    // std::cout << "V " << svd.matrixV() << std::endl;
    Eigen::Vector4d u4 = svd.matrixU().block(0, 3, 4, 1);
    if(u4(3)!=0&&u4(2)/u4(3)>0){
        P_est(0) = u4(0) / u4(3);
        P_est(1) = u4(1) / u4(3);
        P_est(2) = u4(2) / u4(3);
    }
    /* your code end */
    std::cout << "ground truth: \n" << Pw.transpose() << std::endl;
    std::cout <<"your result: \n"<< P_est.transpose() <<std::endl;
    // TODO:: 请如课程讲解中提到的判断三角化结果好坏的方式，绘制奇异值比值变化曲线
    return 0;
}

SVD分解（基于eigen代码实现）

//J= U * S * VT
//J:m*n;U:m*m;V:n*n
JacobiSVD svd(J, ComputeThinU | ComputeThinV);
U=svd.matrixU();
V=svd.matrixV();
A=svd.singularValues();//A为奇异值，即对角线元素

svd分解

Eigen::MatrixXf m = Eigen::MatrixXf::Random(3, 2);
    std::cout << "Here is the matrix m:" << std::endl << m << std::endl;
    Eigen::JacobiSVD<Eigen::MatrixXf> svd(m, Eigen::ComputeThinU | Eigen::ComputeThinV);
    std::cout << "Its singular values are:" << std::endl << svd.singularValues()[0] << std::endl;
    Eigen::Matrix2f diag = Eigen::Matrix2f::Zero(2, 2);
    diag(0, 0) = svd.singularValues()[0];
    diag(1, 1) = svd.singularValues()[1];
    std::cout
        << "Its left singular vectors are the columns of the thin U matrix:"
        << std::endl
        << svd.matrixU() << std::endl;
    std::cout << "Its right singular vectors are the columns of the thin V matrix:"
         << std::endl
         << svd.matrixV() << std::endl;
    Eigen::Matrix2f result_m = svd.matrixU() * diag * svd.matrixV();

矩阵的奇异值分解过程