VSLAM：：[手写VIO_课堂笔记]第四讲(下)_基于滑动窗口算法的VIO系统原理

1. 第四讲(下)_基于滑动窗口算法的VIO系统

1.1. 滑动窗口算法

1.1.1. 图的表示

解释: (假设矩阵左上角元素索引(1,1))

1. ${\Lambda }_1$: (1,2)元素不为空, 表示第1个顶点与第2个顶点之间的残差$r_{12}$与顶点1\2有关系
2. ${\Lambda }_2$: (1,3)元素不为空, 表示第1个顶点与第3个顶点之间的残差$r_{13}$与顶点1\3有关系
3. 这部分可以参考<<视觉SLAM14讲>>

1.1.2. 基于边际概率的滑动窗口算法

根据第四讲(上)部分的叙述, 由于我们只有信息矩阵的数值形式, 但是没有各个状态量$x_1,x_2,x_3$各自相关的项的区分, 因此老的变量移除就涉及到边际概率信息矩阵的计算.

解释:

1. 在丢弃之前, 顶点2,3,4,5都只与顶点1相连
2. 丢弃顶点1之后, 利用边际概率方法更新信息矩阵, 更新之后的信息矩阵所对应的图表示: 丢弃顶点1之后, 顶点2,3,4,5之间相互连接起来了.
3. 总结: 当给定顶点1的情况下时, 顶点2,3,4,5之间是条件独立的, 当顶点1这个条件去掉之后, 顶点2,3,4,5又相互关联了.

1.1.3. 滑动窗口中的FEJ算法

1.1.3.1. 加入一个新的变量

• $造成的问题就是:$ 对于 SLAM 系统而言(如单目 VO), 当我们改变状态量时, 测量不变意味着损失函数不会改变, 更意味着求解最小二乘时对应的信息矩阵$\Lambda$存在着零空间。

1.1.3.2. 可观性

1.1.3.3. 举例

解释:

1. 单目SLAM: 就是说测量到的姿态和位置都是相对于某个坐标系的, 如果这个坐标系没有固定下来, 那么可以对(姿态,位置以及路标点landmark坐标)都乘以某个变换矩阵$T$, 而残差函数$e$的值并不改变.
2. 单目+IMU: IMU根据重力可以获取两个绝对的值:roll和pitch, 这两个值是相对于固定的坐标系的, 如东北天导航系. 另外, 由于IMU的测量信息, 把尺度的不确定性也消除了, 使得尺度因子变成可观. 最后: yaw角与重力没有关系, 没有了绝对的观测, 3维的位置也没有像GPS那样的绝对观测, 这4个自由度是不可观的, 即存在不确定性(最终的结果是一个相对的值而不是绝对的值).

1.1.3.4. 滑动窗口中出现的零空间问题

---

作业

//
// 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));
    }

    // 随机数生成三维特征点
    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);   //Pw= Rwc*pc + twc ==>  pc= Rcw(Pw-twc)

            double x = Pc.x();
            double y = Pc.y();
            double z = Pc.z();
            double z_2 = z * z;
            /// 两个优化变量分别是位姿和世界坐标系3D点(路标)
            /// 因此,需要求误差e分别对 位姿6维 和 世界坐标系3D点求偏导
            Eigen::Matrix<double,2,3> jacobian_uv_Pc;   //这里只是图像点(u,v)对相机坐标系下3D点(x,y,z)求导
            jacobian_uv_Pc<< fx/z, 0 , -x * fx/z_2,
                    0, fy/z, -y * fy/z_2;
            // 这里用了链式求导法则来求对世界坐标系3D点求偏导
            Eigen::Matrix<double,2,3> jacobian_Pj = jacobian_uv_Pc * Rcw;   //实际需要的是 图像点(u,v)对世界坐标系下3D点求导
            Eigen::Matrix<double,2,6> jacobian_Ti;      //图像点(u,v)对姿态6维求偏导,旋转在前,平移在后,推导见<视觉SLAM第二版>P187
            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(j*3 + 6*poseNums,j*3 + 6*poseNums,3,3) +=?????
            // H.block(i*6,j*3 + 6*poseNums, 6,3) += ???;
            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;
//    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;
  
    return 0;
}