《视觉SLAM进阶：从零开始手写VIO》第四讲-基于滑动窗口算法的VIO系统原理-作业

《视觉SLAM进阶：从零开始手写VIO》第四讲-基于滑动窗口算法的VIO系统原理-作业

1 信息矩阵与边缘化

1.1 信息矩阵

$$顶点vertex（要优化的变量）：\mathbf{x}=\left[\begin{array}{c}{\xi }_1\\ {\xi }_2\\ {\xi }_3\\ L_1\\ L_2\\ L_3\end{array}\right]$$

$$边edge（顶点之间构建的残差）：\mathbf{r}=\left[\begin{array}{c}\mathrm{r}(\xi 1,\xi 2)\\ \mathrm{r}\left(\xi 1,{\mathrm{L}}_1\right)\\ \mathrm{r}\left(\xi 1,{\mathrm{L}}_2\right)\\ \mathrm{r}\left(\xi 1,{\mathrm{L}}_2\right)\\ \mathrm{r}\left(\xi 2,{\mathrm{L}}_1\right)\\ \mathrm{r}\left(\xi 2,{\mathrm{L}}_2\right)\\ \mathrm{r}\left(\xi 2,{\mathrm{L}}_3\right)\\ \mathrm{r}\left(\xi 3,{\mathrm{L}}_2\right)\\ \mathrm{r}\left(\xi 3,{\mathrm{L}}_2\right)\end{array}\right]$$

$$公式中的雅克比矩阵：\mathbf{J}=\frac{\partial \mathbf{r}}{\partial \mathbf{x}}=\left[\begin{array}{l}\frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \mathbf{x}}\\ \frac{\partial \mathrm{r}\left(\xi 1,{\mathrm{L}}_1\right)}{\partial \mathbf{x}}\\ \frac{\partial \mathrm{r}\left(\xi 1,{\mathrm{L}}_2\right)}{\partial \mathbf{x}}\\ \frac{\partial \mathrm{r}(\xi 2,\xi 3)}{\partial \mathbf{x}}\\ \frac{\partial \mathrm{r}\left(\xi 2,{\mathrm{L}}_1\right)}{\partial \mathbf{x}}\\ \frac{\partial \mathrm{r}\left(\xi 2,{\mathrm{L}}_2\right)}{\partial \mathbf{x}}\\ \frac{\partial \mathrm{r}\left(\xi 2,{\mathrm{L}}_3\right)}{\partial \mathbf{x}}\\ \frac{\partial \mathrm{r}\left(\xi 3,{\mathrm{L}}_2\right)}{\partial \mathbf{x}}\\ \frac{\partial \mathrm{r}\left(\xi 3,{\mathrm{L}}_3\right)}{\partial \mathbf{x}}\end{array}\right]$$

$$雅克比矩阵具有稀疏性,拿其中的\frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \mathbf{x}}举例$$

$$\begin{gathered} \frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \mathbf{x}}=\left[\begin{array}{ccc}\frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \xi 1}& \frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \xi 2}& \frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \xi 3}\end{array}\begin{array}{ccc}\frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial L1}& \frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial L2}& \frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial L3}\end{array}\right] \\ =\left[\begin{array}{ccc}\frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \xi 1}& \frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \xi 2}& 0\end{array}\begin{array}{ccc}0& 0& 0\end{array}\right] \\ \mathbf{\Lambda }1=\mathbf{J}1{\mathbf{\Sigma }}^{-1}\mathbf{J}1=\left[\begin{array}{c}\frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \xi 1}\\ \frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \xi 2}\\ 0\\ 0\\ 0\end{array}\right]{\mathbf{\Sigma }}^{-1}\left[\begin{array}{ccc}\frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \xi 1}& \frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \xi 2}& 0\end{array}\begin{array}{ccc}0& 0& 0\end{array}\right] \\ =\left[\begin{array}{ccc}\frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \xi 1}{\mathbf{\Sigma }}^{-1}\frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \xi 1}& \frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \xi 1}{\mathbf{\Sigma }}^{-1}\frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \xi 2}& \begin{array}{cccc}0& 0& 0& 0\end{array}\\ \frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \xi 2}{\mathbf{\Sigma }}^{-1}\frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \xi 1}& {\mathbf{\Sigma }}^{-1}\frac{\partial \mathrm{r}(\xi 1,\xi 2)}{\partial \xi 2}& \begin{array}{cccc}0& 0& 0& 0\end{array}\\ \begin{array}{c}0\\ 0\\ 0\\ 0\end{array}& \begin{array}{c}0\\ 0\\ 0\\ 0\end{array}& \begin{array}{cccc}0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\end{array}\right] \end{gathered}$$

同理：

将九个残差的信息矩阵加起来，得到最终的信息矩阵 Λ, 可视化如下：

1.2 利用边际概率移除老的变量ξ1

$${\Lambda }_{\alpha \alpha }-{\Lambda }_{\alpha \beta }{\Lambda }_{\beta \beta }^{-1}{\Lambda }_{\beta \alpha }$$

图片参考链接 https://blog.csdn.net/weixin_44456692/article/details/106891699?spm=1001.2101.3001.6650.2&utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-2-106891699-blog-105613029.t5_download_50w&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-2-106891699-blog-105613029.t5_download_50w&utm_relevant_index=3

2 BA信息矩阵计算

2.1 雅克比计算

关于重投影误差雅克比矩阵14讲上有详细的推导过程
重投影误差对相机位姿李代数的导数:

这个雅可比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系。我们保留了前面的负号，这是因为误差是由观测值减预测值定义的。它当然也可反过来，定义成“预测值减观测值”的形式。在那种情况下，只要去掉前面的负号即可。此外，如果 se(3) 的定义方式是旋转在前，平移在后，只要把这个矩阵的前 3 列与后 3 列对调即可 （代码是这种！）

代码实现: hessian_nullspace_test.cpp

//图像点(u,v)对姿态6维求偏导,旋转在前,平移在后,推导见<视觉SLAM第二版> 7.7.3 Bundle Adjustment
            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_

重投影误差对空间特征点导数:

代码实现: hessian_nullspace_test.cpp

    /// 两个优化变量分别是位姿和世界坐标系3D点(路标)  slam14讲书的原话：另一方面，除了优化位姿，我们还希望优化特征点的空间位置
    /// 因此,需要求误差e分别对 位姿6维 和 世界坐标系3D点求偏导。
    /// 推导见slam14讲：7.7.3 Bundle Adjustment
    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;
    // 这里用了链式求导法则来求e对世界坐标系3D点求偏导
    // 实际需要的是 图像点(u,v)对世界坐标系下3D点求导
    Eigen::Matrix<double,2,3> jacobian_Pj = jacobian_uv_Pc * Rcw;//重投影误差e关于空间点P的导数 

2.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;   // 非对角线

2.3 整体的代码 hessian_nullspace_test.cpp

#include <iostream>
#include <vector>
#include <random>  
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <ostream>
#include <fstream>

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;  //待优化变量总维度 diem = 120
    double fx = 1.;
    double fy = 1.;
    Eigen::MatrixXd H(diem,diem); // H = 120*120维
    H.setZero();

    std::vector<Pose> camera_pose;
    double radius = 8;
    for(int n = 0; n < poseNums; ++n ) {//0-9
        double theta = n * 2 * M_PI / ( poseNums * 4); // 1/4  :90 deg 0-19/20PI   总共旋转:1/4 圆弧
        // 绕 z轴 旋转
        Eigen::Matrix3d R;
        R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());///参考系绕其z轴(转yaw)顺时针旋转90度
        ///Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 45 度
        std::cout<<"-------------------------------"<<std::endl;
        std::cout<< "R:" <<std::endl;
        std::cout<< R <<std::endl;
        Eigen::Vector3d t = Eigen::Vector3d(radius * cos(theta) - radius, radius * sin(theta), 1 * sin(2 * theta));
        std::cout<< "t:" << std::endl;
        std::cout<< t <<std::endl;

        camera_pose.push_back(Pose(R,t));
    }

    // 随机数生成三维特征点
    std::default_random_engine generator;//
    std::vector<Eigen::Vector3d> points;
    for(int j = 0; j < featureNums; ++j)//0-19
    {
        std::uniform_real_distribution<double> xy_rand(-4, 4.0);///左闭右闭区间 uniform_real_distribution：产生均匀分布的实数
        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);//生成世界坐标系下的空间点坐标Pw
        points.push_back(Pw);

        for (int i = 0; i < poseNums; ++i) {//0-9
            Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();//世界坐标系 变换到 相机坐标系 的旋转矩阵
            Eigen::Vector3d Pc = Rcw * (Pw - camera_pose[i].twc);//世界坐标系 变换到 相机坐标系下 的位姿Pc
            //Pw= Rwc*pc + twc ==>  pc= Rcw(Pw-twc)
            //     ——     ——                       ——            ——
            //    | Rwc  twc|                     | Rcw  -Rcw*twc  |
            //Twc=| 0     1 |       Tcw=Twc ^ -1 =|  0      1      |      Rwc = Rcw ^T
            //     ——     ——                       ——            ——
            double x = Pc.x();
            double y = Pc.y();
            double z = Pc.z();
            double z_2 = z * z;
            /// 两个优化变量分别是位姿和世界坐标系3D点(路标)  slam14讲书的原话：另一方面，除了优化位姿，我们还希望优化特征点的空间位置
            /// 因此,需要求误差e分别对 位姿6维 和 世界坐标系3D点求偏导。
            /// 推导见slam14讲：7.7.3 Bundle Adjustment
            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;
            // 这里用了链式求导法则来求e对世界坐标系3D点求偏导
            // 实际需要的是 图像点(u,v)对世界坐标系下3D点求导
            Eigen::Matrix<double,2,3> jacobian_Pj = jacobian_uv_Pc * Rcw;//重投影误差e关于空间点P的导数 
            
            
            //图像点(u,v)对姿态6维求偏导,旋转在前,平移在后,推导见<视觉SLAM第二版> 7.7.3 Bundle Adjustment
            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;//位姿对角线
            /// 请补充完整作业信息矩阵块的计算
            // H.block(j*3 + 6*poseNums,j*3 + 6*poseNums,3,3) +=?????
            // H.block(i*6,j*3 + 6*poseNums, 6,3) += ???;
            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<< "saes.eigenvalues():" << std::endl;
    // std::cout << saes.eigenvalues() <<std::endl;

    std::ofstream fs;
    fs.open("H.txt",std::ios::out);
    std::cout<< "H.cols():" << std::endl;
    std::cout<< H.cols() <<std::endl;

    for (int row=0;row<H.rows();row++)
    {
        for (int col=0;col<H.cols();col++)
        {
            fs<<H(row,col)<<",";
        }
        fs<<std::endl;
    }
    fs.close();

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

2.4 H矩阵的形式

2.3 运行程序，验证零空间维度

运行结果：

svd.singularValues():
     147.51
       .
       .
       .
 0.00172459
0.000422374
3.21708e-17
2.06732e-17
1.43188e-17
7.66992e-18
6.08423e-18
6.05715e-18
3.94363e-18

结论：最后7维确实接近于0.表明零空间的维度为7

参考：

【1】The humble Gaussian distribution http://www.inference.org.uk/mackay/humble.pdf

【2】Exactly sparse extended information filters for feature-based SLAM
http://people.csail.mit.edu/mwalter/papers/walter07.pdf
【3】Observability and Fisher information matrix in nonlinear
https://hal-univ-tln.archives-ouvertes.fr/hal-01820468/document