Vins-mono 源码笔记 (6) 紧耦合优化

优化主流程

视觉惯性紧耦合优化的主要实现在 void Estimator::optimization() 中，其中主要采用ceres完成优化，ceres的使用总结见 slam中ceres的常见用法总结
Vins Mono的视觉惯性紧耦合优化执行的是Motion-only BA,也就是只对相机Pose进行优化，而不优化特征点的逆深度。
下面是该函数的流程：
1、添加优化的参数，即图优化的节点，由一下几个部分构成：
(1)、滑动窗口所有帧的Pose节点。

for (int i = 0; i < WINDOW_SIZE + 1; i++)
    {
        // 位姿pose的参数化  定义pose的增量加法...
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);     // pose Ps、Rs转变  6自由度 7个参数   
        problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);                   // Vs、Bas、Bgs
    } 

(2)、相机的外参数节点。

// 相机的外参
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();   // R  T  
        problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
        if (!ESTIMATE_EXTRINSIC)
        {  
            ROS_DEBUG("fix extinsic param");
            problem.SetParameterBlockConstant(para_Ex_Pose[i]);    // 通过这个将参数固定  不进行优化   
        }
        else
            ROS_DEBUG("estimate extinsic param");
    }

(3)、时间戳

// 时间戳       
if (ESTIMATE_TD)
{
    problem.AddParameterBlock(para_Td[0], 1);
    //problem.SetParameterBlockConstant(para_Td[0]);
}

2、如果存在先验信息，则添加先验信息约束，该模块稍后解析。

   // 如果有先验信息   添加边缘化残差，丢弃帧的约束 MarginalizationFactor
   if (last_marginalization_info)    // 初始化为 nullptr 
   {
       // construct new marginlization_factor
       MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
       problem.AddResidualBlock(marginalization_factor, NULL,
                                last_marginalization_parameter_blocks);
   }

3、添加观测约束：
(1)、IMU的预积分约束

// 添加各个相机的IMU残差   
    for (int i = 0; i < WINDOW_SIZE; i++)
    {
        int j = i + 1;
        if (pre_integrations[j]->sum_dt > 10.0)
            continue;
        // 预积分约束
        IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
        // 添加残差   连接 i-j的pose与速度偏置节点  
        problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
    }

(2)、添加视觉重投影误差

// 添加视觉的重投影误差  
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;

    int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
    
    Vector3d pts_i = it_per_id.feature_per_frame[0].point;

    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;
        // 是否需要估计时间戳对齐    
        if (ESTIMATE_TD)
        {
                ProjectionTdFactor *f_td = new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,
                                                                 it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,
                                                                 it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
                problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);
                /*
                double **para = new double *[5];
                para[0] = para_Pose[imu_i];
                para[1] = para_Pose[imu_j];
                para[2] = para_Ex_Pose[0];
                para[3] = para_Feature[feature_index];
                para[4] = para_Td[0];
                f_td->check(para);
                */
        }
        else     // 直接添加普通的视觉重投影误差  
        {
            ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
            problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);
        }
        f_m_cnt++;
    }
}

这里区分了是否进行时间戳对齐，如果对齐 则采用 ProjectionTdFactor 类来完成残差的构建。

4、回环的残差，暂时先放着不看。
5、对上述构建的优化问题进行求解。

// 优化设置   
ceres::Solver::Options options;
// 求解方式    
options.linear_solver_type = ceres::DENSE_SCHUR;
// options.num_threads = 2    优化方法     DOGLEG   
options.trust_region_strategy_type = ceres::DOGLEG;
// 迭代数量  
options.max_num_iterations = NUM_ITERATIONS;
// options.use_explicit_schur_complement = true;
// options.minimizer_progress_to_stdout = true;
// options.use_nonmonotonic_steps = true;
if (marginalization_flag == MARGIN_OLD)
    options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;
else
    options.max_solver_time_in_seconds = SOLVER_TIME;
TicToc t_solver;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
//cout << summary.BriefReport() << endl;
ROS_DEBUG("Iterations : %d", static_cast<int>(summary.iterations.size()));      
ROS_DEBUG("solver costs: %f", t_solver.toc());

6、消除相机位姿在零空间的变化，同时将优化的结果保存在 该函数稍后单独解析 ！！！

double2vector()

7、优化完成后，由于之后滑动窗口会剔除掉一帧，那么则是对需要剔除的帧进行边缘化，边缘化的目的是为了获取先验信息矩阵，即被剔除的帧对其他帧的约束。
同样分两种情况：(1)、marg最老帧 (2)、marg次新帧
8、slideWindow() 滑动窗口剔除一帧后，对保存的数据进行更新。

边缘化

边缘化过程中，有三个主要的数据结构- MarginalizationFactor、ResidualBlockInfo、MarginalizationInfo。
整个大致的套路都是： 1、构造出Factor ——> 2、 用Factor构造出 ResidualBlockInfo ——> 3、将ResidualBlockInfo通过addResidualBlockInfo()函数添加到MarginalizationInfo类中。

那么为啥要这样设计呢？
首先，ceres定义的每个Factor都可以理解成定义了一种残差以及jacobian的计算方式，它是 ceres::CostFunction 的子类，比如 IMUFactor 定义的是IMU预积分残差，它内部重写的Evaluate()定义了该残差jacobian的计算方式。
而ResidualBlockInfo 则可以理解为利用残差构造的一条边，它的定义为：

// 残差信息块  
struct ResidualBlockInfo
{
    ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector<double *> _parameter_blocks, std::vector<int> _drop_set)
        : cost_function(_cost_function), loss_function(_loss_function), parameter_blocks(_parameter_blocks), drop_set(_drop_set) {}
    // 求该残差的jacobian    
    void Evaluate();
    ceres::CostFunction *cost_function;
    ceres::LossFunction *loss_function;
    // 该边/残差的优化参数   
    std::vector<double *> parameter_blocks;
    // 待marg的优化变量id   
    std::vector<int> drop_set;
    double **raw_jacobians;
    std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobians;
    // 残差   IMU  15×1  视觉  2×1  
    Eigen::VectorXd residuals;
    int localSize(int size)
    {
        return size == 7 ? 6 : size;
    }
};

用一个Factor, 鲁棒核函数 LossFunction，以及该Factor(边/残差)的优化参数parameter_blocks，以及需要marg的参数drop_set 构造出 ResidualBlockInfo 对象 。ResidualBlockInfo类中 定义了函数 Evaluate() , 该 Evaluate() 函数内部则会调用 CostFunction 也就是Factor的 Evaluate()去计算jacobian。

另一个数据结构就是 MarginalizationInfo 类 ，它的定义如下：
MarginalizationInfo类的定义为：

class MarginalizationInfo
{
  public:
    ~MarginalizationInfo();
    int localSize(int size) const;
    int globalSize(int size) const;
    // 添加残差信息  
    void addResidualBlockInfo(ResidualBlockInfo *residual_block_info);
    // marg相关的两个主要函数
    void preMarginalize();     // 求 jacobian
    void marginalize();        // 求解jacobian后  构建A、b矩阵  

    std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift);
    // 所有的残差  类似与图优化中的边  
    std::vector<ResidualBlockInfo *> factors;
    // m 表示 marg掉的总维度     n 表示保留的总维度 
    int m, n;
    // 优化变量参数的地址 + 参数的变量个数  
    std::unordered_map<long, int> parameter_block_size; //global size
    int sum_block_size;
    // 参数 地址 + 状态参数的位置 id  
    std::unordered_map<long, int> parameter_block_idx;  //local size
    // 所有优化变量内存地址 + 数据     preMarginalize()设置  
    std::unordered_map<long, double *> parameter_block_data;
    // 边缘化后   保留的各个优化变量的维度   
    std::vector<int> keep_block_size;     //global size
    // 各个优化变量的id   
    std::vector<int> keep_block_idx;  //local size
    // 
    std::vector<double *> keep_block_data;
    // 最后边缘化后的H矩阵  反求Jacobian
    Eigen::MatrixXd linearized_jacobians;
    // 最后边缘化后反求先验残差  
    Eigen::VectorXd linearized_residuals;
    const double eps = 1e-8;
};

ResidualBlockInfo构造好后，通过 MarginalizationInfo 类的 addResidualBlockInfo()函数添加到 MarginalizationInfo 类对象中，函数如下：

// 给图优化添加一个残差   
void MarginalizationInfo::addResidualBlockInfo(ResidualBlockInfo *residual_block_info)
{   
    // factors  保存所有 ResidualBlockInfo   即将残差添加到图优化中        
    factors.emplace_back(residual_block_info);
    // 提取残差相关的参数数组指针  
    std::vector<double *> &parameter_blocks = residual_block_info->parameter_blocks;
    // 每个参数的变量个数     cost_function 即Factor
    std::vector<int> parameter_block_sizes = residual_block_info->cost_function->parameter_block_sizes();
    // 遍历该残差的全部优化参数
    for (int i = 0; i < static_cast<int>(residual_block_info->parameter_blocks.size()); i++)
    {   // 记录该参数的地址
        double *addr = parameter_blocks[i];    
        // 记录该参数的变量个数  
        int size = parameter_block_sizes[i];
        parameter_block_size[reinterpret_cast<long>(addr)] = size;    // 参数块地址以及变量个数  保存到hash map  
    }
    
    for (int i = 0; i < static_cast<int>(residual_block_info->drop_set.size()); i++)
    {   // 需要marg的状态的地址 
        double *addr = parameter_blocks[residual_block_info->drop_set[i]];
        // 需要marg的状态放置与parameter_block_idx
        parameter_block_idx[reinterpret_cast<long>(addr)] = 0;     // 需要marg的设置为0   
    }
}

这个函数的要点是：
1、将残差添加到 factors 容器中。
2、再将每个残差的优化参数的内存地址以及维度size保存在 parameter_block_size map中 .
3、接着将每个残差需要marg的变量也就是drop_set中的变量，添加到 parameter_block_idx，并且先设置id=0。

梳理了出现的数据结构后，再看 Marginalization 的代码就好理解了，下面是过程：

首先边缘化分为边缘化最老帧与边缘化次新帧，先分析边缘化最老帧 ;

边缘化最老帧

(1)、首先处理老先验信息

// 如果之前存在先验信息矩阵  则在对当前图优化问题边缘化前，要将老先验信息矩阵添加 
if (last_marginalization_info)  //  last_marginalization_info 在该函数的后面被赋值  
{   
    vector<int> drop_set;
    // 检查要marg的状态 位于之前的哪个位置   
    for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
    {   // 首先看要marg的最老帧的状态 位于哪个位置   将位置记录在 drop_set  
        if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||
            last_marginalization_parameter_blocks[i] == para_SpeedBias[0])
            drop_set.push_back(i);
    }
    // 用老先验信息矩阵  construct new marginlization_factor
    MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
    // 构建残差块    即图优化中的边    相当于  problem.AddResidualBlock
    ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
                                                                   last_marginalization_parameter_blocks,
                                                                   drop_set);
    // 添加边 
    marginalization_info->addResidualBlockInfo(residual_block_info);
}

还是那个套路,
先创建Factor,这里用老 MarginalizationInfo 来构建 MarginalizationFactor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);

在该构造函数中，将 MarginalizationFactor的 parameter_block_sizes_ 设置为 last_marginalization_info 的 keep_block_size容器，
以及将MarginalizationFactor的 num_residuals_ 设置为 last_marginalization_info的保留参数维数 n 。

然后用 marginalization_factor 构造 残差块ResidualBlockInfo，这个没什么好说的。

最后将残差块residual_block_info 通过 addResidualBlockInfo()添加到 marginalization_info 中，这个函数参考前面的分析。

(2)、处理0-1帧的预积分观测
首先构建IMU Factor - IMUFactor

用 IMUFactor 构建残差

  ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(imu_factor, NULL,
  vector<double *>{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]},
  vector<int>{0, 1});

优化参数为第0帧与第1帧的P、V、Q， marg掉第0帧的P、V、Q。
然后 残差添加到 marginalization_info 。

(3)、将最老帧看到的特征点构建重投影误差,
首先用第0帧的观测与第j帧的观测构造重投影Factor -
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
然后构建残差 -

ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,
vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]},
vector<int>{0, 3});

marg掉第0帧Pose,以及所有看到的特征点的逆深度。

(4)、preMarginalize() 遍历所有添加的残差 ，然后求jacobian。
(5)、marginalize() 执行边缘化。
(6)、由于执行下一次优化时的marg时，会通过 vector2double()将优化后的状态放入 para_Pose、para_SpeedBias等数组中，而由于 slideWindow()实际上改变了marg后的保留状态在滑动窗口的位置，所以这里提前计算出，marg后保留状态在滑动窗口 para_Pose、para_SpeedBias 等数组中的位置。

// hash_map     
std::unordered_map<long, double *> addr_shift;    
// 窗口滑动后 ，当前状态保存的位置会前移一位           
for (int i = 1; i <= WINDOW_SIZE; i++)
{   // 第i帧的内存地址 关联 第i-1帧的内存地址 
    addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];   // para_Pose[i - 1] 的值为 首地址 可以理解为double [SIZE_POSE]的数组名  
    addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
}
for (int i = 0; i < NUM_OF_CAM; i++)
    addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
if (ESTIMATE_TD)
{
    addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];
}
// 返回的 parameter_blocks 即 下一次marg时，保留的变量 位于的 para_Pose、 para_SpeedBias...等数组的内存地址
// 遍历 parameter_block_idx
vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);

if (last_marginalization_info)
    delete last_marginalization_info;
last_marginalization_info = marginalization_info;          // 将当前先验信息矩阵  保存为 老先验  
last_marginalization_parameter_blocks = parameter_blocks;  // 保存地址    

边缘化次新帧

当次新帧非关键帧时，需要marg掉，此时的次新帧的特征点的观测与周围的关键帧很相似，因此可以直接丢弃(IMU的预积分要传递给下一帧)，所以这里marg不需要添加任何观测的残差，仅仅只把老的先验信息残差添加进来进行marg。
但是下面这里添加旧先验信息的部分，ROS_ASSERT(last_marginalization_parameter_blocks[i] != para_SpeedBias[WINDOW_SIZE - 1])表示不能包含 para_SpeedBias[WINDOW_SIZE - 1] ？但是 para_SpeedBias[WINDOW_SIZE - 1] 应该存在的吧？？ 还有 if (last_marginalization_parameter_blocks[i] == para_Pose[WINDOW_SIZE - 1]) 为什么只有 para_Pose[WINDOW_SIZE - 1] 而不包含 para_SpeedBias[WINDOW_SIZE - 1] ，应该速度偏置也需要marg吧？？ 为啥不处理 para_SpeedBias[WINDOW_SIZE - 1] ???
还望知道的朋友告知！！

if (last_marginalization_info)
{
    vector<int> drop_set;
    // 
    for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
    {
        // 不能包括有 次新帧的速度偏置状态  为啥？？？？？？？？
        ROS_ASSERT(last_marginalization_parameter_blocks[i] != para_SpeedBias[WINDOW_SIZE - 1]);
        // 次新帧的Pose 设置为 marg     速度偏置 不 marg ？？？？？？？？  
        if (last_marginalization_parameter_blocks[i] == para_Pose[WINDOW_SIZE - 1])
            drop_set.push_back(i);
    }
    // construct new marginlization_factor
    MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
    ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
                                                                   last_marginalization_parameter_blocks,
                                                                   drop_set);
    // 添加到Factor             设置 parameter_block_size       添加要marg 的到 parameter_block_idx                                                   
    marginalization_info->addResidualBlockInfo(residual_block_info);
}

补充

1、marg时添加老的先验信息的原因。
进行新的边缘化时，需要将旧的先验约束一并加入，让这些旧的先验约束继续约束保留帧，否则不加先验约束的边缘化会导致系统尺度的缺失，尤其是系统在进行退化运动时(如无人机的悬停和恒速运动)。一般来说只有两个轴向的加速度不为0的时候，才能保证尺度可观，而退化运动对于无人机或者机器人来说是不可避免的。所以在系统处于退化运动的时候，要加入先验信息保证尺度的客观性。
参考： VINS-Mono代码分析与总结(完整版)

preMarginalize()

当所有的残差都添加完毕后，执行 preMarginalize()函数，该函数要点：
1、对每个添加到 MarginalizationInfo 的残差 求jacobian，也就是构建Hi = Ji^T* Wi * Ji 的 Ji 。
2、将每个残差的状态变量拷贝到 parameter_block_data。

// marg的准备  
void MarginalizationInfo::preMarginalize()
{
    // factors 在 addResidualBlockInfo 函数中添加  保存所有的 residual_block_info 
    // factors 就相当于图优化中的边   遍历每一条边
    for (auto it : factors)
    {   // 求解该边的jacobian 
        it->Evaluate();
        // 将状态保存到 parameter_block_data 
        // 获得该残差的各优化参数维度
        std::vector<int> block_sizes = it->cost_function->parameter_block_sizes();
        
        for (int i = 0; i < static_cast<int>(block_sizes.size()); i++)
        {   // 先获得该参数的地址  
            long addr = reinterpret_cast<long>(it->parameter_blocks[i]);
            // 获得该参数的维度    用与复制数据是计算大小 
            int size = block_sizes[i];    
            // 如果没有在 parameter_block_data 中   则添加  
            if (parameter_block_data.find(addr) == parameter_block_data.end())
            {
                double *data = new double[size];
                memcpy(data, it->parameter_blocks[i], sizeof(double) * size);     // 将数据拷贝到 data 中  
                parameter_block_data[addr] = data;      // 保存   
            }
        }
    }
}

然后就是 Marginalization 的正式部分 ——
void MarginalizationInfo::marginalize()

marginalize()

函数整体的流程如下：
1、设置各个状态的位置 ，放置与 parameter_block_idx map中 ，parameter_block_idx 的pair的第一个元素为状态的内存地址，第二个元素为状态的位置。 parameter_block_size 是如何 构建的？- 在 addResidualBlockInfo()函数中更新。需要注意的是，需要marg的状态的位置全部设置在最前面，这样构造的H矩阵中，需要marg的部分就在左上角。

int pos = 0;
    // 遍历需要marg的状态     要marg的状态放置与左上角    这里首先设置的是需要marg的变量 
    for (auto &it : parameter_block_idx)
    {
        it.second = pos;     // 设置 该变量的id  即位置    
        pos += localSize(parameter_block_size[it.first]);          // 通过 状态的地址 查找 该状态的 维度    localSize() 使Pose 由 7 变为 6   
    }
    // m 为 marg掉的维度  
    m = pos;
    // 然后 设置保留的状态的位置  
    for (const auto &it : parameter_block_size)
    {   // 将未填加的状态添加到 parameter_block_idx   
        if (parameter_block_idx.find(it.first) == parameter_block_idx.end())
        {
            parameter_block_idx[it.first] = pos;
            pos += localSize(it.second);
        }
    }

    n = pos - m;

2、多线程构造A与b矩阵。(Ax = b)
先初始化A，b矩阵-

TicToc t_summing;
Eigen::MatrixXd A(pos, pos);    // Pos即总状态维度   
Eigen::VectorXd b(pos);
A.setZero();
b.setZero();

创建四个线程 ，将所有的残差(factor)均分到threadsstruct数组的元素中，然后 每个线程负责一个threadsstruct的 ThreadsStruct。

//multi thread    4个线程   
TicToc t_thread_summing;
pthread_t tids[NUM_THREADS];     // 4个线程
ThreadsStruct threadsstruct[NUM_THREADS];      // 每个元素表示 该线程需要处理的残差   
int i = 0;
// 遍历每一个残差  
for (auto it : factors)
{   // 将每个残差分配到 threadsstruct  
   threadsstruct[i].sub_factors.push_back(it);
   i++;
   i = i % NUM_THREADS;      // 循环   
}

多线程计算 A，b矩阵，这里线程的主函数是 ThreadsConstructA()，计算完毕将结果更新到A，b。

// 遍历  threadsstruct  的每个元素     启动线程计算  
for (int i = 0; i < NUM_THREADS; i++)
{
   TicToc zero_matrix;
   threadsstruct[i].A = Eigen::MatrixXd::Zero(pos,pos);
   threadsstruct[i].b = Eigen::VectorXd::Zero(pos);
   threadsstruct[i].parameter_block_size = parameter_block_size;
   threadsstruct[i].parameter_block_idx = parameter_block_idx;
   // 启动线程  ThreadsConstructA  对  threadsstruct[i] 进行构造  
   int ret = pthread_create( &tids[i], NULL, ThreadsConstructA ,(void*)&(threadsstruct[i]));
   if (ret != 0)
   {
       ROS_WARN("pthread_create error");
       ROS_BREAK();
   }
}

// 更新 矩阵 A b   
for( int i = NUM_THREADS - 1; i >= 0; i--)  
{
    pthread_join( tids[i], NULL ); 
    A += threadsstruct[i].A;
    b += threadsstruct[i].b;
}

接着执行Marginalization, 就是那几个熟悉的公式，不多说：

//TODO      需要marg的部分    
Eigen::MatrixXd Amm = 0.5 * (A.block(0, 0, m, m) + A.block(0, 0, m, m).transpose()); // 对称化  
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm);    // 求解特征值分解   

//ROS_ASSERT_MSG(saes.eigenvalues().minCoeff() >= -1e-4, "min eigenvalue %f", saes.eigenvalues().minCoeff());
//   PtAP = w  =>   A = PwPt  =>   A-1 = P w-1 Pt   
Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd((saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() * saes.eigenvectors().transpose();
//printf("error1: %fn", (Amm * Amm_inv - Eigen::MatrixXd::Identity(m, m)).sum());
// marg 的过程    marg掉 m   
Eigen::VectorXd bmm = b.segment(0, m);
Eigen::MatrixXd Amr = A.block(0, m, m, n);
Eigen::MatrixXd Arm = A.block(m, 0, n, m);
Eigen::MatrixXd Arr = A.block(m, m, n, n);
Eigen::VectorXd brr = b.segment(m, n);
A = Arr - Arm * Amm_inv * Amr;
b = brr - Arm * Amm_inv * bmm;

然后 将A，b 结果反推 先验jacobian 与 先验残差。

// 将marg后的结果 A，b  转换为 先验jacobian与先验残差   
// A = PSPt
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(A);
// 对S中特征值小于 eps 的直接归0 处理  
Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0));
Eigen::VectorXd S_inv = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0));

Eigen::VectorXd S_sqrt = S.cwiseSqrt();
Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();
// A = PSPt = (sqrt(S)*Pt)^t * (sqrt(S)*Pt)    =>  A = JtJ =>   J = (sqrt(S)*Pt)   
linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();
// b = Jte =>   e = (Jt)^-1*b = sqrt(S)^-1 * P^t * b  
linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b;

参考下面公式：

ThreadsConstructA()

构造A矩阵线程的主函数， 各个残差在 preMarginalize()求解出来jacobian矩阵后，在这个函数中合成A矩阵，原理是 A += JtJ, b += Jt * r
这个函数 只对 分配到同一个线程的残差进行处理。
合并的顺序是：

补充：
注意这里构建H矩阵的加速策略，将所有用来构建H矩阵的残差分成4组，每一组分配一个线程进行计算。

滑动窗口

void Estimator::slideWindow() (extimator.cpp中)
调用机制：
1、processImage()函数中，首先通过 addFeatureCheckParallax()判断marg的策略 ，
MARGIN_OLD：marg掉最老的一帧。
MARGIN_SECOND_NEW: marg掉次新帧。
2、初始化时，若初始化失败或者外参标定失败，需要调用 slideWindow() 将滑动窗口的一帧去除。
3、初始化成功后，以及之后进入NON_LINEAR模式，每一帧求解完solveOdometry()后都需要slideWindow()。
可以看到 slideWindow()是非常重要的！！
运行流程如下：

其中：
1、如果是 MARGIN_OLD 即marg掉最老的一帧

void FeatureManager::removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P) ( feature_manager.cpp )
作用： 对feature容器进行更新。
因为最老帧被marg掉了：
遍历feature中所有特征点
1、如果该特征点的起始帧不是最老帧，则该特征点的起始帧start_frame要减一(前移)。
2、如果该特征点的起始帧为最老帧，则删除该特征点 feature_per_frame 最老帧的信息。
3、删除信息之后如果该特征点在滑动窗口中出现的个数小于2了，则feature直接删除该特征点，否则，更新该特征点的estimated_depth，之前estimated_depth等于特征点在start_frame也就是最老帧坐标系下的深度，现在要转换到新的start_frame下的深度。

2、如果是 MARGIN_SECOND_NEW 即marg掉次新帧
因为要将次新帧marg掉，那么当前最新帧就作为下一帧的次新帧，因此就需要将当前最新帧与次新帧的数据合并，即将当前帧的预积分传递到次新帧预积分中，当前帧的imu数据合并到次新帧.

for (unsigned int i = 0; i < dt_buf[frame_count].size(); i++)
{  // 获得时间  IMU测量值    记录了 从 frame_count-1 - frame_count帧之间的IMU数据 
    double tmp_dt = dt_buf[frame_count][i];
    Vector3d tmp_linear_acceleration = linear_acceleration_buf[frame_count][i];
    Vector3d tmp_angular_velocity = angular_velocity_buf[frame_count][i];
    // 继续计算预积分     添加到   frame_count - 2 - frame_count - 1之间的预积分中   
    pre_integrations[frame_count - 1]->push_back(tmp_dt, tmp_linear_acceleration, tmp_angular_velocity);
    
    dt_buf[frame_count - 1].push_back(tmp_dt);
    linear_acceleration_buf[frame_count - 1].push_back(tmp_linear_acceleration);
    angular_velocity_buf[frame_count - 1].push_back(tmp_angular_velocity);
}

下面数据 次新帧数据用最新帧的代替;

// 次新帧的数据用最新帧代替    
Headers[frame_count - 1] = Headers[frame_count];
Ps[frame_count - 1] = Ps[frame_count];
Vs[frame_count - 1] = Vs[frame_count];
Rs[frame_count - 1] = Rs[frame_count];
Bas[frame_count - 1] = Bas[frame_count];
Bgs[frame_count - 1] = Bgs[frame_count];

然后比较重要的是,用最新imu测量值构建新的预积分;

 // 这里重新构造最新帧的预积分！！！！！！
delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};

最后

以上就是雪白唇彩为你收集整理的Vins-mono 源码笔记 (6) 紧耦合优化的全部内容，希望文章能够帮你解决Vins-mono 源码笔记 (6) 紧耦合优化所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。