0. 简介
作为主流框架的前端中常用的方法，划窗优化是很常见迭代策略。因为随着SLAM系统的运行，状态变量规模不断增大，如果使用滑动窗口，只对窗口内的相关变量进行优化便可以大大减小计算量。这些之前在我的博客中有提到，但是之前作者没有深入的去了解这些，只是对边缘化中的舒尔补策略进行了简略的介绍。
而作为划窗优化，我们除了创建滑动窗口的存储空间外，我们还要通过边缘化的方法保留滑窗外的状态，我们可以不去优化划窗外的参数，但也不能直接丢掉，这样会破坏原有的约束关系，损失约束信息。采用边缘化的技巧，将约束信息转化为待优化变量的先验分布，实际上是一个从联合分布中获得变量子集概率分布的问题。
1. 划窗优化的操作步骤
以vins系统为代表，其中和边缘化优化相关的代码存放在下列文件中：
1）vins_estimator/src/factor/marginalization_factor.cpp：边缘化的具体实现
2）vins_estimator/src/estimator.cpp（部分）：
函数optimization负责利用边缘化残差构建优化模型
函数slideWindow负责维护滑动窗口
解释了这三个问题，我们其实就完成了理解VINS-Mono里这个问题的理论部分，下面看代码实践部分。
2. 边缘化的具体实现
2.1 特征提取
这里我们以地图定位滑动窗口模型为例，来介绍特征提取匹配模型的建立。
地图匹配因子的残差函数和雅克比矩阵推导如下图：
其中residual.block对应了上图中的residual变量，而由于上图中表明一个地图定位表示一个定位点，所以r_p等于预测向量与观测相量的误差，r_q通过计算预测旋转量R和观测相量$R_{obs}$的差值，并通过BCH将李群转为李代数，从而得到一个一阶近似的李代数。
在上面的求导过程中第一步为求导：{r_q}的{R}偏导数格式为{[R_ln(exp(\phi)_{R_obs}) - ln(R_{R_obs}) ]/{\phi}}，如果R为转置，则为：{[ln(exp(\phi)_R_{R_obs}) - ln(R_{R_obs}) ]/{\phi}}
第二步和第三步则是通过扩充$R_obs$和其转置然后置换成R*{R_obs}形式，并根据李代数的伴随性质，得到式子3的结果（利用ln乘法变加法的原则），并BCH近似得到式子4.
//
// TODO: get square root of information matrix:
//
// Cholesky 分解 : http://eigen.tuxfamily.org/dox/classEigen_1_1LLT.html
Eigen::Matrix<double, 6, 6> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 6, 6>>(
I_
).matrixL().transpose();
//
// TODO: compute residual:
//
Eigen::Map<Eigen::Matrix<double, 6 ,1>> residual(residuals);
residual.block(INDEX_P, 0 , 3 , 1 ) = pos - pos_prior ;
residual.block(INDEX_R,0 , 3 , 1 ) = (ori*ori_prior.inverse()).log();
//
// TODO: compute jacobians: 一元边
//
if ( jacobians ) {
if ( jacobians[0] ) {
// implement jacobian computing:
Eigen::Map<Eigen::Matrix<double, 6, 15, Eigen::RowMajor>> jacobian_prior(jacobians[0] );
jacobian_prior.setZero();
jacobian_prior.block<3, 3>(INDEX_P, INDEX_P) = Eigen::Matrix3d::Identity();
jacobian_prior.block<3, 3>(INDEX_R, INDEX_R) = JacobianRInv(
residual.block(INDEX_R, 0, 3, 1)) * ori_prior.matrix();
jacobian_prior = sqrt_info * jacobian_prior ;
}
}
2.2 ceres优化调用（本质是第三点内容，这里先提一下，方便连贯）
ceres优化求解，保存优化参数块结果，并压入到边缘化中
// 只有第1个预积分和待边缘化参数块相连
{
if (pre_integrations[1]->sum_dt < 10.0)
{
// 跟构建ceres约束问题一样，这里也需要得到残差和雅克比
IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
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个参数块是需要被边缘化的
marginalization_info->addResidualBlockInfo(residual_block_info);
}
}
2.3 添加残差块信息
上面展示的是每一块待优化变量的优化变量，而其实我们存在有很多残差块，vins中就有一部分是用来添加残差块相关信息（优化变量，待边缘化变量）
void MarginalizationInfo::addResidualBlockInfo(ResidualBlockInfo *residual_block_info)
{
factors.emplace_back(residual_block_info); // 残差块收集起来
std::vector<double *> ¶meter_blocks = residual_block_info->parameter_blocks; // 这个是和该约束相关的参数块
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];
// 这里是个map，避免重复添加
parameter_block_size[reinterpret_cast<long>(addr)] = size; // 地址->global size
}
// 待边缘化的参数块
for (int i = 0; i < static_cast<int>(residual_block_info->drop_set.size()); i++)
{
double *addr = parameter_blocks[residual_block_info->drop_set[i]];
// 先准备好待边缘化的参数块的map
parameter_block_idx[reinterpret_cast<long>(addr)] = 0;
}
}
2.4 preMarginalize：更新参数块
将各个残差块计算残差和雅克比，同时备份所有相关的参数块内容，并更新parameter_block_data
void MarginalizationInfo::preMarginalize()
{
for (auto it : factors)
{
it->Evaluate(); // 调用这个接口计算各个残差块的残差和雅克比矩阵
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]; // 参数块大小
// 把各个参数块都备份起来，使用map避免重复参数块，之所以备份，是为了后面的状态保留
if (parameter_block_data.find(addr) == parameter_block_data.end())
{
double *data = new double[size];
// 深拷贝
memcpy(data, it->parameter_blocks[i], sizeof(double) * size);
parameter_block_data[addr] = data; // 地址->参数块实际内容的地址
}
}
}
}
2.4. marginalize：执行边缘化
多线程构造先验项舒尔补AX=b的结构，边缘化处理，并将结果转换成残差和雅克比的形式
void MarginalizationInfo::marginalize()
{
int pos = 0;
// parameter_block_idx key是各个待边缘化参数块地址 value预设都是0
for (auto &it : parameter_block_idx)
{
it.second = pos; // 这就是在所有参数中排序的idx，待边缘化的排在前面
pos += localSize(parameter_block_size[it.first]); // 因为要进行求导，因此大小时local size，具体一点就是使用李代数
}
m = pos; // 总共待边缘化的参数块总大小（不是个数）
// 其他参数块
for (const auto &it : parameter_block_size)
{
if (parameter_block_idx.find(it.first) == parameter_block_idx.end())
{
parameter_block_idx[it.first] = pos; // 这样每个参数块的大小都能被正确找到
pos += localSize(it.second);
}
}
n = pos - m; // 其他参数块的总大小
//ROS_DEBUG("marginalization, pos: %d, m: %d, n: %d, size: %d", pos, m, n, (int)parameter_block_idx.size());
TicToc t_summing;
Eigen::MatrixXd A(pos, pos); // Ax = b预设大小
Eigen::VectorXd b(pos);
A.setZero();
b.setZero();
/*
for (auto it : factors)
{
for (int i = 0; i < static_cast<int>(it->parameter_blocks.size()); i++)
{
int idx_i = parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[i])];
int size_i = localSize(parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[i])]);
Eigen::MatrixXd jacobian_i = it->jacobians[i].leftCols(size_i);
for (int j = i; j < static_cast<int>(it->parameter_blocks.size()); j++)
{
int idx_j = parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[j])];
int size_j = localSize(parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[j])]);
Eigen::MatrixXd jacobian_j = it->jacobians[j].leftCols(size_j);
if (i == j)
A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;
else
{
A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;
A.block(idx_j, idx_i, size_j, size_i) = A.block(idx_i, idx_j, size_i, size_j).transpose();
}
}
b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals;
}
}
ROS_INFO("summing up costs %f ms", t_summing.toc());
*/
//multi thread
// 往A矩阵和b矩阵中填东西，利用多线程加速
TicToc t_thread_summing;
pthread_t tids[NUM_THREADS];
ThreadsStruct threadsstruct[NUM_THREADS];
int i = 0;
for (auto it : factors)
{
threadsstruct[i].sub_factors.push_back(it); // 每个线程均匀分配任务
i++;
i = i % NUM_THREADS;
}
// 每个线程构造一个A矩阵和b矩阵，最后大家加起来
for (int i = 0; i < NUM_THREADS; i++)
{
TicToc zero_matrix;
// 所以A矩阵和b矩阵大小一样，预设都是0
threadsstruct[i].A = Eigen::MatrixXd::Zero(pos,pos);
threadsstruct[i].b = Eigen::VectorXd::Zero(pos);
// 多线程访问会带来冲突，因此每个线程备份一下要查询的map
threadsstruct[i].parameter_block_size = parameter_block_size; // 大小
threadsstruct[i].parameter_block_idx = parameter_block_idx; // 索引
// 产生若干线程
int ret = pthread_create( &tids[i], NULL, ThreadsConstructA ,(void*)&(threadsstruct[i]));
if (ret != 0)
{
ROS_WARN("pthread_create error");
ROS_BREAK();
}
}
for( int i = NUM_THREADS - 1; i >= 0; i--)
{
// 等待各个线程完成各自的任务
pthread_join( tids[i], NULL );
// 把各个子模块拼起来，就是最终的Hx = g的矩阵了
A += threadsstruct[i].A;
b += threadsstruct[i].b;
}
//ROS_DEBUG("thread summing up costs %f ms", t_thread_summing.toc());
//ROS_INFO("A diff %f , b diff %f ", (A - tmp_A).sum(), (b - tmp_b).sum());
// Amm矩阵的构建是为了保证其正定性，从而确保迭代的稳定性
Eigen::MatrixXd Amm = 0.5 * (A.block(0, 0, m, m) + A.block(0, 0, m, m).transpose());
// 检查矩阵是否是self-adjoint。SelfAdjointEigenSolver 可以通过EigenSolver或者ComplexEigenSolver方法适用于一般矩阵
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm); // 特征值分解
//ROS_ASSERT_MSG(saes.eigenvalues().minCoeff() >= -1e-4, "min eigenvalue %f", saes.eigenvalues().minCoeff());
// 一个逆矩阵的特征值是原矩阵的倒数，特征向量相同 select类似c++中 ? :运算符
// 利用特征值取逆来构造其逆矩阵
Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd((saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() * saes.eigenvectors().transpose();
//printf("error1: %f\n", (Amm * Amm_inv - Eigen::MatrixXd::Identity(m, m)).sum());
// 线性化残差 和 雅克比
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;
// 这个地方根据Ax = b => JT*J = - JT * e
// 对A做特征值分解 A = V * S * VT,其中Ｓ是特征值构成的对角矩阵
// 所以J = S^(1/2) * VT , 这样JT * J = (S^(1/2) * VT)T * S^(1/2) * VT = V * S^(1/2)T * S^(1/2) * VT = V * S * VT(对角矩阵的转置等于其本身)
// e = -(JT)-1 * b = - (S^-(1/2) * V^-1) * b = - (S^-(1/2) * VT) * b
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(A);
// 对A矩阵取逆
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(); // 这个求得就是 S^(1/2)，不过这里是向量还不是矩阵
Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();
// 边缘化为了实现对剩下参数块的约束，为了便于一起优化，就抽象成了残差和雅克比的形式，这样也形成了一种残差约束
linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();
linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b;
//std::cout << A << std::endl
// << std::endl;
//std::cout << linearized_jacobians << std::endl;
//printf("error2: %f %f\n", (linearized_jacobians.transpose() * linearized_jacobians - A).sum(),
// (linearized_jacobians.transpose() * linearized_residuals - b).sum());
}
3. optimization：负责边缘化残差的使用
这个函数位于vins_estimator/src/estimator.cpp文件中，而且它负责整个系统所有的优化工作，边缘化残差的使用只是它功能的一部分。
3.1 定义待优化的参数块
Step 1 定义待优化的参数块，类似g2o的顶点
// 参数块 1： 滑窗中位姿包括位置和姿态，共11帧
for (int i = 0; i < WINDOW_SIZE + 1; i++)
{
// 由于姿态不满足正常的加法，也就是李群上没有加法，因此需要自己定义他的加法
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
}
// 参数块 2： 相机imu间的外参
for (int i = 0; i < NUM_OF_CAM; i++)
{
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
if (!ESTIMATE_EXTRINSIC)
{
ROS_DEBUG("fix extinsic param");
// 如果不需要优化外参就设置为fix
problem.SetParameterBlockConstant(para_Ex_Pose[i]);
}
else
ROS_DEBUG("estimate extinsic param");
}
3.2 添加边缘化残差
Step 2 通过残差约束来添加残差块，类似g2o的边
//上一次的边缘化结果作为这一次的先验
if (last_marginalization_info)
{
// construct new marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
problem.AddResidualBlock(marginalization_factor, NULL,
last_marginalization_parameter_blocks);
}
// 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]);
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
}
..........
3.3 ceres 优化求解并得到结果
// Step 3 ceres优化求解
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
//options.num_threads = 2;
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); // ceres优化求解
//cout << summary.BriefReport() << endl;
ROS_DEBUG("Iterations : %d", static_cast<int>(summary.iterations.size()));
ROS_DEBUG("solver costs: %f", t_solver.toc());
// 把优化后double -> eigen
double2vector();
3.4 边缘化策略
边缘化分两种情况，每种情况有各自的流程
a. 如果次新帧是关键帧，则将边缘化最老帧，及其看到的路标点和IMU数据，转化为先验。具体流程为：
1）将上一次先验残差项传递给marginalization_info
2）将第0帧和第1帧间的IMU因子IMUFactor(pre_integrations[1])，添加到marginalization_info中
3）将第一次观测为第0帧的所有路标点对应的视觉观测，添加到marginalization_info中
4）计算每个残差，对应的Jacobian，并将各参数块拷贝到统一的内存（parameter_block_data）中
5）多线程构造先验项舒尔补AX=b的结构，在X0处线性化计算Jacobian和残差
6）调整参数块在下一次窗口中对应的位置（往前移一格），注意这里是指针，后面slideWindow中会赋新值，这里只是提前占座
b. 如果次新帧不是关键帧，此时具体流程为：
1）保留次新帧的IMU测量，丢弃该帧的视觉测量，将上一次先验残差项传递给marginalization_info
2）premargin：计算每个残差，对应的Jacobian，并更新parameter_block_data
3）marginalize：构造先验项舒尔补AX=b的结构，计算Jacobian和残差
4）调整参数块在下一次窗口中对应的位置（去掉次新帧）
最后都是传入preMarginalize和Marginalize实现优化。
// Step 4 边缘化
// 科普一下舒尔补
TicToc t_whole_marginalization;
if (marginalization_flag == MARGIN_OLD)
{
// 一个用来边缘化操作的对象
MarginalizationInfo *marginalization_info = new MarginalizationInfo();
// 这里类似手写高斯牛顿，因此也需要都转成double数组
vector2double();
// 关于边缘化有几点注意的地方
// 1、找到需要边缘化的参数块，这里是地图点，第0帧位姿，第0帧速度零偏
// 2、找到构造高斯牛顿下降时跟这些待边缘化相关的参数块有关的残差约束，那就是预积分约束，重投影约束，以及上一次边缘化约束
// 3、这些约束连接的参数块中，不需要被边缘化的参数块，就是被提供先验约束的部分，也就是滑窗中剩下的位姿和速度零偏
// 上一次的边缘化结果
if (last_marginalization_info)
{
vector<int> drop_set;
// last_marginalization_parameter_blocks是上一次边缘化对哪些当前参数块有约束
for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
{
// 涉及到的待边缘化的上一次边缘化留下来的当前参数块只有位姿和速度零偏
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);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
last_marginalization_parameter_blocks,
drop_set);
marginalization_info->addResidualBlockInfo(residual_block_info);
}
// 只有第1个预积分和待边缘化参数块相连
{
if (pre_integrations[1]->sum_dt < 10.0)
{
// 跟构建ceres约束问题一样，这里也需要得到残差和雅克比
IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
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个参数块是需要被边缘化的
marginalization_info->addResidualBlockInfo(residual_block_info);
}
}
// 遍历视觉重投影的约束
{
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;
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
// 只找能被第0帧看到的特征点
if (imu_i != 0)
continue;
Vector3d pts_i = it_per_id.feature_per_frame[0].point;
// 遍历看到这个特征点的所有KF，通过这个特征点，建立和第0帧的约束
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());
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f_td, loss_function,
vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]},
vector<int>{0, 3});
marginalization_info->addResidualBlockInfo(residual_block_info);
}
else
{
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}); // 这里第0帧和地图点被margin
marginalization_info->addResidualBlockInfo(residual_block_info);
}
}
}
}
// 所有的残差块都收集好了
TicToc t_pre_margin;
// 进行预处理
marginalization_info->preMarginalize();
ROS_DEBUG("pre marginalization %f ms", t_pre_margin.toc());
TicToc t_margin;
// 边缘化操作
marginalization_info->marginalize();
ROS_DEBUG("marginalization %f ms", t_margin.toc());
// 即将滑窗，因此记录新地址对应的老地址
std::unordered_map<long, double *> addr_shift;
for (int i = 1; i <= WINDOW_SIZE; i++)
{
// 位姿和速度都要滑窗移动
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
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实际上就是addr_shift的索引的集合及搬进去的新地址
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; // 代表该次边缘化对某些参数块形成约束，这些参数块在滑窗之后的地址
}
else // 边缘化倒数第二帧
{
// 要求有上一次边缘化的结果同时，即将被margin掉的在上一次边缘化后的约束中
// 预积分结果合并，因此只有位姿margin掉
if (last_marginalization_info &&
std::count(std::begin(last_marginalization_parameter_blocks), std::end(last_marginalization_parameter_blocks), para_Pose[WINDOW_SIZE - 1]))
{
MarginalizationInfo *marginalization_info = new MarginalizationInfo();
vector2double();
if (last_marginalization_info)
{
vector<int> drop_set;
for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
{
// 速度零偏只会margin第1个，不可能出现倒数第二个
ROS_ASSERT(last_marginalization_parameter_blocks[i] != para_SpeedBias[WINDOW_SIZE - 1]);
// 这种case只会margin掉倒数第二个位姿
if (last_marginalization_parameter_blocks[i] == para_Pose[WINDOW_SIZE - 1])
drop_set.push_back(i);
}
// construct new marginlization_factor
// 这里只会更新一下margin factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
last_marginalization_parameter_blocks,
drop_set);
marginalization_info->addResidualBlockInfo(residual_block_info);
}
// 这里的操作如出一辙
TicToc t_pre_margin;
ROS_DEBUG("begin marginalization");
marginalization_info->preMarginalize();
ROS_DEBUG("end pre marginalization, %f ms", t_pre_margin.toc());
TicToc t_margin;
ROS_DEBUG("begin marginalization");
marginalization_info->marginalize();
ROS_DEBUG("end marginalization, %f ms", t_margin.toc());
std::unordered_map<long, double *> addr_shift;
for (int i = 0; i <= WINDOW_SIZE; i++)
{
if (i == WINDOW_SIZE - 1)
continue;
else if (i == WINDOW_SIZE) // 滑窗，最新帧成为次新帧
{
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
}
else // 其他不变
{
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i];
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i];
}
}
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];
}
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;
}
}
ROS_DEBUG("whole marginalization costs: %f", t_whole_marginalization.toc());
ROS_DEBUG("whole time for ceres: %f", t_whole.toc());
4. slideWindow：实现滑窗的更新
如果次新帧是关键帧，则边缘化最老帧，将其看到的特征点和IMU数据转化为先验信息，如果次新帧不是关键帧，则舍弃视觉测量而保留IMU测量值，从而保证IMU预积分的连贯性。
// 滑动窗口
void Estimator::slideWindow()
{
TicToc t_margin;
// 根据边缘化种类的不同，进行滑窗的方式也不同
if (marginalization_flag == MARGIN_OLD)
{
double t_0 = Headers[0].stamp.toSec();
back_R0 = Rs[0];
back_P0 = Ps[0];
// 必须是填满了滑窗才可以
if (frame_count == WINDOW_SIZE)
{
// 一帧一帧交换过去
for (int i = 0; i < WINDOW_SIZE; i++)
{
Rs[i].swap(Rs[i + 1]);
std::swap(pre_integrations[i], pre_integrations[i + 1]);
dt_buf[i].swap(dt_buf[i + 1]);
linear_acceleration_buf[i].swap(linear_acceleration_buf[i + 1]);
angular_velocity_buf[i].swap(angular_velocity_buf[i + 1]);
Headers[i] = Headers[i + 1];
Ps[i].swap(Ps[i + 1]);
Vs[i].swap(Vs[i + 1]);
Bas[i].swap(Bas[i + 1]);
Bgs[i].swap(Bgs[i + 1]);
}
// 最后一帧的状态量赋上当前值，最为初始值
Headers[WINDOW_SIZE] = Headers[WINDOW_SIZE - 1];
Ps[WINDOW_SIZE] = Ps[WINDOW_SIZE - 1];
Vs[WINDOW_SIZE] = Vs[WINDOW_SIZE - 1];
Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1];
Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1];
Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1];
// 预积分量就得置零
delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};
// buffer清空，等待新的数据来填
dt_buf[WINDOW_SIZE].clear();
linear_acceleration_buf[WINDOW_SIZE].clear();
angular_velocity_buf[WINDOW_SIZE].clear();
// 清空all_image_frame最老帧之前的状态
if (true || solver_flag == INITIAL)
{
// 预积分量是堆上的空间，因此需要手动释放
map<double, ImageFrame>::iterator it_0;
it_0 = all_image_frame.find(t_0);
delete it_0->second.pre_integration;
it_0->second.pre_integration = nullptr;
for (map<double, ImageFrame>::iterator it = all_image_frame.begin(); it != it_0; ++it)
{
if (it->second.pre_integration)
delete it->second.pre_integration;
it->second.pre_integration = NULL;
}
// 释放完空间之后再erase
all_image_frame.erase(all_image_frame.begin(), it_0);
all_image_frame.erase(t_0);
}
slideWindowOld();
}
}
else
{
if (frame_count == WINDOW_SIZE)
{
// 将最后两个预积分观测合并成一个
for (unsigned int i = 0; i < dt_buf[frame_count].size(); i++)
{
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];
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];
// reset最新预积分量
delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};
// clear相关buffer
dt_buf[WINDOW_SIZE].clear();
linear_acceleration_buf[WINDOW_SIZE].clear();
angular_velocity_buf[WINDOW_SIZE].clear();
slideWindowNew();
}
}
}
5. 参考链接
https://zhuanlan.zhihu.com/p/85021338
https://blog.csdn.net/weixin_41281151/article/details/123611615
https://blog.csdn.net/hltt3838/article/details/109455065
评论（0）
您还未登录，请登录后发表或查看评论