VIO学习笔记（四）—— 单目Bundle Adjustment信息矩阵代码分析

最新推荐文章于 2023-07-14 15:16:53 发布

梁小憨憨

最新推荐文章于 2023-07-14 15:16:53 发布

阅读量674

点赞数

分类专栏： VIO 文章标签： slam

本文链接：https://blog.csdn.net/qq_41990294/article/details/105257232

版权

VIO 专栏收录该内容

11 篇文章 18 订阅

订阅专栏

计算单目Bundle Adjustment信息矩阵，并通过奇异值分解表明零空间为７维。

VIO学习笔记（四）—— 基于滑动窗口算法的 VIO 系统:可观性和一致性

预备知识

代码中相机位姿的由来：

在这里插入图片描述

最小二乘问题的图形化表示：

在这里插入图片描述

重投影误差：

重投影误差对转换后坐标点的jacobian
在这里插入图片描述
重投影误差对位姿的jacobian

信息矩阵的稀疏性

相关信息可以参考我的这篇博客
在这里插入图片描述

生成信息矩阵的形式：

在这里插入图片描述

代码部分

//
// Created by hyj on 18-11-11.
//
#include <iostream>
#include <vector>
#include <random>  
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

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);   //信息矩阵１２０×１２０
    H.setZero();

    std::vector<Pose> camera_pose;
    double radius = 8;   //半径
    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/4 圆弧上运动，产生相应的姿态

    // 随机数生成三维特征点
    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
            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;   //残差对世界坐标点的jacobian
            Eigen::Matrix<double,2,6> jacobian_Ti;   //残差对李代数的jacobian
            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;

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

    std::cout << "----------------H--------------------" << std::endl;
    std::cout << H << std::endl;
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(H);
    std::cout << "------------------------------------" << std::endl;
    std::cout << saes.eigenvalues() <<std::endl;

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
    std::cout << "------------------------------------" << std::endl;
    std::cout << svd.singularValues() <<std::endl;
  
    return 0;
}