参考：
https://www.cnblogs.com/feifanrensheng/p/10532918.html
优化过程的五个步骤：
1 imu直接积分预测新进来的帧的位置
2.三角化整个特征点
3.ceres优化
4.滑动窗口
5.去除深度值为负的特征点
在优化完成后，滑动窗口前，必须要做的一件事情必须进行边缘化项的操作：
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);
}
从旧的先验残差项last_marginalization_info新建一个新的marginalization_factor
参与这个残差项的优化变量是：last_marginalization_parameter_blocks 里面的内容
当次新帧为关键帧时，MARGIN_OLD，将 marg 掉最老帧，及其看到的路标点和相关联 的 IMU 数据，将其转化为先验信息加到整体的目标函数中:
MarginalizationInfo *marginalization_info = new MarginalizationInfo();
- 把上一次先验项中的残差项(尺寸为 n)传递给当前先验项，并从中去除需要丢弃 的状态量;
if (last_marginalization_info)
{
vector<int> drop_set;
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);
}
- 将滑窗内第 0 帧和第 1 帧间的 IMU 预积分因子(pre_integrations[1])放到 marginalization_info 中，即图 中上半部分中 x0 和 x1 之间的表示 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);
}
}
- 挑选出第一次观测帧为第 0 帧的路标点，将对应的多组视觉观测放到 marginalization_info 中，即图 中上半部分中 x0 所看到的红色五角星的路标点;
{
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;
if (imu_i != 0)
continue;
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);
}
}
}
}
- marginalization_info->preMarginalize():得到每次 IMU 和视觉观测(cost_function)对 应的参数块(parameter_blocks)，雅可比矩阵(jacobians)，残差值(residuals);
marginalization_info->preMarginalize();
- marginalization_info->marginalize():多线程计整个先验项的参数块，雅可比矩阵和 残差值，其中与代码对应关系为:
marginalization_info->marginalize();
最后移交了优化项需要得到的两个变量：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）特征点
整个流程：：
通过marginalization_factor，imu_factor， ProjectionTdFactor *f_td，三个残差项，构建出三个residual_block_info，最终添加进marginalization_info里边。
最终通过last_marginalization_info 构建出marginalization_factor，这个factor就是先验的残差项。
现在来看比较重要的这三个结构体：
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();
ceres::CostFunction *cost_function;
ceres::LossFunction *loss_function;
std::vector<double *> parameter_blocks; //优化变量数据
std::vector<int> drop_set; //待marg的优化变量id
double **raw_jacobians; //Jacobian
std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobians;
Eigen::VectorXd residuals; //残差，IMU:15×1,视觉:2×1
int localSize(int size)
{
return size == 7 ? 6 : size;
}
};
class MarginalizationInfo
{
public:
~MarginalizationInfo();
int localSize(int size) const;
int globalSize(int size) const;
void addResidualBlockInfo(ResidualBlockInfo *residual_block_info); //计算每个残差对应的Jacobian，并更新parameter_block_data
void preMarginalize(); //加残差块相关信息(优化变量、待marg的变量)
void marginalize();//pos为所有变量维度，m为需要marg掉的变量，n为需要保留的变量
std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift);
std::vector<ResidualBlockInfo *> factors; //所有观测项
int m, n;
//m为要marg掉的变量个数，也就是parameter_block_idx的总localSize，以double为单位，VBias为9，PQ为6
//n为要保留下的优化变量的变量个数，n=localSize(parameter_block_size) – m
//<优化变量内存地址，localSize>
std::unordered_map<long, int> parameter_block_size; //global size
int sum_block_size;
std::unordered_map<long, int> parameter_block_idx; //local size
//<待marg的优化变量内存地址，在 //parameter_block_size中的id,以double为单位>
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;
};
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;
};