VINS-Mono源码解析（五）后端: 紧耦合优化
VINS-Mono源码解析（五）后端: 紧耦合优化
1. 优化原理
a) 优化变量
λi 是第i个特征点的第一个观测对应的 逆深度. 所以总的状态的长度为 15×N+6+M , N 为滑动窗中frame的个数, M 为特征点的个数.
b) 优化函数
优化函数有三部分,
- 第一部分是那些已经从sliding windows中去掉(marginalize)的节点和特征点构成的约束, 暂且简单的理解为marginalization得到的历史约束的prior, 是一个关于 χ 的等式约束.
- 第二部分是IMU 运动模型的误差, 每相邻的两帧IMU之间产生一个residual.
- 第三部分是视觉的误差, 单个特征点 l 在相机 cj 下的投影会产生一个residual.
注意: 下面暂时忽略掉loop closure部分的优化, 只考虑VIO的部分, 这样便于理解些.
2. ceres优化: Estimator::optimization()
整个slidingwindows优化在optimization()函数中求解, 该函数具体步骤如下:
a) 初始化ceres
创建一个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);b) 加入待优化参数项
先添加优化参数量, ceres中参数用ParameterBlock来表示,类似于g2o中的vertex, 这里的参数块有sliding windows中所有帧的para_Pose(7维) 和 para_SpeedBias(9维).
/*parameters.h*/
enum SIZE_PARAMETERIZATION
{
SIZE_POSE = 7, //7 DoF(x,y,z,qx,qy,qz,qw)
SIZE_SPEEDBIAS = 9, //9 DoF(vx,vy,vz, bas_x,bas_y,bas_z, bgs_x,bgs_y,bgs_z)
SIZE_FEATURE = 1 //1 DoF(inv_depth)
};
/*estimator.cpp*/
/*add vertex of: 1)pose, 2)speed and 3)bias of acc and gyro */
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);
}
/*add vertex of: camera extrinsic */
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");
}c) 加入误差项
代码如下, 依次加入margin项,IMU项和视觉feature项. 每一项都是一个factor, 这是ceres的使用方法, 创建一个类继承ceres::CostFunction类, 重写Evaluate()函数定义residual的计算形式. 分别对应marginalization_factor.h, imu_factor.h, projection_factor.h中的MarginalizationInfo, IMUFactor, ProjectionFactor三个类.
// add residual for prior from last marginalization
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);
}
// add residual for IMU
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]);
}
// add residual for per feature to per frame
int f_m_cnt = 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 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;
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++;
}
}误差项总结:
- Marginalization residual: 1个
- IMU residual: WINDOW_SIZE个(总长度WINDOW_SIZE+1), 每相邻两个Pose之间一个IMU residual项.
- feature residual: 观测数大于2的特征, 首次观测与后面的每次观测之间各一个residual项.
d) ceres求解
创建一个求解配置参数Option, 定义成DENSE_SCHUR(为什么不是SPARSE_SCHUR?), 优化算法用的”dog leg”, 设置最大迭代次数和最大求解时间. 创建一个求解描述Summary, 调用ceres::Solve()进行求解.
// ceres solve problem
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);求解完成之后, 还有做marginalization, 这个后面再单独讨论吧. 这里的话, 下面主要分析一下两个Factor类中的Evaluate()函数, 也就是residual定义.
3. IMU Residual
IMUFactor类的声明继承如下:
class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>模板参数的含义如下:
- 15: 残差向量的长度(包括p,v,q,ba,bg)
- 7: 第1个优化参数的长度(para_Pose[i])
- 9: 第2个优化参数的长度(para_SpeedBias[i])
- 7: 第3个优化参数的长度(para_Pose[j])
- 9: 第4个优化参数的长度(para_SpeedBias[j])
对于Evaluate输入double const *const *parameters, parameters[0], parameters[1], parameters[2], parameters[3]分别对应4个输入参数, 它们的长度依次是7,9,7,9
IMUFactor类重写Evaluate()函数输入parameter计算residual, 实际是调用IntegrationBase::evaluate()来真正计算残差.
Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
Pj, Qj, Vj, Baj, Bgj);
Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
//sqrt_info.setIdentity();
residual = sqrt_info * residual;真正IMU残差计算IntegrationBase::evaluate()的代码如下:
Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi, const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)
{
Eigen::Matrix<double, 15, 1> residuals;
Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);
Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);
Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);
Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);
Eigen::Vector3d dba = Bai - linearized_ba;
Eigen::Vector3d dbg = Bgi - linearized_bg;
// IMU预积分的结果,消除掉acc bias和gyro bias的影响, 对应IMU model中的\hat{\alpha},\hat{\beta},\hat{\gamma}
Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
// IMU项residual计算,输入参数是状态的估计值, 上面correct_delta_*是预积分值, 二者求'diff'得到residual.
residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
return residuals;
}该函数输入前后两个时刻的 P,Q,V,Ba,Bg , 根据预积分结果delta_q, delta_v, delta_p求IMU残差, 残差是一个长度为15的向量, 包括 (p,v,q,ba,bg) 共5个3维残差向量, 代码对应计算公式如下:
值得注意的是,这里的 α^bkbk+1,β^bkbk+1,γ^bkbk+1 是经过校正过bias的(根据 p,q,v 对bias的jacobian以及bias的差对预积分量进行修正,具体见上面代码,), 只有acc和gyro的噪声, 是跟bias相关的量, 跟初始时刻的速度及姿态都无关. 在优化迭代的过程中, 预积分值是不变的, 输入的状态值会被不断的更新, 然后不断的调用evaluate()计算更新后的IMU残差.
代码中residual还乘以了一个sqrt_info,这是为什么呢?
这是因为真正的优化项其实是Mahalanobis 距离: d=rTP−1r , P 是协方差, 而ceres只接受最小二乘优化, 也就是 mineTe , 所以把 P−1 做LLT分解即 LLT=P−1 , 那么 d=rTLLTr=(LTr)T(LTr) , 令 r′=LTr 作为新的优化误差, 这样就能用ceres求解了. LTr 就是代码中的sqrt_info.
Mahalanobis距离其实相当于一个残差加权, 协方差大的加权小, 协方差小的加权大, 着重优化那些比较确定的残差. 注掉的那行//sqrt_info.setIdentity();相当于不加权, 这个加权策略的效果需要对比测试才知道.
最后更新jacobian并进行numerical unstable判断, 这一块暂时没看懂, 求大神点拨~
4. Visual Residual
ProjectionFactor类的声明继承如下
class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1>
- 2: 残差长度(err_x, err_y)
- 7: 第1个优化参数pose_i的长度(para_Pose[imu_i]=(px,py,pz,qx,qy,qz,qw) )
- 7: 第2个优化参数pose_j的长度(para_Pose[imu_j])
- 7: 第3个优化参数外参的长度(para_Ex_Pose[0])
- 1: 第4个优化参数feature_inverse_depth的长度(para_Feature[feature_index])
关键计算代码如下:
double inv_dep_i = parameters[3][0];
Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i; // pt in ith camera frame
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; // pt in ith body frame
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; // pt in world frame
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);// pt in jth body frame
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); // pt in jth camera frame
Eigen::Map residual(residuals);
//reprojection error
#ifdef UNIT_SPHERE_ERROR
residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
double dep_j = pts_camera_j.z();
residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif
residual = sqrt_info * residual; 计算过程很简单,就是重投影过程:
i帧中图像中的点=>i帧相机系=>i帧body系=>world系=>j帧body系=>j帧相机系
重投影误差计算有两种, 如果是用SPHERE相机模型, 就取tangent平面上的误差, 否则就用常规的图像平面误差. 然后后面也同样乘以了一个sqrt_info转成马氏距离. 最后也是更新jacobian.
4. Marginalization
sliding windows bounding了优化问题中pose的个数, 从而防止pose和特征的个数随时间不断增加, 使得优化问题始终在一个有限的复杂度内, 不会随时间不断增长.
然而, 将pose移出windows时, 有些约束会被丢弃掉, 这样势必会导致求解的精度下降, 而且当MAV进行一些退化运动(如: 匀速运动)时, 没有历史信息做约束的话是无法求解的. 所以, 在移出位姿或特征的时候, 需要将相关联的约束转变成一个约束项作为prior放到优化问题中. 这就是marginalization要做的事情.
Marginalization数学上是用schur complement来实现, 暂时还没弄太懂, 有一些参考的博文:
1. SLAM中的marginalization 和 Schur complement
2. DSO 中的Windowed Optimization
VINS-MONO中,为了处理一些悬停的case, 引入了一个two-way marginalization, 简单来说就是:
- 如果倒数第二帧是关键帧, 则将最旧的pose移出sliding window, 也就是MARGIN_OLD,
- 如果倒数第二帧不是关键帧, 则将倒数第二帧pose移出sliding window, 也就是MARGIN_NEW.
选取关键帧的策略是视差足够大
在悬停等运动较小的情况下, 会频繁的MARGIN_NEW, 这样也就保留了那些比较旧但是视差比较大的pose. 这种情况如果一直MARGIN_OLD的话, 视觉约束不够强, 状态估计会受IMU积分误差影响, 具有较大的累积误差.
代码中对应marginalization_factor.cpp文件, 这块还没太看懂.