if(result)//初始化成功
{
//先进行一次滑动窗口非线性优化,得到当前帧与第一帧的位姿
solver_flag = NON_LINEAR;
// step4:非线性优化求解vio
solveOdometry();
// step5:滑动窗口
slideWindow();
// step6: 移除无效地图点
f_manager.removeFailures();
ROS_INFO("Initialization finish!");
last_R = Rs[WINDOW_SIZE];
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
//三角化求解所有特征点的深度,并进行非线性优化
void Estimator::solveOdometry()
{
// 保证滑窗中帧数满了
if (frame_count < WINDOW_SIZE)
return;
// 其次要求初始化完成
if (solver_flag == NON_LINEAR)
{
TicToc t_tri;
// 先把应该三角化但是没有三角化的特征点三角化
f_manager.triangulate(Ps, tic, ric);
ROS_DEBUG("triangulation costs %f", t_tri.toc());
// 非线性优化
optimization();
}
}
解析求导相比于自动求导,会使程序的速度加快。特别是对于里程计模块这种对于实时性要求很高的程序而言。
ceres官网
因为在vins中我们知道参数块的大小,所以我在vins中使用SizeCostFunction
const double x = parameters[0][0] // 表示x是第0个参数块的第0个参数
http://ceres-solver.org/nnls_modeling.html#instances
// 由于位姿(李群)不满足正常的加法,因此需要自己定义他的加法
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
// 重新定义LocalParameterization局部参数块,继承以下四个函数
class PoseLocalParameterization : public ceres::LocalParameterization
{
virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const;
virtual bool ComputeJacobian(const double *x, double *jacobian) const;
// 3个平移+4个旋转(四元数)
virtual int GlobalSize() const { return 7; };
// 实际的自由度
virtual int LocalSize() const { return 6; };
};
/**
* x: 参数
* delta: 增量
* x_plus_delta:加法之后的结果
*/
bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
{
// Eigen::Map 把double类型映射为 Eigen::Vector3d 类型
Eigen::Map<const Eigen::Vector3d> _p(x); // 取了x的前三维
Eigen::Map<const Eigen::Quaterniond> _q(x + 3);// 取了x的第四维开始的四个维度,也就是四元数
Eigen::Map<const Eigen::Vector3d> dp(delta);
Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map<const Eigen::Vector3d>(delta + 3));
Eigen::Map<Eigen::Vector3d> p(x_plus_delta);
Eigen::Map<Eigen::Quaterniond> q(x_plus_delta + 3);
p = _p + dp;
// .normalized(): 只有单位四元数才能表示旋转
q = (_q * dq).normalized();
return true;
}
template <typename Derived>
static Eigen::Quaternion<typename Derived::Scalar> deltaQ(const Eigen::MatrixBase<Derived> &theta)
{
typedef typename Derived::Scalar Scalar_t;
Eigen::Quaternion<Scalar_t> dq;
Eigen::Matrix<Scalar_t, 3, 1> half_theta = theta;
half_theta /= static_cast<Scalar_t>(2.0);
dq.w() = static_cast<Scalar_t>(1.0);
dq.x() = half_theta.x();
dq.y() = half_theta.y();
dq.z() = half_theta.z();
return dq;
}
预积分约束:
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]);
// 计算给定相邻帧状态量的残差
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;
// delta_q,delta_v,delta_p:预积分量
// corrected_:补偿后的修正量
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;
// 公式(25)下半部份
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;
}
因为ceres不像g2o,没有增加信息矩阵的接口.
所以不能直接使用 e T ∗ p ∗ e e^T*p*e eT∗p∗e,而是将p分解为 L ∗ L T L*L^T L∗LT,让式子等于 e T ∗ L ∗ L T ∗ e e^T*L*L^T*e eT∗L∗LT∗e. 认为 L T ∗ e L^T*e LT∗e即为新的残差
// LLT分解,residual 还需乘以信息矩阵的sqrt_info
// 因为优化函数其实是d=r^T P^-1 r ,P表示协方差,而ceres只接受最小二乘优化
// 因此需要把P^-1做LLT分解,使d=(L^T r)^T (L^T r) = r'^T r
Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
// 这就是带有信息矩阵的残差,e = L^T*e
residual = sqrt_info * residual;
jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();
jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));
jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));
Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
//.bottomRightCorner<3, 3>():取右下3x3的小块
jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();
整体代码
// 第i帧的IMU位姿 pbi、qbi
if (jacobians[0])
{
Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
jacobian_pose_i.setZero();
jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();
jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));
#if 0
jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix();
#else
Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();
#endif
jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));
jacobian_pose_i = sqrt_info * jacobian_pose_i;
if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8)
{
ROS_WARN("numerical unstable in preintegration");
//std::cout << sqrt_info << std::endl;
//ROS_BREAK();
}
}
jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt;
jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba;
jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg;
jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix();
jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba;
jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg;
jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity();
jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity();
jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * pre_integration->delta_q).bottomRightCorner<3, 3>() * dq_dbg;
λ \lambda λ表示逆深度, 1 / λ 1/\lambda 1/λ表示深度
推导一下:
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]);
残差的计算:
如果把上面的一长串带入到残差,需要把一长串分为x,y,z三个部分。这会非常的复杂,所以我们采用链式求导法则。
以x为例,我们把残差关于待优化变量的导数分解为残差对于第j帧相机坐标系下的点的导数 × 第j帧相机坐标系下点对于各个优化变量的导数
if (jacobians[0])
{
Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
Eigen::Matrix<double, 3, 6> jaco_i;
jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();
jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);
jacobian_pose_i.leftCols<6>() = reduce * jaco_i;
jacobian_pose_i.rightCols<1>().setZero();
}
// 对第j帧的位姿 pbj,qbj
if (jacobians[1])
{
Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[1]);
Eigen::Matrix<double, 3, 6> jaco_j;
jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose();
jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j);
jacobian_pose_j.leftCols<6>() = reduce * jaco_j;
jacobian_pose_j.rightCols<1>().setZero();
}
// 对相机到IMU的外参 pbc,qbc (qic,tic)
if (jacobians[2])
{
Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_ex_pose(jacobians[2]);
Eigen::Matrix<double, 3, 6> jaco_ex;
jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity());
Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric;
jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) +
Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic));
jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex;
jacobian_ex_pose.rightCols<1>().setZero();
}
if (jacobians[3])
{
Eigen::Map<Eigen::Vector2d> jacobian_feature(jacobians[3]);
jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i);
}
由于需要边缘化,所以我们要手动计算H矩阵,而不能靠ceres帮我们自动计算
https://blog.csdn.net/heyijia0327/article/details/52822104
p是位姿,m是地图点,连线表示约束
X p 1 X_{p1} Xp1与 X m 1 X_{m1} Xm1的贡献是{p1,p1},{p1,m1},{m1,p1},{m1,m1}; X p 1 X_{p1} Xp1与 X p 2 X_{p2} Xp2的贡献是{p1,p1},{p1,p2},{p2,p1},{p2,p2}