VIO第四讲
Source
Edit
History

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

Catalogue
  1. 1. 1. 第四讲(下)_基于滑动窗口算法的VIO系统
    1. 1.1. 1.1. 滑动窗口算法
      1. 1.1.1. 1.1.1. 图的表示
      2. 1.1.2. 1.1.2. 基于边际概率的滑动窗口算法
      3. 1.1.3. 1.1.3. 滑动窗口中的FEJ算法
        1. 1.1.3.1. 1.1.3.1. 加入一个新的变量
        2. 1.1.3.2. 1.1.3.2. 可观性
        3. 1.1.3.3. 1.1.3.3. 举例
        4. 1.1.3.4. 1.1.3.4. 滑动窗口中出现的零空间问题
    2. 1.2. 作业

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. 加入一个新的变量

解释: * 就是说, 如果没有进行marg的情况时, 求解过程中的信息矩阵\(\Lambda\)是不满秩的, 此时系统可以有多个满足最小化损失函数的解\(x\)

  • \(\color{red}{造成的问题就是:}\) 对于 SLAM 系统而言(如单目 VO), 把原本多个解的问题变成只有一个确定的解

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. 滑动窗口中出现的零空间问题

作业

1. 画出相机变量\(\xi_1\)被marg之后的信息矩阵 2. 画出相机变量\(\xi_2\)被marg之后的信息矩阵

(不知道是不是这样做?) 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
//
// 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;
}