手写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 
#include 
#include 
#include   
#include 
#include 
#include 
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< 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"< points;
    for(int j = 0; j < featureNums; ++j)
    {
        std::uniform_real_distribution xy_rand(-4, 4.0);//创建取值范围
        std::uniform_real_distribution 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 jacobian_uv_Pc;//相机投影方程关于相机坐标系下的三维点的导数
            jacobian_uv_Pc<< fx/z, 0 , -x * fx/z_2,
                    0, fy/z, -y * fy/z_2;
            Eigen::Matrix jacobian_Pj = jacobian_uv_Pc * Rcw;
            //误差相对于李代数的雅克比矩阵2×6与视觉SLAM十四讲P196公式（8.15）一致
            Eigen::Matrix 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< saes(H);
//    std::cout << saes.eigenvalues() < svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
    std::cout << svd.singularValues()<

代码运行结果：