(2)边缘化 Marginalization 和 FEJ
一、理论部分
目标:
• 控制滑窗大小,从而控制运算量
• 让滑窗中的帧有足够的视差量
• 尽可能使得矩阵稀疏,减小运算量
基于高斯牛顿的非线性优化理论可知, Hδx = b 可写成如下形式:
值得说明的是,上式中 δxa 、δxb 是希望 marg的部分和希望保留的部分。另外, VINS 中的边缘化与 G2O 计算过程中的边缘化意义不大相同(虽然处理方法一致):G2O 中对路标点设置边缘化(pPoint->setMarginalized(true))是为了在计算求解过程中,先消去路标点变量,实现先求解相机位姿,然后再利用求解出来的相机位姿反过来计算路标点的过程,目的是为了加速求解,并非真的将路标点给边缘化掉; 而VINS 中则真正需要边缘化掉滑动窗口中的最老帧或者次新帧,目的是希望不再计算这一帧的位姿或者与其相关的路标点,但是希望保留该帧对窗口内其他帧的约束关系。
假设上式中的δxa 是我们要 marg 的变量, 比如一个相机的 pose, 因此我们更关心如何只去求解我们希望保留的变量δxb ,而不再求解 δxa , 同时我们又不能直接将 δxa 和与其相关的路标点等信息直接删除掉, 因为这样会减少约束, 丢失信息。因此,采用如下 Schur 进行消元:
用Schur补公式,来计算最老帧对滑窗内其他帧的约束关系
注意上式是转化而来,我们并未丢失任何约束,因此不会丢失信息。值得说明的, 上面的公式即为要保留变量 δxb 的先验信息。我们求出 δxb 后，带入上面的公式便可以把 δxa 求出来，换句话说，就是在知道 δxb 的前提下求解 δxa （先验）。
一个边缘化的例子：
下面我们对上图进行详细说明,图(12-a)表示原始的 H 矩阵,注意这里的左上角为路标点相关部分,而右下角是 pose 相关部分。
二、代码部分
margin部分，如果倒数第二帧是关键帧：
这一部分，只边缘化，不求解，求解留给 下一轮优化 的第一部分来进行。
- 把之前存的残差部分加进来。很明显，现在要marg了，要构造新的先验H矩阵，那么要把之前的老先验的遗留信息加进来；
- 把与当前要margin掉的帧所有相关的残差项都加进来，IMU,vision；
- preMarginalize-> 调用Evaluate计算所有ResidualBlock的残差，parameter_block_data parameter_block_idx parameter_block_size是marinalization中存参数块的容器(unordered_map),key都是addr,分别对应这些参数的data，在稀疏矩阵A中的index(要被margin掉的参数会被移到前面)；
- A中的大小，Marginalize->多线程构造Hx=b的结构，H是边缘化后的结果，First Esitimate Jacobian,在X0处线性化；
- margin结束，调整参数块在下一次window中对应的位置（往前移一格），注意这里是指针，后面slideWindow中会赋新值；
这里只是提前占座（知乎上有人问：https://www.zhihu.com/question/63754583/answer/259699612），沈老师这里边缘化有两个策略，如果在sliding window中第二近的frame是关键帧则丢弃sliding window中最老的帧、否则丢弃该帧。无论丢弃哪一帧，都需要边缘化。
1、边缘化掉老 (old) 的帧
[1] 首先，把上一轮残存的信息加进来：
if (marginalization_flag == MARGIN_OLD)//丢弃老的一帧。
{
MarginalizationInfo *marginalization_info = new MarginalizationInfo();
vector2double();//这里是否需要转换?
//先验误差会一直保存，而不是只使用一次。如果上一次边缘化的信息存在。
//要边缘化的参数块是 para_Pose[0] para_SpeedBias[0] 以及 para_Feature[feature_index](滑窗内的第feature_index个点的逆深度)
if (last_marginalization_info)//上一次边缘化的先验信息
{
vector<int> drop_set;//上次边缘化后与Pose[0],SpeedBias[0]有关的参数块
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);
//这一段代码用drop_set把最老帧的先验信息干掉了
}[2] 把这次要marg的IMU信息加进来：
if (pre_integrations[1]->sum_dt < 10.0)
{
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});
marginalization_info->addResidualBlockInfo(residual_block_info);
}【3】 把这次要marg的视觉信息加进来：
{
//C、然后，把这次要marg的视觉信息加进来，只添加起始帧是旧帧且观测次数大于2的Features，
// 此处加上了大括号,有作用吗,暂时已经删除，
int feature_index = -1;
//遍历滑窗内所有的Features
for (auto &it_per_id : f_manager.feature)
{
//该特征点被观测到的次数
it_per_id.used_num = it_per_id.feature_per_frame.size();
//Feature的观测次数不小于2次，且起始帧不属于最后两帧
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;
++feature_index;
//采用这种feature_index的方法必须确保每次从featuresInWindow中取出的features都一致!@todo 如何确保?
// 另外一种思路是记录feature ID的索引号
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
// 只选择被边缘化的帧的Features
//这里不对啊，论文中讲的是只要Features被旧帧观测了就要被marg掉的啊
//这里是合适的，因为只要。。。
if (imu_i != 0)
continue;//特征点的起始帧必须是首帧
//得到该Feature在起始帧下的归一化坐标
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());
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});
marginalization_info->addResidualBlockInfo(residual_block_info);
}//vins把第0帧看到的特征点全都扔了
}
}
}
【4】 将三个ResidualBlockInfo中的参数块综合到marginalization_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());
//将滑窗里关键帧位姿移位，为什么是向右移位了呢？//注意是指针的赋值para_Pose[i]指向para_Pose[i-1]的内存块
//这里是保存了所有状态量的信息，为什么没有保存逆深度的状态量呢 //因为与第一帧有关的所有feature全部被marg了
std::unordered_map<long, double *> addr_shift;
for (int i = 1; i <= WINDOW_SIZE; i++)
{
//1-->10的地址存放的是0-->9对应的参数块
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
}
//此处实际上是将地址信息及其对应的参数存入到map中,后面可以使用地址值来进行索引
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;
}2、边缘化掉（new）的帧
如果倒数第二帧不是关键帧
- 保留该帧的IMU测量，margin该帧的visual；
- pre-marginalize；3.marginalize；4.滑动窗口移动（去掉倒数第二个）
则把这帧的视觉测量舍弃掉（边缘化）而保留IMU测量值在滑动窗口中;其他步骤和上一步骤相同
else//则把这帧的视觉测量舍弃掉（边缘化）而保留IMU测量值在滑动窗口中。（其他步骤和上一步骤相同
{
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++)
{
//寻找倒数第二帧的位姿
ROS_ASSERT(last_marginalization_parameter_blocks[i] != para_SpeedBias[WINDOW_SIZE - 1]);
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);
marginalization_info->addResidualBlockInfo(residual_block_info);
}
//marg倒数第二帧时没有IMU先验和视觉先验
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());
}optimization()结束
5701
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
