1. 代码
- 代码位置:
vins_estimator->src->estimator.cpp
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();
}
}
2. ceres解析求导
解析求导相比于自动求导,会使程序的速度加快。特别是对于里程计模块这种对于实时性要求很高的程序而言。
需要手动求出每次优化的残差和雅可比
因为在vins中我们知道参数块的大小,所以我在vins中使用SizeCostFunction
- 第一步是新建一个类继承SizeCostFunction
ceres::SizeCostFunction<1,1> : 前一个1:残差的维度; 后一个1:参数块的维度,因为这里只有一个参数块,所以只有一个1 - 第二步:定义完类之后需要重载Evaluate虚函数
double const* const* parameters对应各个参数块.由于各个参数块都定义为double数组,如果有好多个参数块就会被定义为数组的数组,也就是这边的双指针. - residuals是一个向量,也就是一个一维数组,所以这里定义为double *
- jacobians的大小是残差*参数块总数的矩阵,对于矩阵来讲它也是一个二维数组,所以定义为double * *
const double x = parameters[0][0] // 表示x是第0个参数块的第0个参数
LOSS Function 核函数
http://ceres-solver.org/nnls_modeling.html#instances
3. ceres李代数加法代码实现
// 由于位姿(李群)不满足正常的加法,因此需要自己定义他的加法
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;
}
4. 预积分约束残差计算
预积分约束:
- 如果仅通过IMU积分得到 b k b_k bk到 b k + 1 b_{k+1} bk+1的位姿,速度和旋转,那么(25)式是成立的.
- 但实际还会收到最小化重投影误差的约束
- 所以最小化 [ 两帧间位姿(速度/旋转)计算出的增量 - IMU预积分得到的增量 ],残差如公式(25)下部分 所示。
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;
5. 预积分雅克比计算
- 对于雅可比矩阵的第一个分块(jacobians[0] 15X7)
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();
}
}
- 对于雅可比矩阵的第三个分块(jacobians[2] 15X7)
- 对于雅可比矩阵的第二个分块(jacobians[1] 15X9)
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;
- 对于雅可比矩阵的第四个分块(jacobians[3] 15X9)
6. 视觉重投影约束
- 第i帧是滑窗中第一个观测到这个3D点的帧
- 3D点的状态量维持的是逆深度,而不是传统的三维向量
- 因为在 J T ∗ J ∗ △ x = − J ∗ f ( x ) J^T*J*△x = -J*f(x) JT∗J∗△x=−J∗f(x)中, J T ∗ J J^T*J JT∗J的维度与 △ x △x △x维度相同,而 x x x在VIO问题中维护的是滑窗中的位姿,外参以及(占比重最大的)3D点,对3D点只存储逆深度(1个维度),可以让x的维度相比维护3D点坐标(3个维度)缩小2/3,加快求解速度
- 为什么用逆深度不用深度:对于极远的点(例如天空),正深度为无穷大,逆深度则为一个很小的值.而且不会存在离相机很近的点,不存在逆深度值很大的情况
- 因为不是存储三维坐标,所以逆深度需要和第i帧绑定
- 优化变量:第i帧IMU的位姿,第j帧IMU的位姿,相机到IMU外参,3D点逆深度
λ
\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);
}
7. 滑动窗口边缘化
由于需要边缘化,所以我们要手动计算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}
- Λ a Λa Λa ={p1,p1}, Λ b Λb Λb = {p1,p2-m6}, Λ c Λc Λc={p2-m6,p2-m6}
上式==