“ 在优化完成后，滑动窗口前，必须进行边缘化项的操作”
一、理论部分
VINS 细节系列 - 窗口优化_努力努力努力-CSDN博客
一个边缘化的例子
下面我们用一个具体例子来形象说明边缘化过程及其导致的矩阵稠密现象(fill-in)。设有 四个相机位姿 xpi ,
以及 6 个路标点xmk (路标点用 xyz 的参数化), 相机与路标点的边表示一次观测, 相邻相机之间的边表示 IMU 约束,
相互关系如下:
下面试图将 xp1 给 marg 掉,然后再将 xm1 给 marg 掉, 看看 H 矩阵会如何变化
图(12-a) 表示原始的 H 矩阵,注意这里的 左上角为路标点相关部分, 而右下角是 pose 相关部分
图(12-b)是把 H 矩阵中跟x p1 相关的部分移动到 H 矩阵左上角
其中各矩阵的维度已在上式标明,也就是说图(12-c)的矩阵大小为 32×32。我们可以看到,图(c)相对于图(b)变得更稠密了,
即 marg 掉一个 pose,会使得剩余的 H 矩阵有 3 个地方被 fill-in,如图(c)中颜色加重区域。
这时图关系则变为:
注意,观察图 16 可发现这时的 xm1 、xm2 和 xm3 彼此之间已经产生了新的约束关系,且xp2 也与xm1 产生了新的约束关系。
图(12-d)是 marg 掉xm1 后的 H 矩阵,详细如下所示:
对应的图关系如下:
我们发现, marg 掉xm1 后,并没有是 H 矩阵更稠密,这是因为xm1 之前并未与其他pose 有约束关系,即并未被观察到,
因此如果 marg 掉那些不被其他帧观测到的路标点,不会显著地使 H 矩阵变得稠密。而要 marg 掉的路标点中,对于那
些被其他帧观测到路标点,要么就别设置为 marg, 要么就宁愿丢弃,这就是 OKVIS 中用到的策略
简单介绍
1、优化变量：
序号 变量 维度
0 para_Pose （6维，相机位姿）
1 para_SpeedBias （9维，相机速度、加速度偏置、角速度偏置）、
2 para_Ex_Pose （6维、相机IMU外参）、
3 para_Feature （1维，特征点深度）、
4 para_Td （1维，标定同步时间）
五部分组成，在后面进行边缘化操作时这些优化变量都是当做整体看待
last_marginalization_parameter_blocks ：Xb变量，也是我们要优化的变量；
last_marginalization_info ：Xm与Xb对应的测量Zb，也是先验残差（细品一下）
2、思路：
边缘化策略
边缘化分两种情况，每种情况有各自的流程
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）调整参数块在下一次窗口中对应的位置（去掉次新帧）
3、marginalization_factor.cpp 代码中有几个变量需要提前说明:
举例说明,当第一次 marg 掉最老帧时,parameter_block_size 为所有变量及其对应的
localSize, parameter_block_data 为对应的数据(double*类型)
二、程序部分
2.1 把下面这个图搞明白！
2.2 看 "marginalization_factor.h" 中类和结构体的定义
1、class ResidualBlockInfo
类描述：这个类保存了待marg变量（xm）与相关联变量（xb）之间的一个约束因子关系 - Zm
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) {}
void Evaluate();//调用cost_function的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;
}
};
| 成员变量 | 描述 |
|---|---|
| cost_function | 对应ceres中表示约束因子的类 |
| parameter_blocks | 该约束因子相关联的参数块变量 |
| drop_set | parameter_blocks中待marg变量的索引 |
2、class 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;
//这里将参数块分为Xm,Xb,Xr,Xm表示被marg掉的参数块，Xb表示与Xm相连接的参数块，Xr表示剩余的参数块
//那么m=Xm的localsize之和，n为Xb的localsize之和，pos为（Xm+Xb）localsize之和
int m, n;
std::unordered_map<long, int> parameter_block_size; //global size 将被marg掉的约束边相关联的参数块，即将被marg掉的参数块以及与它们直接相连的参数快
int sum_block_size;
std::unordered_map<long, int> parameter_block_idx; //local size
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;
};
| 成员变量 | 描述 |
|---|---|
| factors | xm与xb之间的约束因子，Zb |
| m | 所有将被marg掉变量的localsize之和，如上图中xm的localsize |
| n | 所有与将被marg掉变量有约束关系的变量的localsize之和，如上图中xb的localsize |
| parameter_block_size | <参数块地址，参数块的global size>，参数块包括xm和xb |
| parameter_block_idx | <参数块地址，参数块排序好后的索引>，对参数块进行排序，xm排在前面，xb排成后面，使用localsize |
| parameter_block_data | <参数块地址，参数块数据>，需要注意的是这里保存的参数块数据是原始参数块数据的一个拷贝，不再变化，用于记录这些参数块变量在marg时的状态 |
| keep_block_size | <保留下来的参数块地址，参数块的globalsize> |
| keep_block_idx | <保留下来的参数块地址，参数块的索引>，保留下来的参数块是xb |
成员函数信息：
| 成员函数 | 描述 |
|---|---|
| preMarginalize | 得到每次IMU和视觉观测对应的参数块，雅克比矩阵，残差值 |
| marginalize | 开启多线程构建信息矩阵H和b ，同时从H,b中恢复出线性化雅克比和残差 |
3、class MarginalizationFactor
类描述：该类是优化时表示上一步边缘化后保留下来的先验信息代价因子，变量marginalization_info保存了类似约束测量信息
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;
};
2.3 程序讲解
边缘化部分在 optimization 函数里面，当执行完后端优化后，紧接着就会执行边缘化，两个过程都需要添加优化变量和残差，
但区别是边缘化的变量少一点！主要是构建模块的过程！
一、如果边缘化 “老帧”
【1】首先 把上一次先验项中的残差项(尺寸为 n)传递给当前先验项，并从中去除需要丢弃 的状态量;
即从旧的先验残差项 last_marginalization_info 新建一个新的 marginalization_factor，参与这个残差项的优化变量是：last_marginalization_parameter_blocks 里面的内容：para_Pose、para_SpeedBias、para_Ex_Pose、para_Feature、para_Td 都是按顺序排好的，所以要把 要丢弃 的状态量 para_Pose[0]、para_SpeedBias[0] 对应的序号用 drop_set 记录下来
// --------------------------------------marginalization-----------------------------------------
//添加残差以及优化变量的方式和后端线性优化中添加的方式相似，因为边缘化类应该就是仿照ceres写的
TicToc t_whole_marginalization;
//一、边缘化老帧，那就要考虑老帧的情况！
//要边缘化的参数块是 para_Pose[0] para_SpeedBias[0] 以及 para_Feature[feature_index](滑窗内的第feature_index个点的逆深度)
if (marginalization_flag == MARGIN_OLD)
{
MarginalizationInfo *marginalization_info = new MarginalizationInfo();//先验信息
vector2double();
//【1】首先添加上一次先验残差项
if (last_marginalization_info && last_marginalization_info->valid)
{
vector<int> drop_set;
for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
{
//查询last_marginalization_parameter_blocks中是首帧状态量的序号
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 *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
last_marginalization_parameter_blocks,drop_set);
//调用addResidualBlockInfo()函数将各个残差以及残差涉及的优化变量添加入优化变量中
marginalization_info->addResidualBlockInfo(residual_block_info);
}【2】添加IMU的marg信息到 marginalization_info 中：第0帧和第1帧之间的IMU预积分值以及第0帧和第1帧相关优化变量
if(USE_IMU)
{
if (pre_integrations[1]->sum_dt < 10.0)
{
IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
//0和1是para_Pose[0], para_SpeedBias[0]，需要marg的变量
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】最后添加第一次观测滑窗中第0帧的路标点以及其他相关的滑窗中的帧的相关的优化变量 到 marginalization_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 < 4)
continue;
++feature_index;
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
//特征点的起始帧必须是首帧，因此后面用来构建marg矩阵的都是和第一帧有共视关系的滑窗帧，因为 marginalization_flag == MARGIN_OLD
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)//不需要起始观测帧
{
Vector3d pts_j = it_per_frame.point;
ProjectionTwoFrameOneCamFactor *f_td = new ProjectionTwoFrameOneCamFactor(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);
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);
}
if(STEREO && it_per_frame.is_stereo)
{
Vector3d pts_j_right = it_per_frame.pointRight;
if(imu_i != imu_j)
{
ProjectionTwoFrameTwoCamFactor *f = new ProjectionTwoFrameTwoCamFactor(pts_i, pts_j_right, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocityRight,
it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]},vector<int>{0, 4});
marginalization_info->addResidualBlockInfo(residual_block_info);
}
else
{
ProjectionOneFrameTwoCamFactor *f = new ProjectionOneFrameTwoCamFactor(pts_i, pts_j_right, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocityRight,
it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,vector<double *>{para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]},vector<int>{2});
marginalization_info->addResidualBlockInfo(residual_block_info);
}
}
}
}
}【4】marginalization_info->preMarginalize():得到每次 IMU 和视觉观测(cost_function)对 应的参数块(parameter_blocks)，雅可比矩阵(jacobians)，残差值(residuals);
marginalization_info->preMarginalize();
【4】marginalization_info->marginalize():多线程计整个先验项的参数块，雅可比矩阵和 残差值，其中与代码对应关系为:
marginalization_info->marginalize();
【5】最后移交了优化项需要得到的两个变量：last_marginalization_info和last_marginalization_parameter_blocks
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];
}
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;
重要的三个结构变量：
- MarginalizationFactor *marginalization_factor
- *MarginalizationInfo marginalization_info
- ResidualBlockInfo *residual_block_info
MarginalizationFactor *marginalization_factor 两个作用：（1）构建ceres的残差项，即计算residual （2）构建ResidualBlockInfo *residual_block_info
MarginalizationInfo *marginalization_info 由他可以获取边缘化信息，用它来构建MarginalizationFactor *marginalization_factor以及得到对应的参数块
ResidualBlockInfo *residual_block_info 用它来丰富marginalization_info项的信息
从头到位都是marginalization_info这个变量来进行统筹安排进行边缘化。
通过marginalization_info->addResidualBlockInfo()来添加约束，有三个方面的来源：（1）旧的（2）imu预积分项（3）特征点
理解：
上面三步通过调用 addResidualBlockInfo() 已经确定优化变量的数量、存储位置、长度以及待优化变量的数量以及存储位置；
为什么要添加这些量呀？因为在k时刻这些量是我们要边缘化掉的变量，残差；但是k+1时刻就是我们的先验，这不正是我们初衷嘛！
需要注意的是这些都是和第一帧图像有关的！因为我们边缘化的是最老帧！三部分的流程都是一样的，如下：
第一步 定义损失函数，对于先验残差就是MarginalizationFactor，对于IMU就是IMUFactor，对于视觉就是ProjectionTdFactor，
这三个损失函数的类都是继承自ceres的损失函数类ceres::CostFunction，里面都重载了函数; 函数通过传入的优化变量值 parameters，
以及先验值（对于先验残差就是上一时刻的先验残差last_marginalization_info，对于IMU就是预计分值pre_integrations[1]，对于视觉
就是空间的的像素坐标pts_i, pts_j）可以计算出各项残差值residuals，以及残差对应个优化变量的雅克比矩阵jacobians；
bool MarginalizationFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
int n = marginalization_info->n;
int m = marginalization_info->m;
Eigen::VectorXd dx(n);
//遍历本次边缘化保留下的参数块：keep_block_size
for (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++)
{
//得到该参数块的大小和序号
int size = marginalization_info->keep_block_size[i];
int idx = marginalization_info->keep_block_idx[i] - m;
//x 感觉是边缘化之前 BA优化后的值 ？
Eigen::VectorXd x = Eigen::Map<const Eigen::VectorXd>(parameters[i], size);
//x0表示marg时参数块变量的值(即xb)
Eigen::VectorXd x0 = Eigen::Map<const Eigen::VectorXd>(marginalization_info->keep_block_data[i], size);
if (size != 7)//speed_bias 9项
dx.segment(idx, size) = x - x0;//变量的更新
else//位置，姿态项, 姿态不能直接加减了！要变成四元素的 ✖
{
//使用四元数的表达方式
dx.segment<3>(idx + 0) = x.head<3>() - x0.head<3>();
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();
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();
}
}
}
//计算残差，理解，泰勒展开求的公式
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])
{
int size = marginalization_info->keep_block_size[i], local_size = marginalization_info->localSize(size);
int idx = marginalization_info->keep_block_idx[i] - m;//
Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(jacobians[i], n, size);
jacobian.setZero();
jacobian.leftCols(local_size) = marginalization_info->linearized_jacobians.middleCols(idx, local_size);
}
}
}
return true;
}上面这段代码是在优化过程中，对先验约束项的更新，主要是对先验残差的更新，雅克比不再发生变化。
代码中的x0 表示marg xm状态时 xb变量的值; 优化过程中先验项的雅克比保持不变，但是残差需要变化，其变化公式推导如下：
第二步定义ResidualBlockInfo，其构造函数如下：
ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector<double *> _parameter_blocks, std::vector<int> _drop_set)
----------------------------------------------------------------------------------------------------
void ResidualBlockInfo::Evaluate()
{
//获取残差的个数： IMU + 视觉
residuals.resize(cost_function->num_residuals());
//优化变量参数块的变量大小：para_Pose、para_SpeedBias、para_Ex_Pose、para_Feature、para_Td
std::vector<int> block_sizes = cost_function->parameter_block_sizes();
//数组外围的大小，也就是参数块的个数
raw_jacobians = new double *[block_sizes.size()];
jacobians.resize(block_sizes.size());
//分配每一行的大小，残差的维数*每个参数块中参数的个数block_sizes[i]，J矩阵大小的确认！想一下
//比如：两个残差f1,f2;5个变量x1,x2,,,x5, 则J矩阵是2行5列呀
for (int i = 0; i < static_cast<int>(block_sizes.size()); i++)
{
jacobians[i].resize(cost_function->num_residuals(), block_sizes[i]);
raw_jacobians[i] = jacobians[i].data();
}
//利用各自残差的Evaluate函数计算残差和雅克比矩阵。
cost_function->Evaluate(parameter_blocks.data(), residuals.data(), raw_jacobians);
//好像明白了，这个是视觉里面有的Huber核函数；有鲁棒核函数的残差部分，重写雅克比与残差
if (loss_function)
{
double residual_scaling_, alpha_sq_norm_;
double sq_norm, rho[3];
sq_norm = residuals.squaredNorm();
loss_function->Evaluate(sq_norm, rho);
double sqrt_rho1_ = sqrt(rho[1]);
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;
}
//感觉像根据先验残差，推出新的残差和雅可比公式一样！
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_;
}
}这一步是为了将不同的损失函数_cost_function以及优化变量_parameter_blocks统一起来再一起添加到marginalization_info中。
变量_loss_function是核函数，在VINS-mono的边缘化中仅仅视觉残差有用到核函数，另外会设置需要被边缘话的优化变量的位置
_drop_set，这里对于不同损失函数又会有不同：对于先验损失，其待边缘化优化变量是根据是否等于para_Pose[0]或者
para_SpeedBias[0]，也就是说和第一帧相关的优化变量都作为边缘化的对象。对于IMU，其输入的_drop_set是 vector{0, 1}，
也就是说其待边缘化变量是para_Pose[0], para_SpeedBias[0]，也是第一帧相关的变量都作为边缘化的对象，
这里值得注意的是和后端优化不同，这里只添加了第一帧和第二帧的相关变量作为优化变量，因此边缘化构造的信息矩阵会比
后端优化构造的信息矩阵要小对于视觉，其输入的_drop_set是vector{0, 3}，也就是说其待边缘化变量是para_Pose[imu_i]和
para_Feature[feature_index]，从这里可以看出来在VINS-mono的边缘化操作中会不仅仅会边缘化第一帧相关的优化变量，
还会边缘化掉以第一帧为起始观察帧的路标点。
问1：为什么输入的_drop_set是 vector{0, 1}，也就是说其待边缘化变量是para_Pose[0], para_SpeedBias[0]；
输入的_drop_set是vector{0, 3}，也就是说其待边缘化变量是para_Pose[imu_i]和para_Feature[feature_index]；
答1：如前面所示，优化变量是按照固定的顺序排列的
序号 变量
0 para_Pose
1 para_SpeedBias
2 para_Ex_Pose
3 para_Feature
4 para_Td
问2：最后一部分为什么要重写残差和雅可比公式？公式怎么来的？
答2：看过论文的童鞋应该知道，在优化部分，视觉是加了核函数的，这样带来的问题是其对应的残差和雅可比是变的，这个应该很好理解：
比如: f(x) 的雅可比是J， 那么 ρ(f(x))的雅可比肯定变了！ ρ 是核函数！ 公式我没有推出来，还请知道的兄弟告诉一下！
经过努力，得一丝头绪！ 其中核函数如下：
第三步是将定义的residual_block_info添加到marginalization_info中，通过下面这一句
marginalization_info->addResidualBlockInfo(residual_block_info);
addResidualBlockInfo() 这个函数的实现如下：
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];//因为仅仅有地址不行，还需要有地址指向的这个数据的长度
parameter_block_size[reinterpret_cast<long>(addr)] = size;//将指针强转为数据的地址
}
//这里应该是待边缘化的变量
for (int i = 0; i < static_cast<int>(residual_block_info->drop_set.size()); i++)
{
//这个是待边缘化的变量的id
double *addr = parameter_blocks[residual_block_info->drop_set[i]];
//将需要marg的变量的id存入parameter_block_idx
parameter_block_idx[reinterpret_cast<long>(addr)] = 0;
}
}目的：
将不同损失函数对应的优化变量、边缘化位置存入到 parameter_block_sizes 和 parameter_block_idx中，这里注意的是执行到这一步，
parameter_block_idx 中仅仅有待边缘化的优化变量的内存地址的key，而且其对应value全部为0；
【4】调用 preMarginalize() 进行预处理
上面通过调用 addResidualBlockInfo() 已经确定优化变量的数量、存储位置、长度以及待优化变量的数量以及存储位置，
下面就需要调用 preMarginalize() 进行预处理， 得到每次 IMU 和视觉观测 (cost_function) 对 应的参数块(parameter_blocks)，
雅可比矩阵(jacobians)，残差值(residuals);
void MarginalizationInfo::preMarginalize()
{
//遍历所有factor，在前面的addResidualBlockInfo中会将不同的残差块加入到factor中。
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];
//将factor中的参数块复制到parameter_block_data中，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); //重新开辟一块内存
parameter_block_data[addr] = data;//通过之前优化变量的数据的地址和新开辟的内存数据进行关联
}
}
}
}it->Evaluate() 这一句里面其实就是调用各个损失函数中的重载函数Evaluate()，这个函数前面有提到过：
【5】调用marginalize()
开启多线程构建信息矩阵H和b ，同时从H,b中恢复出线性化雅克比和残差
前面已经将数据都准备好了，下面通过调用marginalize()函数就要正式开始进行边缘化操作了，实现如下：
void MarginalizationInfo::marginalize()
{
int pos = 0;
for (auto &it : parameter_block_idx)//遍历待marg的优化变量的内存地址
{
it.second = pos;
pos += localSize(parameter_block_size[it.first]);
}
m = pos;//需要marg掉的变量个数
for (const auto &it : parameter_block_size)//计算除了边缘化之外要保留的参数块
{
//如果这个变量不是是待marg的优化变量。
if (parameter_block_idx.find(it.first) == parameter_block_idx.end())
{
parameter_block_idx[it.first] = pos;//就将这个待marg的变量id设为pos
pos += localSize(it.second); //pos加上这个变量的长度
}
}//上面的操作就会将所有的优化变量进行一个伪排序
n = pos - m;//要保留下来的变量个数
if(m == 0)
{
valid = false;
printf("unstable tracking...\n");
return;
}
TicToc t_summing;
Eigen::MatrixXd A(pos, pos);//整个矩阵大小 --- 没有边缘化之前的矩阵
Eigen::VectorXd b(pos);
A.setZero();
b.setZero();
TicToc t_thread_summing;
pthread_t tids[NUM_THREADS];
ThreadsStruct threadsstruct[NUM_THREADS];//携带每个线程的输入输出信息
//为每个线程均匀分配factor。
int i = 0;
for (auto it : factors)
{
threadsstruct[i].sub_factors.push_back(it);
i++;
i = i % NUM_THREADS;
}
//构造4个线程，并确定线程的主程序
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;
//分别构造矩阵
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;
}
/*代码中求Amm的逆矩阵时，为了保证数值稳定性，做了Amm=1/2*(Amm+Amm^T)的运算，Amm本身是一个对称矩阵，所以 等式成立。接着对Amm进行了特征值分解,再求逆，更加的快速*/
Eigen::MatrixXd Amm = 0.5 * (A.block(0, 0, m, m) + A.block(0, 0, m, m).transpose());
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm);
Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd((saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() * saes.eigenvectors().transpose();
// 设x_{m}为要被marg掉的状态量，x_{r}是与x_{m}相关的状态量，所以在最后我们要保存的是x_{r}的信息
//
// | | | | |
// | Amm | Amr| m |bmm| |x_{m}|
// A = |______|____| b = |__ | A|x_{r}| = b
// | Arm | Arr| n |brr|
// | | | | |
// 使用舒尔补:
// C = Arr - Arm*Amm^{-1}Amr
// d = brr - Arm*Amm^{-1}bmm
//舒尔补，上面这段代码边缘化掉xm变量，保留xb变量
Eigen::VectorXd bmm = b.segment(0, m);
Eigen::MatrixXd Amr = A.block(0, m, m, n);//0，m是开始的位置，m,m是开始位置后的大小
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是marg过的A和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();
Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();
//分别指的是边缘化之后从信息矩阵A和b中恢复出来雅克比矩阵和残差向量；
//两者会作为先验残差带入到下一轮的先验残差的雅克比和残差的计算当中去
linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();
linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b;
}
部分程序：
int pos = 0; //pos表示所有的被marg掉的参数块以及它们的相连接参数块的localsize之和
for (auto &it : parameter_block_idx)
{
it.second = pos;
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())
{
//将被marg掉参数的相连接参数块添加道parameter_block_idx中
parameter_block_idx[it.first] = pos;
pos += localSize(it.second);
}
}
n = pos - m;这里对 参数块变量进行排序，待marg的参数块变量放在前面，其他参数块变量放在后面，
并将每个参数块的对应的下标放在parameter_block_idx中； pos = m + n
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;
}
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;
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;
}这段代码开启多线程来构建信息矩阵A和残差b；将所有的先验约束因子平均分配到NUM_THREADS个线程中，每个线程分别构建一个A和b；
函数会通过多线程快速构造各个残差对应的各个优化变量的信息矩阵（雅克比和残差前面都已经求出来了），如下图所示：
这里构造信息矩阵时采用的正是 parameter_block_idx 作为构造顺序，自然而然地将待边缘化的变量构造在矩阵的左上方；
子函数 void* ThreadsConstructA(void* threadsstruct)
void* ThreadsConstructA(void* threadsstruct)
{
ThreadsStruct* p = ((ThreadsStruct*)threadsstruct);
//遍历该线程分配的所有factors，所有观测项
for (auto it : p->sub_factors)
{
//遍历该factor中的所有参数块，五个参数块，分别计算，理解！
for (int i = 0; i < static_cast<int>(it->parameter_blocks.size()); i++)
{
//得到参数块的大小
int idx_i = p->parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[i])];
int size_i = p->parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[i])];
if (size_i == 7)//对于pose来说，是7维的,最后一维为0，这里取左边6
size_i = 6;
//只提取local size部分，对于pose来说，是7维的,最后一维为0，这里取左边6维
//P.leftCols(cols) = P(:, 1:cols)，取出从1列开始的cols列
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 = p->parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[j])];
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);
//对应对角区域，H*X=b, A代表H矩阵
if (i == j)
p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;
else
{
//对应非对角区域
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，Hx=g，都是根据公式来写程序的！
p->b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals;
}
}
return threadsstruct;
}这部分代码是用来实现构建A和b的：
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());
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;上面这段代码边缘化掉xm变量，保留xb变量，利用的方法是舒尔补
注意：上面这个等式就是先验信息，程序中又变成了 AX = b 的形式，从A和b中恢复出雅克比矩阵和残差，作为下一时刻的先验信息；
代码中求Amm的逆矩阵时，为了保证数值稳定性，做了Amm=1/2*(Amm+Amm^T)的运算，Amm本身是一个对称矩阵，
所以等式成立。接着对Amm进行了特征值分解,再求逆，更加的快速。
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(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();
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;上面这段代码是从A和b中恢复出雅克比矩阵和残差
【6】滑窗预移动
//这里仅仅将指针进行了一次移动，指针对应的数据还是旧数据，调用的 slideWindow() 才能实现真正的滑窗移动
std::unordered_map<long, double *> addr_shift;
for (int i = 1; i <= WINDOW_SIZE; i++)//从1开始，因为第一帧的状态不要了
{
//这一步的操作指的是第i的位置存放的的是i-1的内容，这就意味着窗口向前移动了一格
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
if(USE_IMU)
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];
addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];
//根据地址来得到保留的参数块
vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);
if (last_marginalization_info)//Zb
delete last_marginalization_info; //删除掉上一次的marg相关的内容
last_marginalization_info = marginalization_info; //marg相关内容的递归
last_marginalization_parameter_blocks = parameter_blocks; //优化变量的递归，这里面仅仅是指针
}这一部分应该很容易明白；当边缘化帧的 “新帧”就不说了，把上面的看懂了，估计那个也会了
/------------------------------------------------------------------程序-----------------------------------------------------------------------/
6万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
