【深蓝学院】手写VIO第4章--基于滑动窗口算法的 VIO 系统：可观性和 一致性--笔记

0. 内容

1. T1

参考SLAM14讲P247直接可写，注意${\xi }_1,{\xi }_2$之间有约束（关系）。

套用舒尔补公式：
marg掉${\xi }_1$之后，信息被传递到$L_1和L_2$之间了。

2. T2 证明高斯分布信息矩阵是协方差矩阵的逆

2.1 证明过程

全文读完，文章目的就是为了证明对于Gaussian distribution，二阶偏导矩阵系数H(Hessian Matrix)是等于协方差矩阵的逆：

证明方法即分别求主对角线和非主对角线上的二阶偏导，构造H。
证明步骤如下：
Gaussian Distribution取负对数

求二阶偏导构造H（主对角线$ll$，非主对角线$l{l}^{\prime }(l\ne l^{\prime })$）

主对角线二阶偏导：用中心差分代替求导

中心差分见 之前博客 的2.2节，一加一减，分母取2倍。

H非对角线元素类似：

对于一维高斯分布取负对数：

得证。

2.2 文章总结

本文证明了Gaussian随机分布的Hessian矩阵是协方差矩阵的逆，对于Gamma分布，
当β越小时，用Hessian近似协方差逆的误差越小，文中的实验表明近似误差和步长有关，且
变异系数越小(1/根号α)，误差越小。

3. T3

课上同学的作业分享：

我的结果：

1.

我的维度不对 ，整个H的分布式先pose再landmark，所以landmark需要全部在pose之后，i要改为poseNums。
2.

3. 整体代码
Jacobian计算部分参照《14讲》$P_{186-187}$

//
// Created by hyj on 18-11-11.
//
#include 
#include 
#include   
#include 
#include 
#include 

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

            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 of residual with respect to point in camera coordinate
            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;
            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;

            H.block(i*6,i*6,6,6) += jacobian_Ti.transpose() * jacobian_Ti;//guan yu pose de Jacobian
            /// 请补充完整作业信息矩阵块的计算
            //zhu dui jiao xian 3*3
//            H.block(i * 6 + j * 3,i * 6 + j * 3,3,3) += jacobian_Pj.transpose() * jacobian_Pj; //mine
//            H.block(i * 6 ,i * 6 + j * 3,6,3) += jacobian_Ti.transpose() * jacobian_Pj;
//            H.block(i * 6 + j * 3, i * 6 ,3,6) += jacobian_Pj.transpose() * jacobian_Ti;
            //zheng que de
            H.block(poseNums * 6 + j * 3,poseNums * 6 + j * 3,3,3) += jacobian_Pj.transpose() * jacobian_Pj;
            H.block(i * 6 ,poseNums * 6 + j * 3,6,3) += jacobian_Ti.transpose() * jacobian_Pj;
            H.block(poseNums * 6 + j * 3, i * 6 ,3,6) += jacobian_Pj.transpose() * jacobian_Ti;
        }
    }

//    std::cout << H << std::endl;
//    Eigen::SelfAdjointEigenSolver saes(H);
//    std::cout << saes.eigenvalues() <

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

最后的SVD分解还不是很熟，矩阵论忘差不多了，后面再复习吧。

本章完。

4. 第5章相关内容

尝试了一下知乎的编辑器 https://zhuanlan.zhihu.com/p/659499854