手写VIO总结（四）

手写VIO总结（四）
手写VIO总结一
手写VIO总结二

1、

2、

需要用到的知识：

具体代码实现
CMakeLists.txt

cmake_minimum_required(VERSION 3.5.1)
project(NullSpaceTest)

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

set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)

find_package(Eigen3 REQUIRED)
#find_package(Sophus REQUIRED)

include_directories(
        ${EIGEN3_INCLUDE_DIR}
#        ${Sophus_INCLUDE_DIRS}
)

add_executable(NullSpaceTest hessian_nullspace_test.cpp)
#target_link_libraries(NullSpaceTest  ${Sophus_LIBRARIES})

hessian_nullspace_test.cpp

//
// Created by hyj on 18-11-11.
//
#include <fstream>
#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;//平移向量
};
int main()
{
    int featureNums = 20;//特征点数量，特征点是三维的
    int poseNums = 10;//位姿数量，相机位姿是六维的缺少尺度
    int diem = poseNums * 6 + featureNums * 3;
    double fx = 1.;
    double fy = 1.;
    Eigen::MatrixXd H(diem,diem);//生成器120×120的矩阵
    H.setZero();
    //std::cout<<H<<std::endl;

    std::vector<Pose> camera_pose;//定义相机pose
    double radius = 8;
    /*********************相机位姿************************camera做圆弧运动*/
    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());   //每次绕Z轴旋转2/pi
        //std::cout<<"rotation matrix"<<R<<std::endl;
        Eigen::Vector3d t = Eigen::Vector3d(radius * cos(theta) - radius, radius * sin(theta), 1 * sin(2 * theta));
        //std::cout<<"translation matrix"<<t<<std::endl;
        camera_pose.push_back(Pose(R,t));
    }
    /*****************************************************/
    // 随机数生成三维特征点
    std::default_random_engine generator;//创建随机数引擎
    std::vector<Eigen::Vector3d> points;
    for(int j = 0; j < featureNums; ++j)
    {
        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);//世界坐标系的特征点
        points.push_back(Pw);//产生世界坐标系中的特征点

        for (int i = 0; i < poseNums; ++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();
            double z_2 = z * z;
            Eigen::Matrix<double,2,3> jacobian_uv_Pc;//相机投影方程关于相机坐标系下的三维点的导数
            jacobian_uv_Pc<< fx/z, 0 , -x * fx/z_2,
                    0, fy/z, -y * fy/z_2;
            Eigen::Matrix<double,2,3> jacobian_Pj = jacobian_uv_Pc * Rcw;
            //误差相对于李代数的雅克比矩阵2×6与视觉SLAM十四讲P196公式（8.15）一致
            Eigen::Matrix<double,2,6> jacobian_Ti;
            jacobian_Ti << -x* y * fx/z_2, (1+ x*x/z_2)*fx, -y/z*fx, fx/z, 0 , -x * fx/z_2,
                     -(1+y*y/z_2)*fy, x*y/z_2 * fy, x/z * fy, 0,fy/z, -y * fy/z_2;
            //jacobian_Ti<<fx/z, 0 , -x * fx/z_2,-x* y * fx/z_2, (1+ x*x/z_2)*fx, -y/z*fx,
            //      0,fy/z, -y * fy/z_2,-(1+y*y/z_2)*fy, x*y/z_2 * fy, x/z * fy;
            //solve information matrix

            /********************************************************************************/
            H.block(j*3 + 6*poseNums,j*3 + 6*poseNums,3,3) +=jacobian_Pj.transpose()*jacobian_Pj;
            H.block(j*3 + 6*poseNums,i*6 , 3,6) += jacobian_Pj.transpose() * jacobian_Ti;
            H.block(i*6,j*3 + 6*poseNums, 6,3) += jacobian_Ti.transpose()*jacobian_Pj;
            H.block(i*6,i*6,6,6) += jacobian_Ti.transpose() * jacobian_Ti;
            /********************************************************************************/

        }
    }
    std::cout<<H<<std::endl;
    ofstream outf;
    outf.open("result.txt");
    outf<<H<<"\n";
    outf.close();
//    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(H);
//    std::cout << saes.eigenvalues() <<std::endl;

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
    std::cout << svd.singularValues()<<std::endl;//通过SVD分解得到特征值
    return 0;
}

代码运行结果：