理论知识：
SLAM中的marginalization 和 Schur complement
深入理解SLAM中的Marginalization
VINS6边缘化
DSO 中的Windowed Optimization
- 边缘化留下的先验信息有哪些？
//VINS边缘化有两个策略，如果在sliding window中第二近的frame是关键帧则丢弃sliding window中最老的帧、否则丢弃该帧。无论丢弃哪一帧，都需要边缘化。
if (marginalization_flag == MARGIN_OLD)//丢弃老的一帧。
{
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++)
{
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);//添加上一次边缘化的变量
}
{
if (pre_integrations[1]->sum_dt < 10.0) //IMU预积分留下的边缘化信息
{
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);//添加优化变量
}
}
{
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;
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);//添加优化变量
}
}
}
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];
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
{
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);
}
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];
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());
}
从代码中可以看出，某帧(最老帧或者第二新的帧)要被边缘化掉时， 该帧要被丢弃掉， 如果是最老帧，IMU预积分会给与之相邻的帧留下先验信息，该帧的观测会给与之有共视关系的帧留下先验信息，如果某帧既不是与之有IMU预积分约束的相邻帧，也没有共视点，则不会给该帧留下先验信息。如果边缘化最近的第二帧，则只留下上次的边缘化信息，IMU预积分传给下一帧，共视点的先验信息则不会留下。传到marginalization_info里的残差块与optimization时建立的残差块很类似。
- 在marginalization中加入残差块：
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();
// Cost function signature metadata: number of inputs & their 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++)
{
double *addr = parameter_blocks[residual_block_info->drop_set[i]];
parameter_block_idx[reinterpret_cast<long>(addr)] = 0; //要被marginalize掉的状态参数块
}
}
将残差块加入MarginalizationInfo， 同时将要丢弃的参数块记录下来。
-
marginalization预处理：
void MarginalizationInfo::preMarginalize()
{
for (auto it : factors)
{ //factors有IMU预积分残差块imu_factor,视觉重投影残差块ProjectionTwoFrameOneCamFactor和marginalization_factor
it->Evaluate(); //计算残差块的残差及雅克比矩阵
std::vector block_sizes = it->cost_function->parameter_block_sizes(); //比如imu_factor就涉及到前后两个相机位姿
for (int i = 0; i < static_cast(block_sizes.size()); i++)
{
long addr = reinterpret_cast(it->parameter_blocks[i]);
int size = block_sizes[i];
if (parameter_block_data.find(addr) == parameter_block_data.end())
{//如果当前parameter_block_data里没有it->parameter_blocks,就在parameter_block_data里加入该参数块
double *data = new double[size];
memcpy(data, it->parameter_blocks[i], sizeof(double) * size);
parameter_block_data[addr] = data; //addr是地址,data是地址存放的数据
}
}
}
}
对每个残差块计算残差和雅可比矩阵。 -
进行Marginalization:
void MarginalizationInfo::marginalize()
{
int pos = 0;
for (auto &it : parameter_block_idx) //要被marginalize掉的状态量:一个位姿+一堆特征点
{
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())//不在被marginalize掉的状态里的状态
{
parameter_block_idx[it.first] = pos; //此处记录了参数块将来在H,b矩阵中的位置
pos += localSize(it.second);
}
}
n = pos - m;
//ROS_INFO("marginalization, pos: %d, m: %d, n: %d, size: %d", pos, m, n, (int)parameter_block_idx.size());
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();
//multi thread
//计算A,b矩阵, 优化问题最后会转化成Ax=b, A=J.transpose()*J,b=-J.transpose()*e
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();
}
}
for( int i = NUM_THREADS - 1; i >= 0; i--)
{
pthread_join( tids[i], NULL );
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());
//TODO
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);//提取起始于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得到ceres计算最小二乘问题里需要的雅克比矩阵和残差， ceres优化需求， 否则是没必要这么做的
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;
}
首先根据参数块和要drop掉的参数块大小来确定A、b矩阵的维度，然后根据A、b矩阵的计算方法，在多线程里来计算每个参数块对应处的值，将各个参数块计算的值累加，就得到最后的A、b矩阵， 进行shur补将要丢弃的参数边缘化掉，再反求得到对应的雅克比矩阵和残差。
以上就是vins-mono里marginalization代码解析。 简明地总结一下，每个残差块会涉及到两个参数块，先求每个残差快的残差及其对应参数块的雅可比矩阵，并记住其在H矩阵、b矩阵中的位置， 而后就是根据由雅可比矩阵计算H矩阵的方法往H矩阵里填充即可。H矩阵、b矩阵计算出来进行shur补，然后再反求得出雅克比矩阵和残差，在下一次优化过程中用上该边缘化信息。
Margin_Old的情况下， 最老的一帧会被maigin掉， 观测也会被加入到margin中， 但sliding_window时只删除掉那些在之后的滑动窗口中没有观测的特征点， 而仍有观测帧，即使之前加入到margin中， 也不会被扔掉。
补充：margin过程中计算Amm_inv是最耗时的一个步骤，该步骤涉及到矩阵SVD分解，这也制约着每帧选择特征点的上限数量，优化此处的计算效率，将能极大地提高后端效率。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
