本讲是VINS最核心部分了，前面经历了
1）视觉跟踪feature_tracker、IMU预积分integrationBase类；
2）初始化中SFM纯视觉估计滑动窗中所有帧的位姿和3D路标点深度、SFM与IMU预积分松耦合对齐求解初始化参数。
在完成前面的初始化环节后，本节将进行第3部分，基于滑动窗的紧耦合后端非线性优化：将视觉约束、IMU约束、闭环约束放到一个大的目标函数中进行非线性优化，求解出滑动窗口中所有帧的PVQ、bias等。
在视觉约束和IMU约束中，基本思想是找到优化状态向量，然后通过视觉残差和IMU测量残差分别对状态向量求导，获得视觉和IMU预积分的Jacobian和协方差矩阵。
本文将对Estimator::optimization()函数的代码进行详细讲解。
目录
-
一、VIO残差函数构建
1、需要优化的状态向量
2、目标函数
-
二、Estimator::Optimization（）函数
1、添加ceres参数块,待优化变量 AddParameterBlock
1、初始化ceres并将 (p,v,q,ba,bg) 15自由度优化变量加入 2、Cam到IMU的外参估计加入 3、滑窗内第一时刻相机到IMU时钟差加入 4、在ceres中，vevtor转double类型2、添加残差 AddResidualBlock
1、添加边缘化残差，丢弃帧的约束 MarginalizationFactor 2、添加IMU残差 IMUFactor 3、添加视觉残差 ProjectionTdFactor 4、添加闭环检测残差，计算滑动窗口中与每一个闭环关键帧的相对位姿，这个相对位置是为后面的图优化准备3、ceres求解Solver
4、边缘化处理
前言：MarginalizationInfo类
4.1、边缘化最老帧
1、将上一次先验残差项MarginalizationFactor传递给marginalization_info 2、将第0帧和第1帧间的预积分观测IMU因子IMUFactor(pre_integrations[1])，添加到marginalization_info中 3、将第一次观测为第0帧的所有路标点对应的视觉观测ProjectionFactor，添加到marginalization_info中 4、preMarginalize()函数：计算每个残差对应的Jacobian， 5、marglinalize()函数：多线程构造先验项舒尔补AX=b的结构，在X0处线性化计算Jacobian和残差 6、调整参数块在下一次窗口中对应的位置（往前移一格），注意这里是指针，后面slideWindow中会赋新值，这里只是提前占座4.2、边缘化次新帧
1、保留次新帧的IMU测量，丢弃该帧的视觉测量，将上一次先验残差项传递给marginalization_info 2、preMarginalize():计算每个残差对应的Jacobian 3、marginalize():多线程构造先验项舒尔补AX=b 4、调整参数块在下一次窗口中对应的位置（去掉次新帧） -
三、滑动窗 slideWindow()
1、 MARGIN_OLD 边缘化最老帧
1、保存最老帧信息到back_R0,back_P0 2、滑窗内0-5所有信息前移 3、第5帧情况更新 4、最老帧之前预积分和图像都删除 5、slideWindowOld()函数，首次在原来最老帧出现的特征点转移到现在现在最老帧2、MARGIN_SECOND_NEW 边缘化次新帧，但是不删除IMU约束
1、第5帧图像信息复制到第4帧上 2、更新第5帧 3、当最新一帧(5)不是关键帧时，用于merge滑窗内最新帧(4)
一、VIO残差函数构建
1、需要优化的状态向量
滑动窗口内IMU状态（PVQ、加速度bias、陀螺仪bias）、IMU到Camera的外参、m+1个3D路标点逆深度。
第一个式子是滑动窗口内所有状态量，n是关键帧数量，m是滑动窗内所有观测到的路标点总数。λi是第i个特征点的第一个观测对应的逆深度.特征点逆深度为了满足高斯系统。
第二个式子xk是在第k帧图像捕获到的IMU状态，包括位置，速度，旋转（PVQ）和加速度偏置，陀螺仪偏置。
第三个式子是相机外参。
注意：xk只与IMU项和Marg有关；特征点深度也只与camera和Marg有关；
维度是15n+6+m*
IMU状态 xk {P、V、Q、Ba、Bg} 是15维（5*3=15）；相机外参 为 6 维； 特征点逆深度为1维。
2、目标函数
优化函数有三部分：
边缘化残差：从滑动窗中去掉的节点和特征点构成的约束，形成一个先验Prior
IMU残差：相邻两帧IMU产生的residual
视觉重投影误差：单个特征点在两帧之间投影形成residual
二、Estimator::Optimization（）函数
整个滑动窗优化在optimization函数中求解。基于滑动窗口紧耦合的非线性优化，残差项的构造和求解
1.添加要优化的变量 (p,v,q,ba,bg) 一共15个自由度，Cam到IMU外参，时钟差
2.添加残差，残差项分为4块 先验残差+IMU残差+视觉残差+闭环检测残差
3.ceres求解Solve（）函数
4.边缘化操作
1、添加ceres参数块,待优化变量 AddParameterBlock
待优化变量分为三部分：IMU状态xk (p,v,q,ba,bg)、Cam到IMU外参、时钟差
1、初始化ceres并将 (p,v,q,ba,bg) 15自由度优化变量加入
创建一个ceres Problem实例, loss_function定义为CauchyLoss.柯西核函数
ceres::Problem problem;
ceres::LossFunction *loss_function;// 残差
//loss_function = new ceres::HuberLoss(1.0);
loss_function = new ceres::CauchyLoss(1.0); // 柯西核函数
先添加优化参数量, ceres中参数用ParameterBlock来表示,类似于g2o中的vertex, 这里的参数块有sliding windows中所有帧的para_Pose(7维) 和 para_SpeedBias(9维).
Ps、Rs转变成para_Pose 6自由度7DOF（x,y,z,qx,qy,qz,w），Vs、Bas、Bgs转变成para_SpeedBias 9自由度9DOF(vx,vy,vz,bax,bay,baz,bgx,bgy,bgz)
for (int i = 0; i < WINDOW_SIZE + 1; i++) // 遍历滑动窗数量
{
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
}
2、Cam到IMU的外参估计加入
camera到IMU的外参也添加到估计 para_Ex_Pose
for (int i = 0; i < NUM_OF_CAM; i++)
{
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
if (!ESTIMATE_EXTRINSIC)
{
ROS_DEBUG("fix extinsic param");
problem.SetParameterBlockConstant(para_Ex_Pose[i]);
}
else
ROS_DEBUG("estimate extinsic param");
}
3、滑窗内第一时刻相机到IMU时钟差加入
滑窗内第一个时刻相机到IMU时钟差，保证传感器同步， para_Td
if (ESTIMATE_TD)
{
problem.AddParameterBlock(para_Td[0], 1);
//problem.SetParameterBlockConstant(para_Td[0]);
}
4、在ceres中，vevtor转double类型
- 从Ps、Rs、Vs、Bas、Bgs转化为para_Pose（6维，相机位姿）和para_SpeedBias（9维，相机速度、加速度偏置、角速度偏置）
- 从tic和q转化为para_Ex_Pose （6维，Cam到IMU外参）
- 从dep到para_Feature（1维，特征点深度）
- 从td转化为para_Td（1维，标定同步时间）
void Estimator::vector2double()
{
// 1.遍历滑动窗，IMU的15个自由度的优化变量
for (int i = 0; i <= WINDOW_SIZE; i++)
{
// P、R
para_Pose[i][0] = Ps[i].x();
para_Pose[i][1] = Ps[i].y();
para_Pose[i][2] = Ps[i].z();
Quaterniond q{Rs[i]};
para_Pose[i][3] = q.x();
para_Pose[i][4] = q.y();
para_Pose[i][5] = q.z();
para_Pose[i][6] = q.w();
// V、Ba、Bg
para_SpeedBias[i][0] = Vs[i].x();
para_SpeedBias[i][1] = Vs[i].y();
para_SpeedBias[i][2] = Vs[i].z();
para_SpeedBias[i][3] = Bas[i].x();
para_SpeedBias[i][4] = Bas[i].y();
para_SpeedBias[i][5] = Bas[i].z();
para_SpeedBias[i][6] = Bgs[i].x();
para_SpeedBias[i][7] = Bgs[i].y();
para_SpeedBias[i][8] = Bgs[i].z();
}
// 2.Cam到IMU的外参，6自由度由7个参数表示
for (int i = 0; i < NUM_OF_CAM; i++)
{
para_Ex_Pose[i][0] = tic[i].x();
para_Ex_Pose[i][1] = tic[i].y();
para_Ex_Pose[i][2] = tic[i].z();
Quaterniond q{ric[i]};
para_Ex_Pose[i][3] = q.x();
para_Ex_Pose[i][4] = q.y();
para_Ex_Pose[i][5] = q.z();
para_Ex_Pose[i][6] = q.w();
}
VectorXd dep = f_manager.getDepthVector();
for (int i = 0; i < f_manager.getFeatureCount(); i++)
para_Feature[i][0] = dep(i);
// 3、保证传感器同步的时钟差
if (ESTIMATE_TD)
para_Td[0][0] = td;
}
2、添加残差 AddResidualBlock
依次加入margin项,IMU项和视觉feature项. 每一项都是一个factor, 这是ceres的使用方法, 创建一个类继承ceres::CostFunction类, 重写Evaluate()函数定义residual的计算形式. 分别对应marginalization_factor.h, imu_factor.h, projection_factor.h中的MarginalizationInfo, IMUFactor, ProjectionFactor三个类。首先会调用addResidualBlockInfo()函数将各个残差以及残差涉及的优化变量添加入上面所述的优化变量中
1、添加边缘化残差，丢弃帧的约束 MarginalizationFactor
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);
}
2、添加IMU残差 IMUFactor
for (int i = 0; i < WINDOW_SIZE; i++)
{
int j = i + 1;
if (pre_integrations[j]->sum_dt > 10.0)
continue;
IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
}
int f_m_cnt = 0;
int feature_index = -1;
3、添加视觉残差 ProjectionTdFactor
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;
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());
problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);
/*
double **para = new double *[5];
para[0] = para_Pose[imu_i];
para[1] = para_Pose[imu_j];
para[2] = para_Ex_Pose[0];
para[3] = para_Feature[feature_index];
para[4] = para_Td[0];
f_td->check(para);
*/
}
else
{
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);
}
f_m_cnt++;
}
}
ROS_DEBUG("visual measurement count: %d", f_m_cnt);
ROS_DEBUG("prepare for ceres: %f", t_prepare.toc());
4、添加闭环检测残差，计算滑动窗口中与每一个闭环关键帧的相对位姿，这个相对位置是为后面的图优化准备
if(relocalization_info)
{
//printf("set relocalization factor! \n");
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(relo_Pose, SIZE_POSE, local_parameterization);
int retrive_feature_index = 0;
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 start = it_per_id.start_frame;
if(start <= relo_frame_local_index)
{
while((int)match_points[retrive_feature_index].z() < it_per_id.feature_id)
{
retrive_feature_index++;
}
if((int)match_points[retrive_feature_index].z() == it_per_id.feature_id)
{
Vector3d pts_j = Vector3d(match_points[retrive_feature_index].x(), match_points[retrive_feature_index].y(), 1.0);
Vector3d pts_i = it_per_id.feature_per_frame[0].point;
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[start], relo_Pose, para_Ex_Pose[0], para_Feature[feature_index]);
retrive_feature_index++;
}
}
}
}
3、ceres求解Solver
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
//options.num_threads = 2;
options.trust_region_strategy_type = ceres::DOGLEG;
options.max_num_iterations = NUM_ITERATIONS;
//options.use_explicit_schur_complement = true;
//options.minimizer_progress_to_stdout = true;
//options.use_nonmonotonic_steps = true;
if (marginalization_flag == MARGIN_OLD)
options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;
else
options.max_solver_time_in_seconds = SOLVER_TIME;
TicToc t_solver;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
//cout << summary.BriefReport() << endl;
ROS_DEBUG("Iterations : %d", static_cast<int>(summary.iterations.size()));
ROS_DEBUG("solver costs: %f", t_solver.toc());
4、边缘化处理
移除位姿时将关联的约束转化为先验放入优化问题中。为了限制基于优化的VIO计算复杂度，引入边缘化。有选择地从滑动窗口中将IMU状态xK和特征点λ1边缘化，同时将对应于边缘状态的测量值转换为先验。
分为两种情况，
1、MARGIN_OLD 边缘化最老帧：一种是倒数第二帧如果是关键帧的话，将最旧的pose移出Sliding Window，
将最旧帧关联的视觉和惯性数据边缘化掉。把第一个老关键帧及其测量值被边缘化；Margin_Old作为先验值。
2、MARGIN_SECOND_NEW 边缘化次新帧：如果倒数第二帧不是关键帧的话，那么就只剔除倒数第二帧的视觉观测，
而不剔除它的IMU约束。原因是边缘化保证关键帧之间有足够视差而能够三角化足够多的地图点。
并且保证了IMU预积分的连贯性。
前言：MarginalizationInfo类
边缘化Marg类在vins_estimator/src/factor/marginalization_factor.h中
class MarginalizationInfo
{
public:
~MarginalizationInfo();
int localSize(int size) const;
int globalSize(int size) const;
// 添加残差块相关信息
void addResidualBlockInfo(ResidualBlockInfo *residual_block_info);
// 计算每个残差对应的雅克比，并更新 parameter_block_data
void preMarginalize();
// 多线程构造先验项舒尔补AX=b的结构，计算Jacobian和残差
void marginalize();
std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift);
std::vector<ResidualBlockInfo *> factors;//所有观测项
int m, n;//m为要边缘化的变量个数，n为要保留下来的变量个数
int sum_block_size;
// 1
std::unordered_map<long, int> parameter_block_size; //<优化变量内存地址,localSize>
std::unordered_map<long, int> parameter_block_idx; //<待边缘化的优化变量内存地址,在parameter_block_size中的id>
std::unordered_map<long, double *> parameter_block_data;//<优化变量内存地址,数据>
// 2
std::vector<int> keep_block_size; //global size
std::vector<int> keep_block_idx; //local size
std::vector<double *> keep_block_data;
// 3
Eigen::MatrixXd linearized_jacobians;
Eigen::VectorXd linearized_residuals;
const double eps = 1e-8;
};
主要类的成员内参为：
1、三个undorded_map相关变量分别为：
parameter_block_size、
parameter_block_idx、
parameter_block_data
他们的key都是同一long类型的优化变量的内存地址，value分别是优化变量的长度、id、以及优化变量对应的double指针
2、三个vector相关变量
keep_block_size、
keep_block_idx、
keep_block_data，
他们是进行边缘化之后保留下来的各个优化变量的长度，各个优化变量在id以各个优化变量对应的double指针类型的数据
3、两个Eigen变量
linearized_jacobians、
linearized_residuals，
分别指的是边缘化之后从信息矩阵H恢复出来雅克比矩阵和残差向量
4.1、边缘化最老帧
如果次新帧是关键帧，将边缘化最老帧，及其看到的路标点和IMU数据，将其转化为先验。
if (marginalization_flag == MARGIN_OLD)
{
MarginalizationInfo *marginalization_info = new MarginalizationInfo();
vector2double();
后面主要操作分为：
1、添加三个残差marginalization_info->addResidualBlockInfo（）：边缘化残差MarginalizationFactor、预积分残差IMUFactor、视觉重投影误差ProjectionTdFactor，三个损失函数的类都继承自ceres的损失函数类ceres::CostFunction。
2、计算每个残差对应的Jacobian，上述三个类各自对应的::Evaluate（）函数
3、主函数：构造Ax=b， marginalization_info->marginalize();
4、调整参数块在下一次窗口中对应的位置（往前移一格），注意这里是指针，后面slideWindow中会赋新值，这里只是提前占座
下面1-3操作类似，基本流程是：
1、定义三个损失函数。
三个损失函数的类都继承自ceres的损失函数类ceres::CostFunction。里面都重载了函数
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;
输入：待优化变量parameters；以及先验值（对于先验残差就是上一时刻的先验残差last_marginalization_info，对于IMU就是预计分值pre_integrations[1]，对于视觉就是空间的的像素坐标pts_i, pts_j）
输出：各项残差值residual以及残差对应各优化变量的Jacobian。
2、定义残差ResidualBlockInfo类。
这一步是为了将不同的损失函数_cost_function以及优化变量_parameter_blocks统一起来再一起添加到marginalization_info中。
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;//待边缘化的优化变量id
double **raw_jacobians;
std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobians;
Eigen::VectorXd residuals;//残差 IMU:15X1 视觉2X1
int localSize(int size)
{
return size == 7 ? 6 : size;
}
};
3、将残差添加到marginalization_info中。
marginalization_info->addResidualBlockInfo(residual_block_info);
添加残差块相关信息（优化变量，）
//添加残差块相关信息（优化变量，待边缘化变量）
void MarginalizationInfo::addResidualBlockInfo(ResidualBlockInfo *residual_block_info)
{
factors.emplace_back(residual_block_info);
// merg相关变量
std::vector<double *> ¶meter_blocks = residual_block_info->parameter_blocks;
std::vector<int> parameter_block_sizes = residual_block_info->cost_function->parameter_block_sizes();
// 遍历待优化变量parameter_blocks
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;// 将指针强转为数据的地址
}
// 待边缘化的变量drop_set
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]];//待边缘化变量的ID
parameter_block_idx[reinterpret_cast<long>(addr)] = 0;//将需要marg的变量的id存入parameter_block_idx
}
}
分别将不同损失函数对应的优化变量、边缘化位置存入到parameter_block_sizes和parameter_block_idx中，这里注意的是执行到这一步，parameter_block_idx中仅仅有待边缘化的优化变量的内存地址的key，而且其对应value全部为0。
至此，确定优化变量的数量、存储位置、长度以及待优化变量的数量以及存储位置
1、将上一次先验残差项MarginalizationFactor传递给marginalization_info
//1、将上一次先验残差项传递给marginalization_info,并去除要丢弃的状态量
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);
}
// 1.1 定义损失函数 marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
// 1.2 定义残差
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
last_marginalization_parameter_blocks,
drop_set);
// 1.3 将各个残差以及残差涉及的优化变量添加到 marginalization_info
marginalization_info->addResidualBlockInfo(residual_block_info);
}
2、将第0帧和第1帧间的预积分观测IMU因子IMUFactor(pre_integrations[1])，添加到marginalization_info中
//2、将第0帧和第1帧间的预积分观测IMU因子IMUFactor(pre_integrations[1])，添加到marginalization_info中
{
if (pre_integrations[1]->sum_dt < 10.0)
{
// 2.1 定义损失函数IMUfactor
IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
// 2.2 定义残差，优化变量为para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]，都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});
// 2.3将各个残差和优化变量添加到 marginalization_info
marginalization_info->addResidualBlockInfo(residual_block_info);
}
}
3、将第一次观测为第0帧的所有路标点对应的视觉观测ProjectionFactor，添加到marginalization_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 >= 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)
{
// 3.1定义代价函数
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,
// 3.2定义残差块 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]},
// 3.3 将各个残差和优化变量添加到 marginalization_info 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);
}
}
}
}
4、preMarginalize()函数：计算每个残差对应的Jacobian，
//4、计算每个残差对应的Jacobian，并将各参数块拷贝到统一的内存（parameter_block_data）中
marginalization_info->preMarginalize();
ROS_DEBUG("pre marginalization %f ms", t_pre_margin.toc());
void MarginalizationInfo::preMarginalize()
{
// 在前面的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];
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;
}
}
}
}
主要就是调用各个损失函数中的重载函数Evaluate（）函数。
此外，这里会给parameter_block_data赋值
5、marglinalize()函数：多线程构造先验项舒尔补AX=b的结构，在X0处线性化计算Jacobian和残差
//5、多线程构造先验项舒尔补AX=b的结构，在X0处线性化计算Jacobian和残差
marginalization_info->marginalize();
ROS_DEBUG("marginalization %f ms", t_margin.toc());
pos为所有变量维度，ｍ为需要marg掉的变量，ｎ为需要保留的变量
//多线程构造先验项舒尔补AX=b的结构，计算Jacobian和残差
// pos为所有变量维度，ｍ为需要marg掉的变量，ｎ为需要保留的变量
void MarginalizationInfo::marginalize()
{
// 1.将所有的优化变量进行一个伪排序，待marg的优化变量的idx为0，其他的和起所在的位置相关
int pos = 0;
// 遍历待marg的优化变量的内存地址
for (auto &it : parameter_block_idx)
{
it.second = pos;
pos += localSize(parameter_block_size[it.first]);
}
m = pos;// m是需要marg掉的变量个数
// 遍历所有待优化变量
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;// n是要保留下来的变量个数
//ROS_DEBUG("marginalization, pos: %d, m: %d, n: %d, size: %d", pos, m, n, (int)parameter_block_idx.size());
// 2.通过多线程快速构造各个残差对应的各个优化变量的信息矩阵
TicToc t_summing;
Eigen::MatrixXd A(pos, pos);// 整个矩阵A的大小
Eigen::VectorXd b(pos);
A.setZero();
b.setZero();
TicToc t_thread_summing;
pthread_t tids[NUM_THREADS];
ThreadsStruct threadsstruct[NUM_THREADS];
int i = 0;
// 将各个残差块的Jacoian矩阵分配到各线程中
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());
// 3.通过shur补操作进行边缘化
//舒尔补
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;
// 更新先验残差项
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;
}
第一步，秉承这map数据结构没有即添加，存在即赋值的语法，上面的代码会先补充parameter_block_idx，前面提到经过addResidualBlockInfo()函数仅仅带边缘化的优化变量在parameter_block_idx有key值，这里会将保留的优化变量的内存地址作为key值补充进去，并统一他们的value值是其前面已经放入parameter_block_idx的优化变量的维度之和，同时这里会计算出两个变量m和n，他们分别是待边缘化的优化变量的维度和以及保留的优化变量的维度和。
第二步，函数会通过多线程快速构造各个残差对应的各个优化变量的信息矩阵（雅克比和残差前面都已经求出来了），然后在加起来，如下图所示：
因为这里构造信息矩阵时采用的正是parameter_block_idx作为构造顺序，因此，就会自然而然地将待边缘化的变量构造在矩阵的左上方。
第三步，函数会通过shur补操作进行边缘化，然后再从边缘化后的信息矩阵中恢复出来雅克比矩阵linearized_jacobians和残差linearized_residuals，这两者会作为先验残差带入到下一轮的先验残差的雅克比和残差的计算当中去。
这里参考：https://blog.csdn.net/weixin_44580210/article/details/95748091
6、调整参数块在下一次窗口中对应的位置（往前移一格），注意这里是指针，后面slideWindow中会赋新值，这里只是提前占座
//6.调整参数块在下一次窗口中对应的位置（往前移一格），注意这里是指针，后面slideWindow中会赋新值，这里只是提前占座
std::unordered_map<long, double *> addr_shift;
for (int i = 1; i <= WINDOW_SIZE; i++)// 从1开始，因为第1帧状态不要
{
//第i的位置存放的的是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];
}
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;// 删除掉上一次marg相关内容
last_marginalization_info = marginalization_info;// 更新
last_marginalization_parameter_blocks = parameter_blocks;
}
值得注意的是，这里仅仅是相当于将指针进行了一次移动，指针对应的数据还是旧数据，因此需要结合后面调用的slideWindow()函数才能实现真正的滑窗移动，此外last_marginalization_info就是保留下来的先验残差信息，包括保留下来的雅克比linearized_jacobians、残差linearized_residuals、保留下来的和边缘化有关的数据长度keep_block_size、顺序keep_block_idx以及数据keep_block_data。last_marginalization_info就是保留下来的滑窗内的所有的优化变量
这里需要明确一个概念就是，边缘化操作并不会改变优化变量的值，而仅仅是改变了优化变量之间的关系，而这个关系就是通过信息矩阵体现的。
4.2、边缘化次新帧
1、保留次新帧的IMU测量，丢弃该帧的视觉测量，将上一次先验残差项传递给marginalization_info
//1.保留次新帧的IMU测量，丢弃该帧的视觉测量，将上一次先验残差项传递给marginalization_info
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);
}
2、preMarginalize():计算每个残差对应的Jacobian
TicToc t_pre_margin;
ROS_DEBUG("begin marginalization");
marginalization_info->preMarginalize();
ROS_DEBUG("end pre marginalization, %f ms", t_pre_margin.toc());
3、marginalize():多线程构造先验项舒尔补AX=b
//3、marginalize
TicToc t_margin;
ROS_DEBUG("begin marginalization");
marginalization_info->marginalize();
ROS_DEBUG("end marginalization, %f ms", t_margin.toc());
4、调整参数块在下一次窗口中对应的位置（去掉次新帧）
//4.调整参数块在下一次窗口中对应的位置（去掉次新帧）
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;
}
}
三、滑动窗 slideWindow()
移除位姿时将关联的约束转化为先验放入优化问题中。为了限制基于优化的VIO计算复杂度，引入边缘化。有选择地从滑动窗口中将IMU状态xK和特征点λ1边缘化，同时将对应于边缘状态的测量值转换为先验。
假设WINDOW_SIZE=5(代码中为10），则buffer大小为6(WINDOW_SIZE+1)，最新来的帧放到WINDOW_SIZE的位置，记作X，两种操作之后X加入到优化环节中，
1、MARGIN_OLD 边缘化最老帧：一种是倒数第二帧如果是关键帧的话，将最旧的pose移出Sliding Window，将最旧帧关联的视觉和惯性数据边缘化掉。把第一个老关键帧及其测量值被边缘化；Margin_Old作为先验值。
2、MARGIN_SECOND_NEW 边缘化次新帧：如果倒数第二帧不是关键帧的话，那么就只剔除倒数第二帧的视觉观测，而不剔除它的IMU约束。原因是边缘化保证关键帧之间有足够视差而能够三角化足够多的地图点。并且保证了IMU预积分的连贯性。
1、 MARGIN_OLD 边缘化最老帧
假设滑动窗内一共有10帧，最新一帧为第11帧图像X
if (marginalization_flag == MARGIN_OLD)
1、保存最老帧信息到back_R0,back_P0
back_R0 = Rs[0];
back_P0 = Ps[0];
2、滑窗内0-5所有信息前移
包含IMU状态量（PVQ、Ba、Bg）、IMU预积分量、线速度、角速度、dt、时间戳
for (int i = 0; i < WINDOW_SIZE; i++)// 遍历所有
{
Rs[i].swap(Rs[i + 1]);
std::swap(pre_integrations[i], pre_integrations[i + 1]);
dt_buf[i].swap(dt_buf[i + 1]);
linear_acceleration_buf[i].swap(linear_acceleration_buf[i + 1]);
angular_velocity_buf[i].swap(angular_velocity_buf[i + 1]);
Headers[i] = Headers[i + 1];
Ps[i].swap(Ps[i + 1]);
Vs[i].swap(Vs[i + 1]);
Bas[i].swap(Bas[i + 1]);
Bgs[i].swap(Bgs[i + 1]);
}
3、第5帧情况更新
// 3.1、第10帧信息给了11帧（第10、11帧相同都是新来的）
Headers[WINDOW_SIZE] = Headers[WINDOW_SIZE - 1];
Ps[WINDOW_SIZE] = Ps[WINDOW_SIZE - 1];
Vs[WINDOW_SIZE] = Vs[WINDOW_SIZE - 1];
Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1];
Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1];
Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1];
// 3.2、新实例化一个IMU预积分给到第11帧
delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};
// 3.3、清空第11帧的三个buf
dt_buf[WINDOW_SIZE].clear();
linear_acceleration_buf[WINDOW_SIZE].clear();
angular_velocity_buf[WINDOW_SIZE].clear();
4、最老帧之前预积分和图像都删除
if (true || solver_flag == INITIAL)
{
double t_0 = Headers[0].stamp.toSec();
map<double, ImageFrame>::iterator it_0;
it_0 = all_image_frame.find(t_0);// 最老帧
delete it_0->second.pre_integration;// 删除预积分信息
all_image_frame.erase(all_image_frame.begin(), it_0);// 最老帧和之前图像帧都删除
}
5、slideWindowOld()函数，首次在原来最老帧出现的特征点转移到现在现在最老帧
void Estimator::slideWindowOld()
{
// 1、统计一共多少次merg滑窗第一帧情况
sum_of_back++;
bool shift_depth = solver_flag == NON_LINEAR ? true : false;
if (shift_depth)// 非线性
{
Matrix3d R0, R1;
Vector3d P0, P1;
//back_R0、back_P0为窗口中最老帧的位姿
//Rs[0]、Ps[0]为滑动窗口后第0帧的位姿，即原来的第1帧
R0 = back_R0 * ric[0];// 滑窗原来第0帧
R1 = Rs[0] * ric[0];// 滑窗原来第1帧（现在最老的第0帧）
P0 = back_P0 + back_R0 * tic[0];
P1 = Ps[0] + Rs[0] * tic[0];
// 首次在原来最老帧出现的特征点转移到现在现在最老帧
f_manager.removeBackShiftDepth(R0, P0, R1, P1);
}
else
// 当最新一帧是关键帧，merg滑窗内最老帧
f_manager.removeBack();
}
2、MARGIN_SECOND_NEW 边缘化次新帧，但是不删除IMU约束
WINDOW_SIZE=5，则buffer大小为6(WINDOW_SIZE+1)
1、第5帧图像信息复制到第4帧上
if (frame_count == WINDOW_SIZE)
{
for (unsigned int i = 0; i < dt_buf[frame_count].size(); i++)
{
// 1.
// 取出最新一帧的信息
double tmp_dt = dt_buf[frame_count][i];
Vector3d tmp_linear_acceleration = linear_acceleration_buf[frame_count][i];
Vector3d tmp_angular_velocity = angular_velocity_buf[frame_count][i];
// 第5帧信息复制到第4帧上
dt_buf[frame_count - 1].push_back(tmp_dt);
linear_acceleration_buf[frame_count - 1].push_back(tmp_linear_acceleration);
angular_velocity_buf[frame_count - 1].push_back(tmp_angular_velocity);
// 留着IMU约束，当前帧和前一帧的IMU预积分转化为当前帧和前二帧
pre_integrations[frame_count - 1]->push_back(tmp_dt, tmp_linear_acceleration, tmp_angular_velocity);
}
// 第5帧IMU信息复制到第4帧上
Headers[frame_count - 1] = Headers[frame_count];
Ps[frame_count - 1] = Ps[frame_count];
Vs[frame_count - 1] = Vs[frame_count];
Rs[frame_count - 1] = Rs[frame_count];
Bas[frame_count - 1] = Bas[frame_count];
Bgs[frame_count - 1] = Bgs[frame_count];
2、更新第5帧
// 新实例化一个IMU预积分给到第5帧
delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};
// 清空第5帧的三个buf
dt_buf[WINDOW_SIZE].clear();
linear_acceleration_buf[WINDOW_SIZE].clear();
angular_velocity_buf[WINDOW_SIZE].clear();
3、当最新一帧(5)不是关键帧时，用于merge滑窗内最新帧(4)
void Estimator::slideWindowNew()
{
sum_of_front++;
f_manager.removeFront(frame_count);
}
3170
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
