VINS-Mono+Fusion源码解析系列（二十）：边缘化约束
发布时间
阅读量：
阅读量
1. 舒尔补
1.1 舒尔补的实现
每当滑动窗口接收一个新的关键帧时（即），由于滑动窗口大小固定导致必须舍弃一个旧的关键帧（否则无法维持窗口内关键帧的数量）。如果直接采用硬性删除的方式去除边缘化的帧会导致丢失与其相关的关联信息。因此必须采用舒尔补的方法，在确保保留该关键帧与其剩余键之间的关系的前提下进行删除操作。
在滑动窗口内优化求解增量方程的过程中，在线更新相关参数，并将其结果进行处理以满足精度要求；基于待边缘化的状态量，在上述公式中进行特定划分如下所示：
其中，在此划分下，
\delta x_a代表需要被边缘化的状态变量，
而\delta x_b则表示保留下来的状态变量；
相应的矩阵元素H和向量b也被相应地划分为块状结构以适应这种分割方式。
通过舒尔补进行简化如下：
\left[ \begin{array}{cc} I & 0 \\ -\Lambda_b^T\Lambda_a^{-1} & 1 \end{array} \right]\left[ \begin{array}{cc} \Lambda_a & \Lambda_b \\ \Lambda_b^T &\Lambda_c\end{array} \right]\left[ \begin{array}{cc} \delta x_a \\ \delta x_b \end{array} \right] = \left[ \begin{array}{cc} I & 0 \\ -\Lambda_b^T\Lambda_a^{-1} & 1 \end{array} \right]\left[ \begin{array}{cc} g_a \\ g_b \end{array} right]
其结果为：
$\left[ begin{ array}{cc}
Λ_a & Λ_b \
0 & Λ_c - Λ_bTΛ_a{-1}Λ_b
end{ array}
ight]
left[
begin{ array}{c}
delta x_a \
delta x_b
end{ array}
ight]
left[
begin{ array}{c}
g_a \
g_b - Λ_bTΛ_a{-1}g_a
end{ array}
ight]
进一步计算得到：
(Λ_c - Λ_bTΛ_a{-1}Λ_b)δx_b = g_b - ΛbTΛa{-1}g a
通过施加舒尔补进行边缘化处理后得到的增量方程仍呈现Hδx = b的形式，
其中H可分解为J^ TJ，
而b可分解为-J e
这种分解后的结果中，
J与 e$即为经过边缘化处理后的约束条件
1.2 舒尔补后H矩阵的变化
（1）图优化建模
假设有这样一个最小二乘系统，并且其相应的图模型如图所示：
\xi=argmin_{\xi}\frac{1}{2}\sum_i{\vert| r_i \vert|^2}_{\sum_i}
- 节点：代表顶点，在优化过程中需要估算的参数。
- 边：代表顶点间建立的残差关系。包括单变量边（仅连接一个节点）、双变量边以及多变量边等类型。
该最小二乘问题对应的高斯-牛顿优化方法为：
J^T\Sigma^{-1}J\delta{\xi} = -J^T\Sigma^{-1}r
其中雅可比矩阵J的形式如下：
进而可以将该高斯-牛顿求解公式重新表述为：∑从i=1到5的J_i转置乘以Σ_i逆矩阵再乘以J_iδξ等于负号∑从i=1到5的J转置乘以Σ_i逆矩阵再乘以r
由于每个残差只和某几个状态量有关，因此，雅可比矩阵求导时，无关项的雅可比为0。比如对于残差r_{13}而言，通过前面的图模型可知，它只和状态量1与3有关，与其他状态量无关，因此对应雅可比为：
J_2=\frac{\partial r_{13}}{\partial \xi}=\left[\begin{array}{cc} \frac{\partial r_{13}}{\partial \xi_1} & 0 & \frac{\partial r_{13}}{\partial \xi_3} & 0 & 0 & 0 \end{array} \right]
对应的信息矩阵如下：
\Lambda_2=J_2^T\Sigma_2^{-1}J=\left[\begin{array}{cc} (\frac{\partial r_{13}}{\partial \xi_1})^T\Sigma_2^{-1}\frac{\partial r_{13}}{\partial \xi_1} & 0 & (\frac{\partial r_{13}}{\partial \xi_1})^T\Sigma_2^{-1}\frac{\partial r_{13}}{\partial \xi_3} & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \\ (\frac{\partial r_{13}}{\partial \xi_3})^T\Sigma_2^{-1}\frac{\partial r_{13}}{\partial \xi_1} & 0 & (\frac{\partial r_{13}}{\partial \xi_3})^T\Sigma_2^{-1}\frac{\partial r_{13}}{\partial \xi_3} & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \end{array} \right]
同理，\Lambda_1,\Lambda_3,\Lambda_4,\Lambda_5也是稀疏的。将五个残差的信息矩阵加起来，得到最终的信息矩阵\Lambda，可视化如下：
（3）边缘化后的信息矩阵H
使用舒尔补的方式，将变量\xi_1边缘化掉，其信息矩阵的变化过程如下：
基于变量\xi_1作为分界点，在信息矩阵\Lambda上实施分区处理
1.3 fill-in
边缘化会导致fill-in现象出现，并使原本的稀疏矩阵转变为稠密矩阵，在求解过程中不利于利用稀疏矩阵的特性来加速计算过程。由于在进行边缘化操作时不可避免地会引入稠密矩阵这一问题，在VINS-Mono算法中选择了对所有地图点实施边缘化处理的方式，并因此使得H矩阵的空间主要由地图点所占据而减少了整体的空间维度
2. VINS-Mono中边缘化的实现
（1）大致流程
-
倒数第二帧是关键帧，将最老的一帧相关信息边缘化掉，主要执行如下操作：
- 构造边缘化类对象marginalization_info，用于管理边缘化操作
- 将边缘化相关变量转换成double类型（vector2double）
- 若上一次进行了边缘化操作而产生了边缘化约束 ，则在上一次边缘化操作的参数块last_marginalization_parameter_blocks中，将第0帧的位姿和速度bias作为待边缘化的状态量（上一次边缘化对第0帧的位姿和速度bias有约束），构建残差块添加到marginalization_info中
- 由于相邻的第1帧与第0帧之间存在预积分约束 ，在边缘化最老帧（第0帧）时二者之间的预积分会产生影响。因此，在第1帧与第0帧之间的预积分中将第0帧的位姿和速度bias作为待边缘化的状态量，构建残差块添加到marginalization_info中
- 滑窗中可能存在其他帧与第0帧之间形成共视点，二者会产生视觉重投影约束 ，在边缘化最老帧（第0帧）时会对二者的视觉重投影约束产生影响。因此遍历特征点，在与第0帧之间产生的视觉重投影参数块中将第0帧位姿以及所有的3D地图点作为待边缘化的状态量，构建残差块添加到marginalization_info中
- 依次执行边缘化预处理marginalization_info->preMarginalize和边缘化操作marginalization_info->marginalize
- 更新滑窗状态量：位姿、速度零偏；外参和时间延时不变(不需要在滑窗中维护)
- 用当前边缘化信息更新上一次边缘化信息，从当前边缘化信息中取出的参数块更新上一次边缘化信息的参数块约束，为下一次优化的边缘化操作做准备
-
若倒数第二帧属于非关键帧，则需将其边缘化。
在满足上一次边缘化的条件并受到其约束的前提下： -
创建一个名为marginalization_info的对象来执行边缘化操作
-
将相关的变量转换为double类型（vector2double）
-
在last_marginalization_parameter_blocks参数块中添加由上一次边缘化施加的状态量（仅限于倒数第二帧的位姿受约束），并将其残差块附加至marginalization_info对象中
-
依次执行预处理步骤preMarginalize和边际化操作marginalize
-
更新滑窗中的状态量：位姿与速度零偏；外参及时间延时保持不变（无需在滑窗内维护）
-
更新上次边际化的信息：从当前边际化的信息中提取参数块并设置为本次边际化的约束条件
（2）代码实现
// vins_estimator/src/estimator.cpp/optimation()
TicToc t_whole_marginalization;
// 若倒数第二帧是关键帧，则将最老的一帧相关信息边缘化掉
if (marginalization_flag == MARGIN_OLD)
{
// 一个用来管理边缘化操作的对象
MarginalizationInfo *marginalization_info = new MarginalizationInfo();
// 这里类似于手写高斯牛顿，由于ceres的参数块都是double数组
// 因此后面用到的costFunction是和double数组绑定的，故也需要将eigen状态量都转成double数组
vector2double();
/*
关于边缘化有几点注意的地方
1、找到需要边缘化的参数块，这里是地图点，第0帧位姿，第0帧速度零偏
2、找到构造高斯牛顿下降时跟这些待边缘化相关的参数块有关的残差约束，具体如下:
a. 预积分残差: 待边缘化的第0帧与第1帧之间的预积分残差，所关联到的状态量有两帧之间的位姿和速度零偏
b. 视觉重投影残差约束: 待边缘化的第0帧上的特征点与其他帧的特征点之间能够投影到同一地图点，形成视觉重投影约束. 它所关联到的状态量有: 第0帧以及与之共视的滑窗中的其他帧，3d地图点的逆深度，相机与IMU之间的外参.
c. 上一次边缘化约束
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);
}
MarginalizationFactor *marginalization_factor = new
MarginalizationFactor(last_marginalization_info);
/*
构建和残差块相关的类ResidualBlockInfo
参数1：costFunction 参数2：核函数(NULL)
参数3：上一次边缘化产生的约束
参数4：在参数3的约束量中，需要被边缘化掉状态量，这里是第0帧的位姿和速度bias需要被边缘化掉
在舒尔补边缘化中，被边缘化掉的量需要放在前面
*/
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(
marginalization_factor, NULL,
last_marginalization_parameter_blocks,drop_set);
// 往边缘化对象中加入残差块
marginalization_info->addResidualBlockInfo(residual_block_info);
}
// 只有第1帧与第0帧之间存在预积分约束，故下面考虑的是pre_integrations[1]对应的残差块信息和边缘化操作
{
// 若预积分的累积积分的时间跨度超过10，则不能形成约束
if (pre_integrations[1]->sum_dt < 10.0)
{
/*
跟构建ceres约束问题一样，这里也需要得到残差和雅克比
IMUFactor类相当于前面的costFunction，在里面重载Evaluate函数，计算残差和雅可比，在之前的预积分部分中已详细介绍
*/
IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
/*
构建和残差块相关的类ResidualBlockInfo
参数1：costFunction 参数2：核函数(NULL)
参数3：约束量 第0帧的位姿、第0帧的速度bias、第1帧的位姿、第1帧的速度bias
参数4：在参数3的约束量中，哪些是需要被边缘化掉的，这里是第0和第1个参数块需要被边缘化掉，即第0帧的位姿和速度bias需要被边缘化掉
在舒尔补边缘化中，被边缘化掉的量需要放在前面
*/
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);
}
}
// 遍历视觉重投影的约束
{
int feature_index = -1;
for (auto &it_per_id : f_manager.feature) // 遍历每个特征点
{
// 当前特征点被观察到的帧数
it_per_id.used_num = it_per_id.feature_per_frame.size();
// 当前特征点的有效性：至少被2帧观察到，且观察到的第一帧不能是滑窗中倒数第2帧
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;
++feature_index;
// imu_i：观察到当前特征点的第一帧 imu_j: 用于获取观察到当前特征点的其他KF帧
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
// 要与imu_i(第0帧)形成视觉重投影约束
if (imu_i != 0)
continue;
// 若imu_i == 0，表示当前特征点能够被待边缘化的第0帧看到，故可以和被边缘化的参数块构成约束
// 获取当前特征点在被观察到的第0帧上的归一化投影点
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;
// 获取当前特征点在其他KF帧上的归一化投影点
Vector3d pts_j = it_per_frame.point;
// 下面是根据pts_i, pts_j构建重投影误差
// 根据是否约束延时确定残差阵 后续详细补上
if (ESTIMATE_TD)
{
}
else
{
// 根据pts_i和pts_j计算重投影误差来构建cost_function
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
/*
计算残差块
参数1：刚计算出来的cost_fuction
参数2：核函数loss_function
参数3：和当前重投影约束有关的参数块 第i帧和第j帧的位姿、外参以及特征点的逆深度
参数4：待边缘化的参数块 第0帧的位姿以及3D地图点(因为边缘化会将稀疏矩阵变成稠密矩阵，为避免矩阵维度过大，将3D地图点均边缘化掉)
*/
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);
}
}
}
}
// 所有的残差块都收集好了
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 // 若倒数第二帧不是关键帧，则将倒数第二帧边缘化掉
{
// 要求有上一次边缘化的结果(last_marginalization_info)，同时当前倒数第二帧是受到上一次边缘化的
// 约束的(即上一次边缘化对滑窗中倒数第二帧有约束，std::count > 0)
// last_marginalization_parameter_blocks: 表示上一次边缘化对哪些参数块形成约束
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++)
{
/*
对于IMU预积分中的速度零偏只会margin第1个，不可能margin掉倒数第二个
因为只有相邻帧才会形成预积分约束，当前边缘化倒数第二帧，那么上一次边缘化一定是第一帧，它与第二帧会形成
预积分约束，但与倒数第二帧不会形成预积分约束，因此没有IMU预积分的速度零偏先验约束。但由于上一次边缘化
中可能存在视觉重投影，因此会存在对位姿的约束
*/
ROS_ASSERT(last_marginalization_parameter_blocks[i] !=
para_SpeedBias[WINDOW_SIZE - 1]);
// 若上一次边缘化的参数中含有倒数第二帧，则将该参数push到drop_set中
// 以便在此次优化中将上一次边缘化产生的对当前倒数第二帧的约束去掉(边缘化掉)
if (last_marginalization_parameter_blocks[i] == para_Pose[WINDOW_SIZE-1])
drop_set.push_back(i);
}
// 定义一个MarginalizationFactor作为loss function，它用于上一次边缘化对当前倒数第二帧产生的残差约束
// 此时，在当前倒数第二帧边缘化过程中需要将上一次边缘化对当前倒数第二帧产生约束的参数块给边缘化掉
MarginalizationFactor *marginalization_factor = new
MarginalizationFactor(last_marginalization_info);
// 参数1: loss function 参数2: 参数块 参数3: 待边缘化的参数索引
ResidualBlockInfo *residual_block_info = new
ResidualBlockInfo(marginalization_factor, NULL,
last_marginalization_parameter_blocks,
drop_set);
// 将残差块信息residual_block_info添加到marginalization_info中
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;
// addr_shift的first表示滑窗中的各帧在边缘化之前的位置
// addr_shift的second表示滑窗中的各帧在边缘化之后的位置
for (int i = 0; i <= WINDOW_SIZE; i++)
{
// 若遍历到倒数第二帧，它要被边缘化掉，在滑窗中不存在，因此continue
if (i == WINDOW_SIZE - 1)
continue;
// 若遍历到倒数第一帧，则它的位姿和速度零偏都要往前移到倒数第二帧位置上
else if (i == WINDOW_SIZE)
{
// key: para_Pose[i]是指原来的位置 value: para_Pose[i - 1]是指新的位置
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];
}
// parameter_blocks：去掉待边缘化的倒数第二帧之后的参数块
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;
}
}3. MarginalizationInfo
3.1 边缘化信息类MarginalizationInfo的定义
class MarginalizationInfo
{
public:
~MarginalizationInfo();
int localSize(int size) const;
int globalSize(int size) const;
void addResidualBlockInfo(ResidualBlockInfo *residual_block_info);
void preMarginalize();
void marginalize();
std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift);
std::vector<ResidualBlockInfo *> factors;
int m, n;
std::unordered_map<long, int> parameter_block_size; //global size 地址->global size
int sum_block_size;
std::unordered_map<long, int> parameter_block_idx; //local size 地址->参数排列的顺序idx
std::unordered_map<long, double *> parameter_block_data; // 地址->参数块实际内容的地址
std::vector<int> keep_block_size; // global size
std::vector<int> keep_block_idx; // local size
std::vector<double *> keep_block_data;
Eigen::MatrixXd linearized_jacobians;
Eigen::VectorXd linearized_residuals;
const double eps = 1e-8;
};3.2 添加残差块信息，为边缘化做准备（addResidualBlockInfo）
/** * @brief a. 将所有残差块信息放到factors中
* b. 将所有残差块参数以首地址 ———— 大小的键值对形式放到unordered_map类型的parameter_block_size中
* c. 将待边缘化的残差块参数以首地址 ———— 索引的键值对形式放到unordered_map类型的parameter_block_idx中
* * @param[in] residual_block_info
*/
void MarginalizationInfo::addResidualBlockInfo(ResidualBlockInfo *residual_block_info)
{
factors.emplace_back(residual_block_info); // 将残差块信息收集到factors中
std::vector<double *> ¶meter_blocks = residual_block_info->parameter_blocks;
// 通过残差块信息residual_block_info里面cost_function的ceres中的parameter_block_sizes()接口获取每个约束参数块的大小
std::vector<int> parameter_block_sizes = residual_block_info->cost_function->
parameter_block_sizes(); // 各个参数块的大小
// 遍历每个约束参数块
// 最终parameter_block_size里面存储的是: 所有约束的参数块首地址 ———— 大小 (键值对)
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]; // 获取遍历到的参数块的大小
// unordered_map可避免重复添加(如当预积分约束和视觉重投影约束涉及到相同参数块时，这样可以避免重复添加该相同参数块)
// map是一个红黑树，unordered_map是一个哈希表
// 二者的区别在于：map增删改查的时间复杂度是O(log(n))
// 哈希表的本质是一个数组，其增删改查的时间复杂度是O(1)
// 将地址转为long类型，以地址作为key，对应的大小作为value
parameter_block_size[reinterpret_cast<long>(addr)] = size;
}
// 遍历待边缘化的参数块
// 最终parameter_block_idx里面存储的是：所有待边缘化的参数块首地址 ———— 索引(键值对)
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]];
// 待边缘化的参数块的unordered_map key：转换为long类型的地址 value：0
parameter_block_idx[reinterpret_cast<long>(addr)] = 0;
}
}3.3 边缘化预处理preMarginalize
/** * @brief 将各个残差块计算残差和雅克比，同时备份所有相关的参数块内容
* 以便在边缘化之后，对滑窗中参数块内容进行更新
* */
void MarginalizationInfo::preMarginalize()
{
for (auto it : factors) // 遍历之前收集到的残差块
{
// 调用类ResidualBlockInfo里面的Evaluate接口计算各个残差块的残差和雅克比矩阵
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]; // 参数块大小
/*
把各个参数块都备份到parameter_block_data中，使用unordered_map避免重复参数块，之所以备份，是为了后面的状态保留.
如果在parameter_block_data中没有找到当前参数块的地址，则执行下面if内容，新建地址data并将当前参数块内容拷贝到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);
parameter_block_data[addr] = data; // 地址->参数块实际内容的地址
}
}
}
}3.4 边缘化marginalize
（1）大致流程
遵循前面边缘化舒尔补理论框架，在此基础之上将增量方程进行重新表述如下：
(\Lambda_c - \Lambda_b^T\Lambda_a^{-1}\Lambda_b)\delta x_b = g_b - \Lambda_b^T\Lambda_a^{-1}g_a
构建信息矩阵H的过程需要明确每个参数块在其对应的矩阵中的索引位置，并将那些需要被边缘化的参数块安排至序列的前部位置；而那些不需要被边缘化的则应放置于序列的后端部分。
利用多线程，加速计算增量方程Hx=g中的H和g
基于特征值与特征向量之间的关系式Ax=\lambda x进一步推导得出逆矩阵的表达式为A^{-1}=x^{-1}\lambda^{-1}x；通过计算增量方程中的相关项\Lambda_a^{-1}来确定系统的动态特性。
基于舒尔补的方法对矩阵块进行分割，并通过奇异值分解(SVD)来计算得到边缘化后的雅可比矩阵J及其对应的残差e。
（2）代码实现
/** * @brief 边缘化处理，并将结果转换成残差和雅克比的形式
* */
void MarginalizationInfo::marginalize()
{
int pos = 0; // pos表示参数块在大H系数矩阵中的起始位置
// parameter_block_idx key在添加残差块信息时表示各个待边缘化参数块地址 value预设都是0
// 下面for循环是将所有待边缘化的参数块依次放到系数矩阵H的前面
for (auto &it : parameter_block_idx)
{
it.second = pos; // 这就是在所有参数中排序的idx，待边缘化的排在前面
// 获取待边缘化的参数块在H矩阵中的位置
// 因为要进行求导，因此大小是local size，具体一点就是使用李代数
pos += localSize(parameter_block_size[it.first]);
}
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; // 其他参数块的总大小
// 以上是将所有参数块进行了排序，待边缘化的参数块排在前面，不需边缘化的参数块排在后面
TicToc t_summing;
Eigen::MatrixXd A(pos, pos); // Ax = b预设大小 遍历到这里pos表示矩阵的总大小
Eigen::VectorXd b(pos);
A.setZero();
b.setZero();
// 往A矩阵和b矩阵中添加相应内容，利用多线程加速
TicToc t_thread_summing;
pthread_t tids[NUM_THREADS];
ThreadsStruct threadsstruct[NUM_THREADS];
int i = 0;
for (auto it : factors)
{
// 将前面收集的残差块信息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);
// 多线程访问会带来冲突，因此每个线程备份一下要查询的总的残差块索引及大小(unordered_map)
threadsstruct[i].parameter_block_size = parameter_block_size; // 大小
threadsstruct[i].parameter_block_idx = parameter_block_idx; // 索引
// 创建线程threadsstruct[i] 每个线程要执行的任务ThreadsConstructA
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;
}
// 以上是构建Hx = g的整体方程，下面进行边缘化
// Amm矩阵是大矩阵A中待边缘化部分，下面求其与转置的平均，是为了保证其正定性，可便于对其求逆
Eigen::MatrixXd Amm = 0.5 * (A.block(0, 0, m, m) + A.block(0, 0, m, m).transpose());
// 对矩阵Amm进行特征值分解，然后可根据分解得到的特征值求出矩阵Amm的逆矩阵
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm);
/*
一个逆矩阵的特征值是原矩阵的倒数，特征向量相同 select类似c++中 ? :运算符
(saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)
若特征值saes.eigenvalues().array() 大于 eps(0)，则取saes.eigenvalues().array().inverse(特征值的逆)，否则取0
利用特征值取逆来构造其逆矩阵
A的逆 = 特征值对应的特征向量 * 特征值的倒数构成的对角矩阵 * 特征值对应的特征向量的转置
*/
Eigen::MatrixXd Amm_inv = saes.eigenvectors() *
Eigen::VectorXd((saes.eigenvalues().array() > eps).select
(saes.eigenvalues().array().inverse(), 0)).asDiagonal() *
saes.eigenvectors().transpose();
// 大H矩阵的分块：H_{11} = A_mm H_{12} = Amr H_{21} = Arm H_{22} = Arr
// 大b矩阵的分块：b_1 = bmm b_2 = brr
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和b
A = Arr - Arm * Amm_inv * Amr;
b = brr - Arm * Amm_inv * bmm;
/*
下面根据Ax = b => JT*J = - JT * e 计算新的残差e和雅可比J
对A做特征值分解 A = V * S * VT,其中Ｓ是特征值构成的对角矩阵，V是其特征值对应的特征向量构成的矩阵
所以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
*/
// 对新的A矩阵进行特征值分解，分解形式为：A = V * S * VT
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(A);
// 对A矩阵取逆 S为A的特征值构成的对角矩阵 S_inv为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));
// 这个求得就是 S^(1/2)，用于求新的雅可比J 不过这里是向量还不是矩阵
Eigen::VectorXd S_sqrt = S.cwiseSqrt();
// 这个求得的就是S^(-1/2)，用于求增量方程的e
Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();
// 边缘化是为了实现对剩下参数块的约束，为了便于一起优化，就抽象成了残差和雅克比的形式
// J = S^(1/2) * VT
linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();
// e = (JT)-1 * b
linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose()* b;
}（3）构造子增量方程ThreadsConstructA
/** * @brief 分线程构造Ax = b
* * @param[in] threadsstruct
* @return void* */
void* ThreadsConstructA(void* threadsstruct)
{
// 将void*类型指针恢复成ThreadsStruct*类型
ThreadsStruct* p = ((ThreadsStruct*)threadsstruct);
// 遍历当前线程p分配的子任务sub_factors
for (auto it : p->sub_factors)
{
// 遍历当前子任务it(如某一约束---重投影约束)中的参数块
for (int i = 0; i < static_cast<int>(it->parameter_blocks.size()); i++)
{
// idx_i: 表示当前子任务(某一约束)中遍历到的当前参数块的索引
int idx_i = p->parameter_block_idx[reinterpret_cast<long>(it->
parameter_blocks[i])]; // 在大矩阵中的id，也就是落座的位置
// size_i: 表示当前子任务(某一约束)中遍历到的当前参数块的大小
int size_i = p->parameter_block_size[reinterpret_cast<long>(it->
parameter_blocks[i])];
// 确保是local size 将global size转成local size
if (size_i == 7)
size_i = 6;
// 之前pre margin 已经算好了各个残差和雅克比，这里取出来
Eigen::MatrixXd jacobian_i = it->jacobians[i].leftCols(size_i);
// 和本身以及其他雅克比块构造H矩阵 i: 当前参数块， j: 另一个参数块
for (int j = i; j < static_cast<int>(it->parameter_blocks.size()); j++)
{
int idx_j = p->parameter_block_idx[reinterpret_cast<long>(it->
parameter_blocks[j])]; // 在大矩阵中的id，也就是落座的位置
int size_j = p->parameter_block_size[reinterpret_cast<long>(it->
parameter_blocks[j])];
if (size_j == 7)
size_j = 6;
Eigen::MatrixXd jacobian_j = it->jacobians[j].leftCols(size_j);
// 两个雅克比都取出来了
if (i == j) // 自己与自己约束的雅可比，位于主对角线上
p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;
else // 自己与别人约束的雅可比，关于主对角线对称
{
// (i, j)
p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;
// 主对角线对称
p->A.block(idx_j, idx_i, size_j, size_i) = p->A.block(idx_i, idx_j, size_i, size_j).transpose();
}
}
// 然后构建g矩阵
p->b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals;
}
}
return threadsstruct;
}3.5 获取边缘化后的参数块getParameterBlocks
std::vector<double *> MarginalizationInfo::getParameterBlocks(
std::unordered_map<long, double *> &addr_shift)
{
std::vector<double *> keep_block_addr;
keep_block_size.clear();
keep_block_idx.clear();
keep_block_data.clear();
for (const auto &it : parameter_block_idx) // 遍历边缘化相关的每个参数块
{
// it.second >= m表明it是保留下来的参数块
// it.second < m表明it是被边缘化掉的参数块，不会出现在滑窗里面
if (it.second >= m) // 如果是留下来的，说明后续会对其形成约束
{
// 留下来的参数块大小 global size
keep_block_size.push_back(parameter_block_size[it.first]);
// 留下来的参数块在原向量中的排序
keep_block_idx.push_back(parameter_block_idx[it.first]);
// 留下来的参数块在边缘化前的值的备份
keep_block_data.push_back(parameter_block_data[it.first]);
// 留下来的参数块在边缘化后的对应新地址
keep_block_addr.push_back(addr_shift[it.first]);
}
}
// 留下来的边缘化后的参数块总大小
sum_block_size = std::accumulate(std::begin(keep_block_size),
std::end(keep_block_size), 0);
return keep_block_addr; // 返回受到边缘化约束后保留下来的参数块的新地址
}4. MarginalizationFactor
4.1 MarginalizationFactor类的定义
// 由于边缘化的costfuntion不是固定大小的，不能明确知道约束的参数块个数和大小，因此只能继承最基本的类
// IMUFactor继承的是SizedCostFunction
class MarginalizationFactor : public ceres::CostFunction
{
public:
MarginalizationFactor(MarginalizationInfo* _marginalization_info);
virtual bool Evaluate(double const *const *parameters,
double *residuals, double **jacobians) const;
MarginalizationInfo* marginalization_info;
};4.2 MarginalizationFactor构造函数
/** * @brief Construct a new Marginalization Factor:: Marginalization Factor object
* 边缘化信息结果的构造函数，根据边缘化信息在构造函数中确定参数块总数和大小以及残差维数
* * @param[in] _marginalization_info
*/
MarginalizationFactor::MarginalizationFactor(MarginalizationInfo* _marginalization_info):marginalization_info(_marginalization_info)
{
int cnt = 0;
// keep_block_size表示上一次边缘化留下来的参数块的大小
for (auto it : marginalization_info->keep_block_size)
{
mutable_parameter_block_sizes()->push_back(it); // 调用ceres接口，添加参数块大小信息
cnt += it;
}
// 残差维数就是所有剩余状态量的维数和，这里是local size
set_num_residuals(marginalization_info->n);
};4.3 Evaluate
（1）大致步骤
- 针对未边缘化的剩余残差块进行遍历处理。
- 重新计算残差块的位置索引值（原始位置坐标减去边缘化区域的残差块数量）。
- 依据FEJ理论，在相同的线性化基点上利用一阶泰勒展开式来更新边缘化后的误差项e。
- 同时更新状态变化量：位置坐标与姿态信息的总和。
（2）代码实现
/** * @brief 边缘化结果残差和雅克比的计算
* * @param[in] parameters
* @param[in] residuals
* @param[in] jacobians
* @return true
* @return false
*/
bool MarginalizationFactor::Evaluate(double const *const *parameters,
double *residuals, double **jacobians) const
{
int n = marginalization_info->n; // 上一次边缘化保留的残差块的local size的和,也就是残差维数
int m = marginalization_info->m; // 上一次边缘化的被margin的残差块总和
Eigen::VectorXd dx(n); // 用来存储残差
// 遍历所有的剩下的有约束的残差块
for (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++)
{
// 保留下来的残差块大小
int size = marginalization_info->keep_block_size[i];
// 将保留下来的索引idx起点统一到0
int idx = marginalization_info->keep_block_idx[i] - m;
// x: 经过上一次边缘化约束后调整的当前参数块 ———— 参数块大小(键值对)
// x0: 上一次边缘化时保留下来的当时参数块 ———— 参数块大小(键值对)
Eigen::VectorXd x = Eigen::Map<const Eigen::VectorXd>(parameters[i], size);
Eigen::VectorXd x0 = Eigen::Map<const Eigen::VectorXd>(marginalization_info->keep_block_data[i], size);
if (size != 7) // 说明不是位姿
// 不需要local param的直接做差 当前值和边缘化后的值的差
dx.segment(idx, size) = x - x0;
else // 代表位姿的param
{
// 前面3维表示位置，采用广义减法做差
dx.segment<3>(idx + 0) = x.head<3>() - x0.head<3>();
// 后面4维表示旋转，采用李代数做差 取虚部的theta (乘以2.0)
dx.segment<3>(idx + 3) = 2.0 * Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();
// 确保实部大于0
if (!((Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() *
Eigen::Quaterniond(x(6), x(3), x(4), x(5))).w() >= 0))
{
dx.segment<3>(idx + 3) = 2.0 * -Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();
}
}
}
// 更新残差 边缘化后的先验误差 e = e0 + J * dx
// 个人理解：根据FEJ．先验约束的雅克比保持不变，但残差会随优化而变化，因此下面不更新雅克比,只更新残差
// 可以参考
// marginalization_info->linearized_residuals: 原先的残差e0
// marginalization_info->linearized_jacobians: 不变的雅可比J
Eigen::Map<Eigen::VectorXd>(residuals, n) = marginalization_info->
linearized_residuals + marginalization_info->linearized_jacobians * dx;
// 下面是保证雅可比不变
if (jacobians)
{
for (int i = 0; i < static_cast<int>(marginalization_info->
keep_block_size.size()); i++)
{
if (jacobians[i])
{
// size为保留下来的残差块大小 local_size为FEJ之后的残差块大小
int size = marginalization_info->keep_block_size[i],
local_size = marginalization_info->localSize(size);
// idx表示统一到起点为0的残差块索引
int idx = marginalization_info->keep_block_idx[i] - m;
// 根据残差维度n和参数块维度size定义雅可比jacobian
Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>> jacobian(jacobians[i], n, size);
jacobian.setZero(); // 先将定义的雅可比清零
// 从之前的大雅可比矩阵marginalization_info->linearized_jacobians中对应idx处开始
// 取出子雅可比矩阵作为当前参数块的雅可比
jacobian.leftCols(local_size) = marginalization_info->
linearized_jacobians.middleCols(idx, local_size);
}
}
}
return true;
}5. ResidualBlockInfo
5.1 ResidualBlockInfo类的定义
struct ResidualBlockInfo
{
// 构造函数需要，cost function（约束），loss function：残差的计算方式，相关联的参数块，待边缘化的参数块的索引
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) {}
void Evaluate();
ceres::CostFunction *cost_function;
ceres::LossFunction *loss_function;
std::vector<double *> parameter_blocks;
std::vector<int> drop_set;
double **raw_jacobians;
std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobians;
Eigen::VectorXd residuals;
int localSize(int size)
{
return size == 7 ? 6 : size;
}
};5.2 边缘化模块的残差和雅可比计算ResidualBlockInfo::Evaluate
/** * @brief 待边缘化的各个残差块计算残差和雅克比矩阵，同时处理核函数的case
* */
void ResidualBlockInfo::Evaluate()
{
residuals.resize(cost_function->num_residuals()); // 确定残差的维数
// 确定相关的参数块数目及对应的参数块大小
std::vector<int> block_sizes = cost_function->parameter_block_sizes();
/*
raw_jacobians与jacobians的区别:
raw_jacobians是double类型的二维数组，里面存放的是指针(雅可比地址)
jacobians是eigen类型的二维数组，里面存储的是Matrix类型的雅可比
下面通过for循环，将jacobians中每个参数块的雅可比首地址赋给raw_jacobians
这样操作的原因是在调用cost_function的Evaluate接口计算雅可比时是需要转为double类型的
*/
// ceres接口都是double数组，因此这里给雅克比准备数组
raw_jacobians = new double *[block_sizes.size()];
jacobians.resize(block_sizes.size());
// 这里就是把jacobians每个matrix地址赋给raw_jacobians
// 然后把raw_jacobians传递给ceres的接口，这样计算结果直接放进了这个matrix
for (int i = 0; i < static_cast<int>(block_sizes.size()); i++)
{
// 雅克比矩阵大小: cost_function->num_residuals(): 残差维度 block_sizes[i]: 参数块维度
jacobians[i].resize(cost_function->num_residuals(), block_sizes[i]);
raw_jacobians[i] = jacobians[i].data();
}
// 调用各自重载的接口计算残差和雅克比 这里实际上结果放在了jacobians
// 参数块 残差 雅可比
cost_function->Evaluate(parameter_blocks.data(), residuals.data(), raw_jacobians);
// 如果有核函数，那么就对残差进行相应调整
if (loss_function)
{
double residual_scaling_, alpha_sq_norm_;
double sq_norm, rho[3];
sq_norm = residuals.squaredNorm(); // 获得残差的模
// 疑问: 这里是如何通过Evaluate计算出rho来的？？？
// rho[0]:核函数这个点的值 rho[1]这个点的一阶导数 rho[2]这个点的二阶导数
loss_function->Evaluate(sq_norm, rho);
double sqrt_rho1_ = sqrt(rho[1]); // 取出核函数一阶导的根号
// 柯西核p = log(s+1),rho[2]<＝0始终成立，一般核函数二阶导数都是小于0, 因此执行if不执行else
if ((sq_norm == 0.0) || (rho[2] <= 0.0))
{
residual_scaling_ = sqrt_rho1_;
alpha_sq_norm_ = 0.0;
}
else
{
const double D = 1.0 + 2.0 * sq_norm * rho[2] / rho[1];
const double alpha = 1.0 - sqrt(D);
residual_scaling_ = sqrt_rho1_ / (1 - alpha);
alpha_sq_norm_ = alpha / sq_norm;
}
// 这里就相当于残差雅克比都乘上sqrt_rho1_，及核函数所在的点的一阶导
// (上面的if中也是sqrt_rho1_), 基本都是小于1
for (int i = 0; i < static_cast<int>(parameter_blocks.size()); i++)
{
jacobians[i] = sqrt_rho1_ * (jacobians[i] - alpha_sq_norm_ * residuals *
(residuals.transpose() * jacobians[i]));
}
residuals *= residual_scaling_;
}
}全部评论 (0)
还没有任何评论哟~