VIO 之 视觉IMU紧耦合优化_imu视觉紧耦合

深蓝学院VIO课程代码的学习总结 ，项目地址：
github-VINS-Course

solveOdometry()

void Estimator::solveOdometry()

void Estimator::solveOdometry()
{
    if (frame_count < WINDOW_SIZE)
        return;
    if (solver_flag == NON_LINEAR)
    {
        TicToc t_tri;
        // 首先三角化  
        f_manager.triangulate(Ps, tic, ric);
        //cout << "triangulation costs : " << t_tri.toc() << endl;        
        backendOptimization();
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13

可以看到三角化更新一些点后，就进入 backendOptimization() ，在这个函数中实现紧耦合优化的核心流程。

backendOptimization()

vector2double()

void Estimator::vector2double()
这个函数将滑动窗口的状态数据赋值给二维数组。这是因为ceres使用时指定的优化参数的形式是数组，这里将数据转换为数组以便之后使用ceres优化，虽然后面没有使用ceres，但是仍然保留这种做法。
参与优化的数组：
para_Pose[i]： 7维 ，保存滑窗每一帧位姿，前3维赋值 Ps[i] ，后4维赋值Rs[i]。
para_SpeedBias[i]: 9维， 依次保存滑窗每一帧的速度Vs[i]、Bas[i]、Bgs[i]。
para_Ex_Pose[i]：7维 ，保存每一个相机的外参，单目就只有一个相机啦，
para_Feature[i][]: 保存FeatureManager的feature容器中第i个特征点的逆深度。
para_Td[0][0]：保存IMU与图像时间戳的偏移。

problemSolve()

void Estimator::problemSolve()

1、将视觉IMU外参数节点 vertexExt 添加进图。

 // 先把 外参数 节点加入图优化，这个节点在以后一直会被用到，所以我们把他放在第一个
    shared_ptr<backend::VertexPose> vertexExt(new backend::VertexPose());     // 外参数节点也是pose节点 
    {
        Eigen::VectorXd pose(7);
	// 获取外参数值
        pose << para_Ex_Pose[0][0], para_Ex_Pose[0][1], para_Ex_Pose[0][2], para_Ex_Pose[0][3], para_Ex_Pose[0][4], para_Ex_Pose[0][5], para_Ex_Pose[0][6];
        vertexExt->SetParameters(pose);    // 节点设置参数  
        // 如果不优化外参则fix
        if (!ESTIMATE_EXTRINSIC)     // 外参估计完后设置为1  
        {
            //ROS_DEBUG("fix extinsic param");
            // TODO:: set Hessian prior to zero
            vertexExt->SetFixed();      // 固定 
        }
        else{
            //ROS_DEBUG("estimate extinsic param");
        }
        problem.AddVertex(vertexExt);
        pose_dim += vertexExt->LocalDimension();
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

2、将滑窗内所有相机的pose节点 vertexCam 添加，以及将每一帧相机的速度偏置节点 vertexVB 添加 。

// 遍历滑动窗口   添加滑动窗口所有的节点 - 位姿、速度、偏置
    for (int i = 0; i < WINDOW_SIZE + 1; i++)
    {
        shared_ptr<backend::VertexPose> vertexCam(new backend::VertexPose());     
        Eigen::VectorXd pose(7);
	// 相机的Pose
        pose << para_Pose[i][0], para_Pose[i][1], para_Pose[i][2], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5], para_Pose[i][6];
        vertexCam->SetParameters(pose);
        vertexCams_vec.push_back(vertexCam);
        problem.AddVertex(vertexCam);
        pose_dim += vertexCam->LocalDimension();
        // 相机的速度、偏置节点 
        shared_ptr<backend::VertexSpeedBias> vertexVB(new backend::VertexSpeedBias());
        Eigen::VectorXd vb(9);
        vb << para_SpeedBias[i][0], para_SpeedBias[i][1], para_SpeedBias[i][2],
            para_SpeedBias[i][3], para_SpeedBias[i][4], para_SpeedBias[i][5],
            para_SpeedBias[i][6], para_SpeedBias[i][7], para_SpeedBias[i][8];
        vertexVB->SetParameters(vb);
        vertexVB_vec.push_back(vertexVB);
        problem.AddVertex(vertexVB);
        pose_dim += vertexVB->LocalDimension();
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

3、为滑窗1-11帧添加预积分约束边，该边为四元边。

// IMU     添加IMU约束边
for (int i = 0; i < WINDOW_SIZE; i++)
{
   int j = i + 1;
// 这个条件在什么时候不会满足 ？？？？？ 
   if (pre_integrations[j]->sum_dt > 10.0)
       continue;
   // IMU对节点的约束     传入预积分量   
   std::shared_ptr<backend::EdgeImu> imuEdge(new backend::EdgeImu(pre_integrations[j]));
// 有多个顶点则要创建一个容器  将顶点放入
   std::vector<std::shared_ptr<backend::Vertex>> edge_vertex;
// 为边添加顶点      改变连接第i - j个顶点      
   edge_vertex.push_back(vertexCams_vec[i]);
   edge_vertex.push_back(vertexVB_vec[i]);
   edge_vertex.push_back(vertexCams_vec[j]);
   edge_vertex.push_back(vertexVB_vec[j]);
   imuEdge->SetVertex(edge_vertex);         // 顶点存放与边的  verticies_  
   problem.AddEdge(imuEdge);         
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

4、遍历 FeatureManager 的 feature容器中所有的特征点，对于每个合法的特征点，将其起始观测帧的深度构建逆深度节点，然后遍历该特征点所有的观测，对除起始帧之外的帧构建重投影边 EdgeReprojection ，该边是一个4元边，连接 逆深度节点、相机节点i、相机节点j, 外参数节点 , 添加信息矩阵和鲁棒核函数后加入图优化。

// Visual Factor
    vector<shared_ptr<backend::VertexInverseDepth>> vertexPt_vec;
    {
        int feature_index = -1;
        // 遍历每一个特征
        for (auto &it_per_id : f_manager.feature)
        {   // 特征合法条件  
            it_per_id.used_num = it_per_id.feature_per_frame.size();
            if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
                continue;

            ++feature_index;
            // imu_i 从起始帧开始  
            int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
            Vector3d pts_i = it_per_id.feature_per_frame[0].point;                // 起始帧的归一化坐标
            // 逆深度节点  
            shared_ptr<backend::VertexInverseDepth> verterxPoint(new backend::VertexInverseDepth());
            VecX inv_d(1);
            inv_d << para_Feature[feature_index][0];        // 获取该特征点的逆深度   
            verterxPoint->SetParameters(inv_d);
            problem.AddVertex(verterxPoint);                     // 添加顶点
            vertexPt_vec.push_back(verterxPoint);

            // 遍历该特征点的所有观测,对不是该特征点起始观测帧的帧构造重投影边 
            for (auto &it_per_frame : it_per_id.feature_per_frame)
            {
                imu_j++;
                if (imu_i == imu_j)
                    continue;
                // 获得改观测的归一化坐标
                Vector3d pts_j = it_per_frame.point;
                // 创建重投影边    输入该特征点在其起始帧观测的结果以及第j帧的观测结果   
                std::shared_ptr<backend::EdgeReprojection> edge(new backend::EdgeReprojection(pts_i, pts_j));
                std::vector<std::shared_ptr<backend::Vertex>> edge_vertex;
		// 添加该边的顶点   4元边     
                edge_vertex.push_back(verterxPoint);                       // 逆深度节点
                edge_vertex.push_back(vertexCams_vec[imu_i]);  // 相机节点i
                edge_vertex.push_back(vertexCams_vec[imu_j]);  // 相机节点j 
                edge_vertex.push_back(vertexExt);                             //外参数节点 

                edge->SetVertex(edge_vertex);                                     
		// 信息矩阵    project_sqrt_info_ = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
                edge->SetInformation(project_sqrt_info_.transpose() * project_sqrt_info_); 
                // 由于相机观测可能有外点   采用鲁棒核函数        
                edge->SetLossFunction(lossfunction);
		// 添加边  
                problem.AddEdge(edge);
            }
        }
    }
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

5、处理先验， 如果 Hprior_ 非空：

将 Hprior_ 赋值给problem类的H_prior_ ;
将 bprior_赋值给problem类的b_prior_ ; 
将 errprior_赋值给problem类的err_prior_ ; 
将 Jprior_inv_赋值给problem类的Jt_prior_inv_ ; 
扩大 H_prior_ 、b_prior_的维度  + 15;
1
2
3
4
5

先验信息是关于Pose的，没有landmark的部分。

6、进行求解 problem.Solve(10)， 迭代次数10，迭代过程中需要不断更新先验残差b_prior_， 细节稍后解析。
7、优化后更新Estimator类的先验残差 bprior_、errprior_， 获取problem类的 b_prior_ 赋值给 bprior_ ，获取 problem类的err_prior_赋值给 errprior_，Hprior_ 不需要更新 因为被marg掉的节点的观测信息都被丢弃了。
8、更新状态，读取图优化节点的状态赋值给 para_Pose、para_SpeedBias、para_Feature。

// update parameter   更新滑动窗口全部状态
for (int i = 0; i < WINDOW_SIZE + 1; i++)
{  // 获取该帧的pos节点的状态  
    VecX p = vertexCams_vec[i]->Parameters();
    for (int j = 0; j < 7; ++j)
    {
        para_Pose[i][j] = p[j];
    }
    // 获取该帧的速度偏置节点的状态  
    VecX vb = vertexVB_vec[i]->Parameters();
    for (int j = 0; j < 9; ++j)
    {
        para_SpeedBias[i][j] = vb[j];
    }
}
// 遍历每一个特征
for (int i = 0; i < vertexPt_vec.size(); ++i)
{  // 获取逆深度节点的状态 
    VecX f = vertexPt_vec[i]->Parameters();
    para_Feature[i][0] = f[0];           
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

优化求解 Solve()

bool Problem::Solve(int iterations)
附 LM算法流程：

如何添加FEJ？？？？？？？
1、void Problem::SetOrdering()
(1)、求得图优化中优化变量的总维度 ordering_generic_，pose节点(包括速度偏置节点)的总维度 ordering_poses_，landmark节点的总维度ordering_landmarks_ 。
(2)、遍历 verticies_,依次设置每个节点的 ordering_id_参数，表示该节点的状态变量在其相同类别的节点状态变量(Pose节点或landmark节点)的排序，这个变量也将决定节点的jacobian在H和b矩阵中的位置。 注意，verticies_是一个有序的map，内部节点的顺序依据节点的id从小到大排列，而节点间id大小取决于节点间创建的先后顺序，越早创建则id越小，因此 verticies_中节点的顺序应该是 外参Pose节点，IMUpose节点与速度偏置节点交替？, 逆深度节点。
(3)、将所有的Pose节点放置于map idx_pose_vertices_中，将所有的landmark节点放置与
map idx_landmark_vertices_中，key为节点的id。
(4)、每个landmark节点的ordering_id_都增加ordering_poses_，使得每个landmark节点的状态变量排列在Pose节点之后。
疑问： Pose节点与速度偏置节点交替排列 ？？

2、void Problem::MakeHessian()
遍历当前添加的全部残差边，求出每条边关于节点状态的jacobian，构造出 Hessian_、b_矩阵。 构造完了后，再将之前的先验添加，如下:

 // 如果先验存在 .....
if(H_prior_.rows() > 0)
{
   MatXX H_prior_tmp = H_prior_;
   VecX b_prior_tmp = b_prior_;

/// 遍历所有 POSE 顶点，将需要fix的节点 设置相应的先验维度为 0 .但是 除了外参节点可能fix 其他的节点都没有fix 
   for (auto vertex: verticies_) {
// Pose节点且Fix     因为先验信息不包含landmark   所以只考虑Pose
       if (IsPoseVertex(vertex.second) && vertex.second->IsFixed() ) {
           int idx = vertex.second->OrderingId();
           int dim = vertex.second->LocalDimension();
           H_prior_tmp.block(idx,0, dim, H_prior_tmp.cols()).setZero();
           H_prior_tmp.block(0,idx, H_prior_tmp.rows(), dim).setZero();
           b_prior_tmp.segment(idx,dim).setZero();
//                std::cout << " fixed prior, set the Hprior and bprior part to zero, idx: "<<idx <<" dim: "<<dim<<std::endl;
       }
   }
   // Hessian_ Pose节点的部分叠加先验   目前先验保存的内容是关于0-10帧的   扩充的15维 为 0 
   Hessian_.topLeftCorner(ordering_poses_, ordering_poses_) += H_prior_tmp;
   b_.head(ordering_poses_) += b_prior_tmp;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

由于先验矩阵 扩维了15，所以维度等于ordering_poses_，所以先验直接叠加到Hessian_、b_的左上角
最终获得添加了先验信息的 Hessian_，b_ 矩阵。

3、进行迭代求解
其中：
(1)、SolveLinearSystem() 该函数求解出状态的优化增量 delta_x_，采取先边缘化landmark状态，求解出Pose 状态增量 delta_x_pp ，再反求出landmark的状态增量 delta_x_ll，最后合并获得最终的 delta_x_ 。
(2)、UpdateStates() 在求解出delta_x_后对各个节点的状态进行更新，同时随着状态的变化，先验 b_prior_ 也要变化！！！

// update prior  先验的信息矩阵虽然不变，但是随着优化将状态不断的改变，先验的残差需要跟随变化
if (err_prior_.rows() > 0) {
   // 记录当前的先验误差     如果本次迭代无效 则要通过这个变量 恢复之前的先验误差    
   b_prior_backup_ = b_prior_;
   err_prior_backup_ = err_prior_;
   //  b_prior_ = b_prior_ - H_prior_*dx   
   // H_prior_ 扩维了15   
   b_prior_ -= H_prior_ * delta_x_.head(ordering_poses_);     // update the error_prior
// b_prior_ = -J^T*error =>  error = -J^-T*b_prior_
   err_prior_ = -Jt_prior_inv_ * b_prior_.head(ordering_poses_ - 15);       // Jt_prior_inv_没扩维  所以要-15  

}
1
2
3
4
5
6
7
8
9
10
11
12

先验 b_prior_ 如下所示:

其中J为残差关于状态的jacobian, 由于残差已经被丢弃所以无法更新.
P为观测量的协方差矩阵, 也无法更新.
而rB为残差, 优化迭代时, 状态被更新, 所以这个残差也可以更新,
因此总体的更新可以写成下式子:

状态更新后，IsGoodStepInLM()判断迭代是否有效，如果有效，则重新构造 MakeHessian(),此时在新的状态点计算jacobian以及残差。如果无效，则恢复原来的状态RollbackStates()，改变阻尼系数重新计算SolveLinearSystem()，直到优化完成。

while (!stop && (iter < iterations)) {
std::cout << "iter: " << iter << " , chi= " << currentChi_ << " , Lambda= " << currentLambda_ << std::endl;
bool oneStepSuccess = false;
int false_cnt = 0;
while (!oneStepSuccess && false_cnt < 10)                             // 不断尝试 Lambda, 直到成功迭代一步
{
    // 第四步，解线性方程      获得 增量 delta_x_ 
    SolveLinearSystem();
    // 优化退出条件1： delta_x_ 很小则退出
//            if (delta_x_.squaredNorm() <= 1e-6 || false_cnt > 10)
    // TODO:: 退出条件还是有问题, 好多次误差都没变化了，还在迭代计算，应该搞一个误差不变了就中止
//            if ( false_cnt > 10)
//            {
//                stop = true;
//                break;
//            }
    // 更新状态量
    UpdateStates();
    // 判断当前步是否可行以及 LM 的 lambda 怎么更新, chi2 也计算一下
    oneStepSuccess = IsGoodStepInLM();
    // 后续处理，
    if (oneStepSuccess) {
        // 在新线性化点 构建 hessian
        MakeHessian();
        false_cnt = 0;
    } else {
        false_cnt ++;
        RollbackStates();   // 误差没下降，回滚
    }
}
iter++;
// 前后两次的误差已经不再变化
if(last_chi_ - currentChi_ < 1e-5)
{
    std::cout << "sqrt(currentChi_) <= stopThresholdLM_" << std::endl;
    stop = true;
}
last_chi_ = currentChi_;        // 记录本次总残差
}
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

double2vector()

void Estimator::double2vector()
去除求解的状态在零空间上的变化。

边缘化

边缘化有两种方式：
一、marg掉最老帧。 二、marg掉次新帧。 如下：

// 如果是marg掉最后一帧  
    if (marginalization_flag == MARGIN_OLD)
    {
        vector2double();
        MargOldFrame();
    }
    else
    {   // marg掉次新帧
        if (Hprior_.rows() > 0)
        {
            vector2double();
            MargNewFrame();
        }
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14

MargOldFrame()

void Estimator::MargOldFrame()
功能：marg掉最后一帧。
说明： 当次新帧为关键帧，我们marg掉最老帧。目的是将最老帧相关的约束，转移到其他的帧，所以首先我们需要获取最老帧的全部约束即边，也就是预积分边与重投影边，然后将这些观测构建出信息矩阵，最后marg掉最老帧节点以及最老帧相连接的landmark节点得到先验信息矩阵，这样就完成了将最老帧相关的约束转移给了滑动窗口其他帧。
流程：
0、构建一个图优化Problem对象，注意后面添加的顶点、边都是添加到这个problem里面。
backend::Problem problem(backend::Problem::ProblemType::SLAM_PROBLEM);
1、添加外参数Pose节点。
2、将滑动窗口所有帧的Pose节点与速度偏置节点添加。
3、添加第0个节点到第1个节点的IMU预积分约束边。
4、所有被第0帧观测到的特征点构建逆深度节点，第0帧与其他观测到该特征点的帧构建重投影残差边。
5、如果存在老先验矩阵，老先验矩阵也应该参与marg，即将老先验继续传递，老先验矩阵的维度只包含10帧的Pose节点以及外参节点，此时需要将老先验矩阵扩维15，这样才能和包含11帧Pose节点的信息矩阵叠加, 老先验的处理如下：

// 老先验矩阵如果存在  则也要参与marg      即将之前的先验信息继续传递  
{
   // 已经有 Prior 了     那么marg之前需要将这个先验信息矩阵添加
   if (Hprior_.rows() > 0)
   {  
       problem.SetHessianPrior(Hprior_);           // 告诉这个 problem  H_prior_ 
       problem.SetbPrior(bprior_);
       problem.SetErrPrior(errprior_);
       problem.SetJtPrior(Jprior_inv_);
 //因为  老先验信息矩阵只包含10帧的节点   而添加老先验矩阵时   当时的矩阵包含11帧的节点   所以这里要扩维15   
       problem.ExtendHessiansPriorSize(15);    
   }
   else
   {   // 第一次marg  则没有先验信息矩阵      
// 构造先验信息矩阵   维度为MargOldFrame（）中添加的全部pose节点维度   
       Hprior_ = MatXX(pose_dim, pose_dim);      
       Hprior_.setZero();
// 构造先验残差   维度为MargOldFrame（）中添加的全部pose节点维度   
       bprior_ = VecX(pose_dim);
       bprior_.setZero();
       problem.SetHessianPrior(Hprior_);     // 告诉这个 problem     设置 problem  H_prior_
       problem.SetbPrior(bprior_);                  //  设置 problem  b_prior_ 
   }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

6、 执行marg - Marginalize(), marg的节点是最老帧的Pose 以及V bias节点以及与最老帧连接的landmark节点 , 注意要叠加老先验矩阵, Marginalize()的细节在下。
7、获得marg后的先验信息，赋值给 Hprior_、bprior_、errprior_、Jprior_inv_。

MargNewFrame()

marg掉窗口次新帧，marg次新帧和marg最老帧有很大不同，它不会添加次新帧的任何的边来构建信息矩阵，，而是直接将原有的先验信息矩阵进行marg，因为，次新帧与最新帧的观测很相似，直接丢弃不会损失太多的约束。
0、构建一个图优化Problem对象，注意后面添加的顶点、边都是添加到这个problem里面。
backend::Problem problem(backend::Problem::ProblemType::SLAM_PROBLEM);
1、添加外参数Pose节点。
2、将滑动窗口所有帧的Pose节点与速度偏置节点添加。
3、处理老先验矩阵。

// 先验
{
    // 已经有 Prior 了
    if (Hprior_.rows() > 0)
    {
        problem.SetHessianPrior(Hprior_); // 告诉这个 problem
        problem.SetbPrior(bprior_);
        problem.SetErrPrior(errprior_);
        problem.SetJtPrior(Jprior_inv_);

        problem.ExtendHessiansPriorSize(15); // 但是这个 prior 还是之前的维度，需要扩展下装新的pose
    }
    else
    {
        Hprior_ = MatXX(pose_dim, pose_dim);
        Hprior_.setZero();
        bprior_ = VecX(pose_dim);
        bprior_.setZero();
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

4、 Marginalize()，marg掉窗口次新帧的Pose与速度偏置节点，这里marg只是将老先验信息矩阵添加，并对其操作。
5、获得marg后的先验信息，赋值给 Hprior_、bprior_、errprior_、Jprior_inv_。

Marginalize()

这个函数是边缘化的核心！最终求得先验H_prior_、b_prior_、err_prior_， 该函数流程如下：
1、SetOrdering() 对当前Problem中的节点状态变量进行排列，为构建信息矩阵做准备。
2、如果marg最老帧，则获取与最老帧Pose节点连接的所有边，遍历这些边构造出信息矩阵。

/// 找到第0帧的所有约束   - 连接的预积分边与重投影边   
std::vector<shared_ptr<Edge>> marg_edges = GetConnectedEdges(margVertexs[0]);

//    std::cout << "pose dim: " << pose_dim <<std::endl;
int cols = ordering_generic_;                                                          // marg的矩阵维度为当前所有pose节点与landmark节点的维度总和  
/// 构建误差 H 矩阵 H = H_marg + H_pp_prior
MatXX H_marg(MatXX::Zero(cols, cols));
VecX b_marg(VecX::Zero(cols));
int ii = 0;
// 遍历每一条边  构建H矩阵
for (auto edge: marg_edges) {
    edge->ComputeResidual();
    edge->ComputeJacobians();     
    auto jacobians = edge->Jacobians();
    auto verticies = edge->Verticies();
    ii++;
    assert(jacobians.size() == verticies.size());
// 该边的顶点 
    for (size_t i = 0; i < verticies.size(); ++i) {
        auto v_i = verticies[i];
        auto jacobian_i = jacobians[i];
        ulong index_i = v_i->OrderingId();
        ulong dim_i = v_i->LocalDimension();

        double drho;
        MatXX robustInfo(edge->Information().rows(),edge->Information().cols());
        edge->RobustInfo(drho,robustInfo);

        for (size_t j = i; j < verticies.size(); ++j) {
            auto v_j = verticies[j];
            auto jacobian_j = jacobians[j];
            ulong index_j = v_j->OrderingId();
            ulong dim_j = v_j->LocalDimension();
            // Jt W J
            MatXX hessian = jacobian_i.transpose() * robustInfo * jacobian_j;

            assert(hessian.rows() == v_i->LocalDimension() && hessian.cols() == v_j->LocalDimension());
            // 所有的信息矩阵叠加起来
            H_marg.block(index_i, index_j, dim_i, dim_j) += hessian;
            if (j != i) {
                // 对称的下三角
                H_marg.block(index_j, index_i, dim_j, dim_i) += hessian.transpose();
            }
        }
        b_marg.segment(index_i, dim_i) -= drho * jacobian_i.transpose() * edge->Information() * edge->Residual();
    }

}
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

3、如果marg最老帧，则先marg掉landmark节点，图模型添加的landmark节点全部是与最老帧pose节点相连接的。

 /// marg landmark
  int reserve_size =ordering_poses_;         // marg掉landmark后   剩下pose节点  
if (ordering_landmarks_ > 0) {
   int marg_size = ordering_landmarks_;
/*
*   | Hpp    Hpm   |
*   | Hmp   Hmm  |
*/
   MatXX Hmm = H_marg.block(reserve_size, reserve_size, marg_size, marg_size);
   MatXX Hpm = H_marg.block(0, reserve_size, reserve_size, marg_size);
   MatXX Hmp = H_marg.block(reserve_size, 0, marg_size, reserve_size);
   VecX bpp = b_marg.segment(0, reserve_size);
   VecX bmm = b_marg.segment(reserve_size, marg_size);

   // Hmm 是对角线矩阵，它的求逆可以直接为对角线块分别求逆，如果是逆深度，对角线块为1维的，则直接为对角线的倒数，这里可以加速
   MatXX Hmm_inv(MatXX::Zero(marg_size, marg_size));
   // TODO:: use openMP
   for (auto iter: idx_landmark_vertices_) {
       int idx = iter.second->OrderingId() - reserve_size;
 //      int size = iter.second->LocalDimension();    // 1    
 //      Hmm_inv.block(idx, idx, size, size) = Hmm.block(idx, idx, size, size).inverse();
Hmm_inv(idx,idx) = 1/Hmm(idx,idx);   // 直接倒数 
   }
   // Hpp - Hpm*Hmm^-1*Hmp
   MatXX tempH = Hpm * Hmm_inv;
   MatXX Hpp = H_marg.block(0, 0, reserve_size, reserve_size) - tempH * Hmp;
   bpp = bpp - tempH * bmm;
   H_marg = Hpp;
   b_marg = bpp;
}
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

4、老先验信息矩阵的处理，和老先验信息矩阵叠加，老先验不能丢，叠加后参与marg变成新的先验继续用。

// 添加老先验信息矩阵
if(H_prior_.rows() > 0)
{
    H_marg += H_prior_;          // H_prior_ 之前扩维了15  现在和H_marg维度相同了
    b_marg += b_prior_;
}
1
2
3
4
5
6

5、marg掉需要marg的frame以及V，bias节点 。 首先将需要marg的节点移动到最后(更新H矩阵与b 矩阵) ，接着同时marg掉这些节点，得到 H_prior_、b_prior_ 矩阵。
6、对 H_prior_ 进行特征分解 H_prior_ = USUt ，求得先验残差的jacobian矩阵 , 然后求得先验残差 err_prior_。

// 对  H_prior_ 进行特征分解           H_prior_ = U*S*Ut      
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(H_prior_);
    Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0));     // S   获取特征值   小于eps的赋值0  
    Eigen::VectorXd S_inv = Eigen::VectorXd(
            (saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0));     // S^-1
    // H_prior_ = U*S*Ut = JtJ => J = sqrt(S)*Ut
    Eigen::VectorXd S_sqrt = S.cwiseSqrt();    // sqrt(S)
    Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();    // sqrt(S^-1)
    Jt_prior_inv_ = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose();      // sqrt(S^-1)*Ut
    // 先验残差   因为   b_prior_ = -Jt*e => e = -Jt^-1*b_prior_ = -sqrt(S^-1)*Ut*b_prior_
    err_prior_ = -Jt_prior_inv_ * b_prior_;   // 在计算阻尼系数时使用    
    // J =  sqrt(S)*Ut   
    MatXX J = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();
    H_prior_ = J.transpose() * J;      // JtJ  = U*S*UT 
    MatXX tmp_h = MatXX( (H_prior_.array().abs() > 1e-9).select(H_prior_.array(),0) );     // H_prior_小于1e-9的元素直接赋0  
    H_prior_ = tmp_h;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

原理如下：

疑问：
1、err_prior_ 有什么用？？？？？

滑动窗口

Vins中滑动窗口的函数是void Estimator::slideWindow()，边缘化后，我们获取了marg后的节点的先验信息矩阵，接着为了滑动窗口接受新的帧，我们需要将marg掉的帧的信息移除。移除的方式和marg的策略有关。

重点问题

1、进行优化时构造最小二乘信息矩阵H ，先验信息矩阵是如何融合的？ 每一次优化需要迭代很多步，每迭代一次H矩阵就要重新构建一次，这时先验信息矩阵如何处理？
答：
在void Estimator::problemSolve()中，如果有先验信息矩阵，则将其放置到problem类中，并扩维15

// 先验
{
   // 已经有 Prior 了    在首次marg之前为空
   if (Hprior_.rows() > 0)
   {
       // 外参数先验设置为 0. TODO:: 这个应该放到 solver 里去弄
       //            Hprior_.block(0,0,6,Hprior_.cols()).setZero();
       //            Hprior_.block(0,0,Hprior_.rows(),6).setZero();
       problem.SetHessianPrior(Hprior_);       // 告诉这个 problem    设置为 H_prior_ 
       problem.SetbPrior(bprior_);                    // b_prior_
       problem.SetErrPrior(errprior_);               // err_prior_    
       problem.SetJtPrior(Jprior_inv_);            // Jt_prior_inv_
// 扩大 H_prior_ 、b_prior_的维度  + 15   先验信息矩阵由于marg掉了一帧   只包含10帧的维度   需要扩展下装新的pose
       problem.ExtendHessiansPriorSize(15);   
   }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

然后进入 problem.Solve(), 在MakeHessian()中添加先验信息矩阵，将先验信息矩阵添加到左上角

// 如果先验存在 .....
    if(H_prior_.rows() > 0)
    {
        MatXX H_prior_tmp = H_prior_;
        VecX b_prior_tmp = b_prior_;

        /// 遍历所有 POSE 顶点，将需要fix的节点 设置相应的先验维度为 0 .    但是 除了外参节点可能fix 其他的节点都没有fix 
        for (auto vertex: verticies_) {
	    // Pose节点且Fix  因为先验信息不包含landmark   所以只考虑Pose
            if (IsPoseVertex(vertex.second) && vertex.second->IsFixed() ) {
                int idx = vertex.second->OrderingId();
                int dim = vertex.second->LocalDimension();
                H_prior_tmp.block(idx,0, dim, H_prior_tmp.cols()).setZero();
                H_prior_tmp.block(0,idx, H_prior_tmp.rows(), dim).setZero();
                b_prior_tmp.segment(idx,dim).setZero();
//                std::cout << " fixed prior, set the Hprior and bprior part to zero, idx: "<<idx <<" dim: "<<dim<<std::endl;
            }
        }
        //  Hessian_ Pose节点的部分叠加先验   目前先验保存的内容是关于0-10帧的   扩充了为 0的15维  
        // 注意这里 Hessian_ 包含的维度有Pose与landmark   而先验只有Pose的  所以取左上部分
        Hessian_.topLeftCorner(ordering_poses_, ordering_poses_) += H_prior_tmp;
        b_.head(ordering_poses_) += b_prior_tmp;
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

每次迭代时，要随着状态的更新同步更新先验残差 b_prior_ ，但是H_prior 不用变化。

2、每次marg的时候，对于旧先验信息矩阵是怎样处理的？
新的先验信息矩阵先marg掉landmark的部分，然后老先验信息矩阵扩维之后与新先验信息矩阵叠加。

3、非线性优化时，如果要使某一节点状态变量保持不变应该要怎么做，为什么？

4、什么是FEJ，如何在VINS中添加FEJ？