《自动驾驶与机器人中的SLAM技术》ch4：基于预积分和图优化的 GINS

前言：预积分图优化的结构

1 预积分的图优化顶点

这里使用《自动驾驶与机器人中的SLAM技术》ch4：预积分学中提到的散装的形式来实现预积分的顶点部分，所以每个状态被分为位姿（ $R,p$ ）、速度、陀螺零偏、加计零偏四种顶点（共 15 维）。后三者实际上都是 $\mathbb{R}^3$ 的变量，可以直接使用继承来实现。g2o 部分可参考 G2O (General Graph Optimization) 。

下文中提到的顶点的维度为优化变量的维度。

1.1 旋转在前，平移在后的位姿顶点（6维）

位姿顶点的顺序为旋转在前，平移在后，雅可比矩阵的顺序要与之对应。

/*** 旋转在前，平移在后的的SO3+t类型pose，6自由度，存储时伪装为g2o::VertexSE3，供g2o_viewer查看*/
class VertexPose : public g2o::BaseVertex<6, SE3> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWVertexPose() {}// 读写函数是为了保存顶点信息到 .g2o 文件中的，该文件可使用 g2o_viewer 可视化bool read(std::istream& is) override {double data[7];for (int i = 0; i < 7; i++) {is >> data[i];}setEstimate(SE3(Quatd(data[6], data[3], data[4], data[5]), Vec3d(data[0], data[1], data[2])));return true;}bool write(std::ostream& os) const override {os << "VERTEX_SE3:QUAT ";os << id() << " ";Quatd q = _estimate.unit_quaternion();os << _estimate.translation().transpose() << " ";os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << std::endl;return true;}// 重置。这里未实现该函数的功能virtual void setToOriginImpl() {}// 更新virtual void oplusImpl(const double* update_) {// Eigen::Map<const Vec3d>(&update_[0]) ：使用 &update_[0] 获取指向数组中第 1 个元素的地址，从该地址开始，将 update_ 数组连续的 3 个元素映射为一个 Eigen::Vector3d 对象// Eigen::Map<const Vec3d>(&update_[3]) ：使用 &update_[3] 获取指向数组中第 4 个元素的地址，从该地址开始，将 update_ 数组连续的 3 个元素映射为一个 Eigen::Vector3d 对象// Eigen::Map: 这是一个模板类，用于将现有的内存块映射为 Eigen 对象。这种映射是零开销的，因为它不会复制数据，而是直接使用提供的内存。_estimate.so3() = _estimate.so3() * SO3::exp(Eigen::Map<const Vec3d>(&update_[0]));  // 旋转部分，右乘更新_estimate.translation() += Eigen::Map<const Vec3d>(&update_[3]);                     // 平移部分updateCache();}
};

1.2 速度顶点（3维）

/*** 速度顶点，单纯的Vec3d*/
class VertexVelocity : public g2o::BaseVertex<3, Vec3d> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWVertexVelocity() {}virtual bool read(std::istream& is) { return false; }virtual bool write(std::ostream& os) const { return false; }virtual void setToOriginImpl() { _estimate.setZero(); }virtual void oplusImpl(const double* update_) { _estimate += Eigen::Map<const Vec3d>(update_); }
};

1.3 陀螺仪和加速度计零偏顶点（3维）

陀螺仪零偏顶点和加速度计零偏顶点均继承自速度顶点。

/*** 陀螺零偏顶点，亦为Vec3d，从速度顶点继承*/
class VertexGyroBias : public VertexVelocity {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWVertexGyroBias() {}
};/*** 加计零偏顶点，Vec3d，亦从速度顶点继承*/
class VertexAccBias : public VertexVelocity {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWVertexAccBias() {}
};

2 预积分的图优化边

在基于预积分和图优化的 GINS 系统中，边主要有以下几种：

1. 预积分的边，约束上一时刻的 15 维状态与下一时刻的旋转、平移、速度；
2. 零偏随机游走的边，共两种，连接两个时刻的零偏状态；
3. GNSS 的观测边。因为我们使用六自由度观测，所以它关联单个时刻的位姿；
4. 先验信息，刻画上一时刻的状态分布，关联下一时刻的 15 维状态；
5. 轮速计的观测边。关联上一时刻的速度顶点。

下文中提到的边的维度为 观测值维度 。

2.1 预积分边（9维多元边）

注意：预积分边需要从外部传入预积分对象、重力矢量和权重因子（为信息矩阵添加的乘积因子）。其中需要从预积分类中获取整体预积分时间、预积分的协方差矩阵（其逆乘以权重因子作为信息矩阵）、预积分陀螺仪零偏（计算 $\delta\boldsymbol{b}_{g,i}$ ）、雅可比矩阵（ $\frac{\partial\Delta\tilde{\boldsymbol{R}}_{ij}}{\partial\boldsymbol{b}_{g,i}}, \frac{\partial\Delta\tilde{\boldsymbol{v}}_{ij}}{\partial\boldsymbol{b}_{g,i}}, \frac{\partial\Delta\tilde{\boldsymbol{v}}_{ij}}{\partial\boldsymbol{b}_{a,i}}, \frac{\partial\Delta\tilde{\boldsymbol{p}}_{ij}}{\partial\boldsymbol{b}_{g,i}}, \frac{\partial\Delta\tilde{\boldsymbol{p}}_{ij}}{\partial\boldsymbol{b}_{a,i}}$ ）和零偏更新时修正后的预积分观测量。

其中残差的定义及残差对状态变量的雅可比矩阵见《自动驾驶与机器人中的SLAM技术》ch4：预积分学中的推导。

计算预积分残差对状态变量的雅可比矩阵的结构如下图所示：

代码实现如下：

/// 与预积分相关的vertex, edge
/*** 预积分边（多元边）* 连接6个顶点：上一帧的pose, v, bg, ba，下一帧的pose, v* 观测量为9维，即预积分残差, 顺序：R, v, p* information从预积分类中获取，构造函数中计算* 旋转在前，平移在后*/
class EdgeInertial : public g2o::BaseMultiEdge<9, Vec9d> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW/*** 构造函数中需要指定预积分类对象* @param preinteg  预积分对象指针* @param gravity   重力矢量* @param weight    权重。为信息矩阵添加的乘积因子*/EdgeInertial(std::shared_ptr<IMUPreintegration> preinteg, const Vec3d& gravity, double weight = 1.0);bool read(std::istream& is) override { return false; }bool write(std::ostream& os) const override { return false; }// 计算边的误差，即预积分残差void computeError() override;// 计算雅可比矩阵void linearizeOplus() override;// 计算 Hessian 矩阵Eigen::Matrix<double, 24, 24> GetHessian() {linearizeOplus();Eigen::Matrix<double, 9, 24> J;J.block<9, 6>(0, 0) = _jacobianOplus[0];J.block<9, 3>(0, 6) = _jacobianOplus[1];J.block<9, 3>(0, 9) = _jacobianOplus[2];J.block<9, 3>(0, 12) = _jacobianOplus[3];J.block<9, 6>(0, 15) = _jacobianOplus[4];J.block<9, 3>(0, 21) = _jacobianOplus[5];return J.transpose() * information() * J;}private:const double dt_;std::shared_ptr<IMUPreintegration> preint_ = nullptr;Vec3d grav_;
};

EdgeInertial::EdgeInertial(std::shared_ptr<IMUPreintegration> preinteg, const Vec3d& gravity, double weight): preint_(preinteg), dt_(preinteg->dt_) {resize(6);  // 6个关联顶点grav_ = gravity;setInformation(preinteg->cov_.inverse() * weight);
}void EdgeInertial::computeError() {auto* p1 = dynamic_cast<const VertexPose*>(_vertices[0]);auto* v1 = dynamic_cast<const VertexVelocity*>(_vertices[1]);auto* bg1 = dynamic_cast<const VertexGyroBias*>(_vertices[2]);auto* ba1 = dynamic_cast<const VertexAccBias*>(_vertices[3]);auto* p2 = dynamic_cast<const VertexPose*>(_vertices[4]);auto* v2 = dynamic_cast<const VertexVelocity*>(_vertices[5]);Vec3d bg = bg1->estimate();Vec3d ba = ba1->estimate();// 零偏的更新（假设预积分的测量值是随零偏线性变化的，当零偏更新时，在原先预积分测量值的基础上进行修正得到新的预积分测量值）const SO3 dR = preint_->GetDeltaRotation(bg);const Vec3d dv = preint_->GetDeltaVelocity(bg, ba);const Vec3d dp = preint_->GetDeltaPosition(bg, ba);/// 预积分误差项 p112 （4.41）// 预积分残差项const Vec3d er = (dR.inverse() * p1->estimate().so3().inverse() * p2->estimate().so3()).log();Mat3d RiT = p1->estimate().so3().inverse().matrix();const Vec3d ev = RiT * (v2->estimate() - v1->estimate() - grav_ * dt_) - dv;const Vec3d ep = RiT * (p2->estimate().translation() - p1->estimate().translation() - v1->estimate() * dt_ -grav_ * dt_ * dt_ / 2) -dp;_error << er, ev, ep;
}void EdgeInertial::linearizeOplus() {// 在 setVertex() 函数中设置 void setVertex(size_t i, Vertex* v) {_vertices[i]=v;}// 顺序为 v0_pose、v0_vel、v0_bg、v0_ba、v1_pose、v1_vel（这里体现了 旋转在前，平移在后）auto* p1 = dynamic_cast<const VertexPose*>(_vertices[0]);auto* v1 = dynamic_cast<const VertexVelocity*>(_vertices[1]);auto* bg1 = dynamic_cast<const VertexGyroBias*>(_vertices[2]);auto* ba1 = dynamic_cast<const VertexAccBias*>(_vertices[3]);auto* p2 = dynamic_cast<const VertexPose*>(_vertices[4]);auto* v2 = dynamic_cast<const VertexVelocity*>(_vertices[5]);Vec3d bg = bg1->estimate();Vec3d ba = ba1->estimate();Vec3d dbg = bg - preint_->bg_;// 一些中间符号const SO3 R1 = p1->estimate().so3();const SO3 R1T = R1.inverse();const SO3 R2 = p2->estimate().so3();auto dR_dbg = preint_->dR_dbg_;auto dv_dbg = preint_->dV_dbg_;auto dp_dbg = preint_->dP_dbg_;auto dv_dba = preint_->dV_dba_;auto dp_dba = preint_->dP_dba_;// 估计值Vec3d vi = v1->estimate();Vec3d vj = v2->estimate();Vec3d pi = p1->estimate().translation();Vec3d pj = p2->estimate().translation();const SO3 dR = preint_->GetDeltaRotation(bg);const SO3 eR = SO3(dR).inverse() * R1T * R2;const Vec3d er = eR.log();// 修复 bug const Mat3d invJr = SO3::jr_inv(er);/// 雅可比矩阵/// 注意有3个index, 顶点的，自己误差的，顶点内部变量的/// 变量顺序：pose1(R1,p1), v1, bg1, ba1, pose2(R2,p2), v2/// 残差顺序：eR, ev, ep，残差顺序为行(row)，变量顺序为列(col)//       | R1 | p1 | v1 | bg1 | ba1 | R2 | p2 | v2 |//  vert | 0       | 1  | 2   | 3   | 4       | 5  |//  col  | 0    3  | 0  | 0   | 0   | 0    3  | 0  |//    row//  eR 0 |//  ev 3 |//  ep 6 |/// 残差对R1, 9x3_jacobianOplus[0].setZero();// dR/dR1, p113 (4.42)_jacobianOplus[0].block<3, 3>(0, 0) = -invJr * (R2.inverse() * R1).matrix();// dv/dR1, p115 (4.47)_jacobianOplus[0].block<3, 3>(3, 0) = SO3::hat(R1T * (vj - vi - grav_ * dt_));// dp/dR1, p115 (4.48d）_jacobianOplus[0].block<3, 3>(6, 0) = SO3::hat(R1T * (pj - pi - vi * dt_ - 0.5 * grav_ * dt_ * dt_));/// 残差对p1, 9x3// dp/dp1, p115 (4.48a)_jacobianOplus[0].block<3, 3>(6, 3) = -R1T.matrix();/// 残差对v1, 9x3_jacobianOplus[1].setZero();// dv/dv1, p114 (4.46a)_jacobianOplus[1].block<3, 3>(3, 0) = -R1T.matrix();// dp/dv1, p115 (4.48c)_jacobianOplus[1].block<3, 3>(6, 0) = -R1T.matrix() * dt_;/// 残差对bg1_jacobianOplus[2].setZero();// dR/dbg1, p114 (4.45)_jacobianOplus[2].block<3, 3>(0, 0) = -invJr * eR.inverse().matrix() * SO3::jr((dR_dbg * dbg).eval()) * dR_dbg;// dv/dbg1  p111 (4.38)前添加负号_jacobianOplus[2].block<3, 3>(3, 0) = -dv_dbg;// dp/dbg1  p111 (4.38)前添加负号_jacobianOplus[2].block<3, 3>(6, 0) = -dp_dbg;/// 残差对ba1_jacobianOplus[3].setZero();// dv/dba1  p111 (4.38)前添加负号_jacobianOplus[3].block<3, 3>(3, 0) = -dv_dba;// dp/dba1  p111 (4.38)前添加负号_jacobianOplus[3].block<3, 3>(6, 0) = -dp_dba;/// 残差对pose2_jacobianOplus[4].setZero();// dr/dr2, p114 (4.43)_jacobianOplus[4].block<3, 3>(0, 0) = invJr;// dp/dp2, p115 (4.48b)_jacobianOplus[4].block<3, 3>(6, 3) = R1T.matrix();/// 残差对v2_jacobianOplus[5].setZero();// dv/dv2, p114 (4,46b)_jacobianOplus[5].block<3, 3>(3, 0) = R1T.matrix();  // OK
}

2.2 零偏随机游走边（3维双元边）

2.2.1 陀螺仪随机游走边

残差定义：

$r_{ b_{g,ij}}=b_{g,j}-b_{g,i}$

残差对状态变量的雅可比矩阵：

$\begin{aligned} & \frac{\partial r_{ b_{g,ij}}}{\partial b_{g,i}}=-I, \\ & \frac{\partial r_{ b_{g,ij}}}{\partial b_{g,j}}=I . \end{aligned}$

/*** 陀螺仪随机游走边（双元边）*/
class EdgeGyroRW : public g2o::BaseBinaryEdge<3, Vec3d, VertexGyroBias, VertexGyroBias> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWEdgeGyroRW() {}virtual bool read(std::istream& is) { return false; }virtual bool write(std::ostream& os) const { return false; }void computeError() {// 在 setVertex() 函数中设置 void setVertex(size_t i, Vertex* v) {_vertices[i]=v;}// 顺序为 v0_bg、v1_bgconst auto* VG1 = dynamic_cast<const VertexGyroBias*>(_vertices[0]);const auto* VG2 = dynamic_cast<const VertexGyroBias*>(_vertices[1]);_error = VG2->estimate() - VG1->estimate();}virtual void linearizeOplus() {// 残差对 bg1, 3x3_jacobianOplusXi = -Mat3d::Identity();// 残差对 bg2, 3x3_jacobianOplusXj.setIdentity();}Eigen::Matrix<double, 6, 6> GetHessian() {linearizeOplus();Eigen::Matrix<double, 3, 6> J;J.block<3, 3>(0, 0) = _jacobianOplusXi;J.block<3, 3>(0, 3) = _jacobianOplusXj;return J.transpose() * information() * J;}
};

2.2.2 加速度计随机游走边

残差定义：

$r_{ b_{a,ij}}=b_{a,j}-b_{a,i}$

残差对状态变量的雅可比矩阵：

$\begin{aligned} & \frac{\partial r_{ b_{a,ij}}}{\partial b_{a,i}}=-I, \\ & \frac{\partial r_{ b_{a,ij}}}{\partial b_{a,j}}=I . \end{aligned}$

/*** 加速度计随机游走边（双元边）*/
class EdgeAccRW : public g2o::BaseBinaryEdge<3, Vec3d, VertexAccBias, VertexAccBias> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWEdgeAccRW() {}virtual bool read(std::istream& is) { return false; }virtual bool write(std::ostream& os) const { return false; }void computeError() {// 在 setVertex() 函数中设置 void setVertex(size_t i, Vertex* v) {_vertices[i]=v;}// 顺序为 v0_ba、v1_baconst auto* VA1 = dynamic_cast<const VertexAccBias*>(_vertices[0]);const auto* VA2 = dynamic_cast<const VertexAccBias*>(_vertices[1]);_error = VA2->estimate() - VA1->estimate();}virtual void linearizeOplus() {// 残差对 ba1, 3x3_jacobianOplusXi = -Mat3d::Identity();// 残差对 ba2, 3x3_jacobianOplusXj.setIdentity();}Eigen::Matrix<double, 6, 6> GetHessian() {linearizeOplus();Eigen::Matrix<double, 3, 6> J;J.block<3, 3>(0, 0) = _jacobianOplusXi;J.block<3, 3>(0, 3) = _jacobianOplusXj;return J.transpose() * information() * J;}
};

2.3 GNSS 观测边（单边）

GNSS 方案分为双天线方案和单天线方案。

2.3.1 双天线方案（6维旋转+位移）

注意：双天线 GNSS 边需要从外部传入 GNSS 测量值（ $T_{WB}$ ）。

残差定义：

$\begin{aligned} & r_{ R_{i}}=\mathrm{Log}\left(R_{gnss}^{T}R_{i}\right), \\&r_{ p_{i}}=p_{i}-p_{gnss}. \end{aligned}$

残差对状态变量的雅可比矩阵：

$\begin{aligned} & \frac{\partial r_{ R_{i}}}{\partial R_{i}}=J_r^{-1}\left({ R_{gnss}^{T}R_{i}}\right), \\ &\frac{\partial r_{ p_{i}}}{\partial p_{i}}=I. \end{aligned}$

$j$ 时刻的 GNSS 残差和雅可比矩阵同上。

/*** 6 自由度的GNSS* 误差的旋转在前，平移在后*/
class EdgeGNSS : public g2o::BaseUnaryEdge<6, SE3, VertexPose> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;EdgeGNSS() = default;EdgeGNSS(VertexPose* v, const SE3& obs) {setVertex(0, v);setMeasurement(obs);}// 这里使用的是 bag包或者 .txt数据，其中的 GNSS 数据已经被转换到车体坐标系下了，是直接对 R,p 的观测void computeError() override {// 在 setVertex() 函数中设置 void setVertex(size_t i, Vertex* v) {_vertices[i]=v;}// 顺序为 p_gnss_0 或者 p_gnss_1VertexPose* v = (VertexPose*)_vertices[0];_error.head<3>() = (_measurement.so3().inverse() * v->estimate().so3()).log();_error.tail<3>() = v->estimate().translation() - _measurement.translation();};void linearizeOplus() override {VertexPose* v = (VertexPose*)_vertices[0];// jacobian 6x6_jacobianOplusXi.setZero();_jacobianOplusXi.block<3, 3>(0, 0) = (_measurement.so3().inverse() * v->estimate().so3()).jr_inv();  // dR/dR_jacobianOplusXi.block<3, 3>(3, 3) = Mat3d::Identity();                                              // dp/dp}Mat6d GetHessian() {linearizeOplus();return _jacobianOplusXi.transpose() * information() * _jacobianOplusXi;}virtual bool read(std::istream& in) { return true; }virtual bool write(std::ostream& out) const { return true; }private:
};

2.3.2 单天线方案（3维位移）

注意：单天线 GNSS 边需要从外部传入传感器外参（ $T_{BG}$ ）和 GNSS 测量值（此时是 $T_{WG}$ 而不是双天线的 $T_{WB}$ ）。

残差定义：

$\begin{aligned} &r_{ p_{i}}=p_{i}*T_{BG}-p_{gnss}. \end{aligned}$

残差对状态变量的雅可比矩阵：

$\begin{aligned} & \frac{\partial r_{ p_{i}}}{\partial p_{i}}=I. \end{aligned}$

$j$ 时刻的 GNSS 残差和雅可比矩阵同上。

/*** 只有平移的GNSS* 此时需要提供RTK外参 TBG，才能正确施加约束*/
class EdgeGNSSTransOnly : public g2o::BaseUnaryEdge<3, Vec3d, VertexPose> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;/*** 指定位姿顶点、RTK观测 t_WG、外参TGB* @param v* @param obs*/EdgeGNSSTransOnly(VertexPose* v, const Vec3d& obs, const SE3& TBG) : TBG_(TBG) {setVertex(0, v);setMeasurement(obs);}void computeError() override {// 在 setVertex() 函数中设置 void setVertex(size_t i, Vertex* v) {_vertices[i]=v;}// 顺序为 p_gnss_0 或者 p_gnss_1VertexPose* v = (VertexPose*)_vertices[0];// RTK 读数为 T_WG_error = (v->estimate() * TBG_).translation() - _measurement;};// 可以由用户提供雅可比矩阵的解析解；或者省略 linearizeOplus() 函数使用 g2o 默认的数值方式自动计算雅可比矩阵的数值解，但效率较低并且可能会引入误差// void linearizeOplus() override {//     VertexPose* v = (VertexPose*)_vertices[0];//     // jacobian 6x6//     _jacobianOplusXi.setZero();//     _jacobianOplusXi.block<3, 3>(0, 0) = (_measurement.so3().inverse() * v->estimate().so3()).jr_inv();  // dR/dR//     _jacobianOplusXi.block<3, 3>(3, 3) = Mat3d::Identity();                                              // dp/dp// }virtual bool read(std::istream& in) { return true; }virtual bool write(std::ostream& out) const { return true; }private:SE3 TBG_;
};

2.4 先验因子边（15维多元边）

注意：先验信息边需要从外部传入 上一轮优化（ $i-n$ 时刻到 $i$ 时刻）得到的 $i$ 时刻关键帧状态（ $R,p,v,b_g,b_a$ ，这里加上下标 $last\underline{~}state$ 区分）和先验信息矩阵（上一轮优化完成后，利用边缘化（对 Hessian 矩阵）的方法，求出的 $i$ 时刻关键帧状态的协方差矩阵，作为当前轮优化时 $i$ 时刻状态的先验因子使用）。

残差定义：

$\begin{aligned} & r_{ R_{i}}=\mathrm{Log}\left(R_{last\underline{~}state}^{T}R_{i}\right), \\&r_{ p_{i}}=p_{i}-p_{last\underline{~}state}, \\&r_{ v_{i}}=v_{i}-v_{last\underline{~}state}, \\& r_{ b_{g,i}}=b_{g,i}-b_{g,last\underline{~}state}, \\& r_{ b_{a,i}}=b_{a,i}-b_{a,last\underline{~}state}. \end{aligned}$

残差对状态变量的雅可比矩阵：

$\begin{aligned} & \frac{\partial r_{ R_{i}}}{\partial R_{i}}=J_r^{-1}(r_{ R_{i}}), \\ &\frac{\partial r_{ p_{i}}}{\partial p_{i}}=I, \\ &\frac{\partial r_{ v_{i}}}{\partial v_{i}}=I, \\ &\frac{\partial r_{ b_{g,i}}}{\partial b_{g,i}}=I, \\ & \frac{\partial r_{ b_{a,i}}}{\partial b_{a,i}}=I . \end{aligned}$

/*** 先验信息边（多元变）* 对上一帧IMU pvq bias的先验* info 由外部指定，通过时间窗口边缘化给出** 顶点顺序：pose, v, bg, ba* 残差顺序：R, p, v, bg, ba，15维* 残差的旋转在前，平移在后*/
class EdgePriorPoseNavState : public g2o::BaseMultiEdge<15, Vec15d> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWEdgePriorPoseNavState(const NavStated& state, const Mat15d& info);virtual bool read(std::istream& is) { return false; }virtual bool write(std::ostream& os) const { return false; }void computeError();virtual void linearizeOplus();Eigen::Matrix<double, 15, 15> GetHessian() {linearizeOplus();Eigen::Matrix<double, 15, 15> J;J.block<15, 6>(0, 0) = _jacobianOplus[0];J.block<15, 3>(0, 6) = _jacobianOplus[1];J.block<15, 3>(0, 9) = _jacobianOplus[2];J.block<15, 3>(0, 12) = _jacobianOplus[3];return J.transpose() * information() * J;}NavStated state_;
};

EdgePriorPoseNavState::EdgePriorPoseNavState(const NavStated& state, const Mat15d& info) {resize(4);state_ = state;setInformation(info);
}void EdgePriorPoseNavState::computeError() {// 在 setVertex() 函数中设置 void setVertex(size_t i, Vertex* v) {_vertices[i]=v;}// 顺序为 v0_pose、v0_vel、v0_bg、v0_baauto* vp = dynamic_cast<const VertexPose*>(_vertices[0]);auto* vv = dynamic_cast<const VertexVelocity*>(_vertices[1]);auto* vg = dynamic_cast<const VertexGyroBias*>(_vertices[2]);auto* va = dynamic_cast<const VertexAccBias*>(_vertices[3]);const Vec3d er = SO3(state_.R_.matrix().transpose() * vp->estimate().so3().matrix()).log();const Vec3d ep = vp->estimate().translation() - state_.p_;const Vec3d ev = vv->estimate() - state_.v_;const Vec3d ebg = vg->estimate() - state_.bg_;const Vec3d eba = va->estimate() - state_.ba_;_error << er, ep, ev, ebg, eba;
}void EdgePriorPoseNavState::linearizeOplus() {const auto* vp = dynamic_cast<const VertexPose*>(_vertices[0]);const Vec3d er = SO3(state_.R_.matrix().transpose() * vp->estimate().so3().matrix()).log();//       | R1 | p1 | v1 | bg1 | ba1 |//  vert | 0       | 1  | 2   | 3   |//  col  | 0    3  | 0  | 0   | 0   |//    row//  eR 0 |//  ep 3 |//  ev 6 |// ebg 9 |// eba 12|/// 注意有3个index, 顶点的，自己误差的，顶点内部变量的_jacobianOplus[0].setZero();_jacobianOplus[0].block<3, 3>(0, 0) = SO3::jr_inv(er);    // dr/dr_jacobianOplus[0].block<3, 3>(3, 3) = Mat3d::Identity();  // dp/dp_jacobianOplus[1].setZero();_jacobianOplus[1].block<3, 3>(6, 0) = Mat3d::Identity();  // dv/dv_jacobianOplus[2].setZero();_jacobianOplus[2].block<3, 3>(9, 0) = Mat3d::Identity();  // dbg/dbg_jacobianOplus[3].setZero();_jacobianOplus[3].block<3, 3>(12, 0) = Mat3d::Identity();  // dba/dba
}

2.5 轮速计边（3维单元边）

注意：轮速计边需要从外部传入经过转换后的 $v_{odom}$ （世界坐标系下），ODOM 观测的是车体坐标系下的车头朝向的速度 $v_{b,x}$ 。

残差定义：

$\begin{aligned} &r_{ v_{j}}=v_{j}*-v_{odom}. \end{aligned}$

残差对状态变量的雅可比矩阵：

$\begin{aligned} & \frac{\partial r_{ v_{j}}}{\partial v_{j}}=I. \end{aligned}$

/*** 3维 轮速计观测边* 轮速观测世界速度在自车坐标系下矢量, 3维情况下假设自车不会有y和z方向速度*/
class EdgeEncoder3D : public g2o::BaseUnaryEdge<3, Vec3d, VertexVelocity> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;EdgeEncoder3D() = default;/*** 构造函数需要知道世界系下速度* @param v0* @param speed*/EdgeEncoder3D(VertexVelocity* v0, const Vec3d& speed) {setVertex(0, v0);setMeasurement(speed);}void computeError() override {VertexVelocity* v0 = (VertexVelocity*)_vertices[0];_error = v0->estimate() - _measurement;}void linearizeOplus() override { _jacobianOplusXi.setIdentity(); }virtual bool read(std::istream& in) { return true; }virtual bool write(std::ostream& out) const { return true; }
};

3 实现基于预积分和图优化的 GINS

最后我们利用前面定义的图优化顶点和边，来实现一个类似于 ESKF 的 GNSS 惯导融合定位。读者也可借这个实验来更深入地考察图优化与滤波器之间的异同。这个基于图优化的 GINS 系统逻辑和 ESKF 大体相同，一样需要静态的 IMU 初始化来确定初始的 IMU 零偏与重力方向。我们把这些逻辑处理放到一个单独的类中，重点关注这个图优化模型是如何构建的。它的基本逻辑如下：

1. 使用 IMU 静止初始化算法来获取初始的零偏（ $b_g,b_a$ ）和重力方向（ $g$ ），随后初始化各类信息矩阵（ $b_g$ 、 $b_a$ 、GNSS、ODOM、先验因子的信息矩阵），然后使用首个带姿态的 GNSS 数据来获取初始位置与姿态，初始速度为 0，初始的零偏等于预积分类中的零偏，即 IMU 静止初始化得到的零偏。如果 IMU 和 GNSS 都有效，就开始进行预测和优化过程。
2. 当 IMU 数据到达时，使用预积分器来累计 IMU 的积分信息。
3. 当 ODOM 数据到达时，我们将它记录为最近时刻的速度观测并保留它的读数。
4. 在 GNSS 数据到达时，先使用 last_imu_ 数据预积分到 GNSS 时刻。
5. 使用 IMU 预积分的预测值来作为优化的初始值（ $j$ 时刻优化的初始值，代码中为 this_frame_），构建前一个时刻的 GNSS 与当前时刻的 GNSS 间的图优化问题。当然也可以用 GNSS 的观测值来计算，两种方式的优化初值会不太一样，但在本例中结果相似。该问题的节点和边定义如下：

• 节点：前一时刻与当前时刻的位姿、速度、两个零偏，共 8 个顶点。

• 边：两个时刻间的预积分观测边，两个时刻的 GNSS 的观测边，前一个时刻的先验边，两个零偏随机游走边，速度观测边。这样一共有 7 个边。

3.1 代码实现

1. 使用 IMU 静止初始化算法来获取初始的零偏（ $b_g,b_a$ ）和重力方向（ $g$ ），随后初始化各类信息矩阵（ $b_g$ 、 $b_a$ 、GNSS、ODOM、先验因子的信息矩阵），然后使用首个带姿态的 GNSS 数据来获取初始位置与姿态，初始速度为 0，初始的零偏等于预积分类中的零偏，即 IMU 静止初始化得到的零偏。如果 IMU 和 GNSS 都有效，就开始进行预测和优化过程。

IMU 静止初始化代码如下：

run_gins_pre_integ.cc 主函数/// IMU 处理函数// IMU 静止初始化if (!imu_init.InitSuccess()) {imu_init.AddIMU(imu);return;}

IMU 静止初始化完成后，使用静止初始化得到的 $b_g,b_a,g$ 初始化预积分类的 $b_g,b_a,g$ ，然后初始化各类信息矩阵（ $b_g$ 、 $b_a$ 、GNSS、ODOM、先验因子）：

run_gins_pre_integ.cc 主函数/// 需要IMU初始化if (!imu_inited) {// 读取初始零偏，设置GINSsad::GinsPreInteg::Options options;options.preinteg_options_.init_bg_ = imu_init.GetInitBg();options.preinteg_options_.init_ba_ = imu_init.GetInitBa();options.gravity_ = imu_init.GetGravity();gins.SetOptions(options);imu_inited = true;std::cout << "imu_inited = true" <<std::endl;std::cout << "imu time:" << std::fixed << std::setprecision(8) << imu.timestamp_ << std::endl;return;}

初始化各类信息矩阵的代码如下（ $b_g$ 、 $b_a$ 、GNSS、ODOM、先验因子），其中预积分边的信息矩阵由预积分类中的协方差矩阵之逆初始化：

void GinsPreInteg::SetOptions(sad::GinsPreInteg::Options options) {options_ = options;// 设置 bg 的信息矩阵double bg_rw2 = 1.0 / (options_.bias_gyro_var_ * options_.bias_gyro_var_);options_.bg_rw_info_.diagonal() << bg_rw2, bg_rw2, bg_rw2;// 设置 ba 的信息矩阵double ba_rw2 = 1.0 / (options_.bias_acce_var_ * options_.bias_acce_var_);options_.ba_rw_info_.diagonal() << ba_rw2, ba_rw2, ba_rw2;// 设置 GNSS 的信息矩阵double gp2 = options_.gnss_pos_noise_ * options_.gnss_pos_noise_;double gh2 = options_.gnss_height_noise_ * options_.gnss_height_noise_;double ga2 = options_.gnss_ang_noise_ * options_.gnss_ang_noise_;options_.gnss_info_.diagonal() << 1.0 / ga2, 1.0 / ga2, 1.0 / ga2, 1.0 / gp2, 1.0 / gp2, 1.0 / gh2;// 初始化 IMU预积分类 的 实例对象pre_integ_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);// 设置 ODOM 的信息矩阵double o2 = 1.0 / (options_.odom_var_ * options_.odom_var_);options_.odom_info_.diagonal() << o2, o2, o2;// 先验因子的信息矩阵prior_info_.block<6, 6>(9, 9) = Mat6d ::Identity() * 1e6;if (this_frame_) {this_frame_->bg_ = options_.preinteg_options_.init_bg_;this_frame_->ba_ = options_.preinteg_options_.init_ba_;}
}

当首个带姿态的 GNSS 数据到来时，使用该数据初始化初始位姿，初始速度为 0，初始的零偏等于预积分类中的零偏，即 IMU 静止初始化得到的零偏：

void GinsPreInteg::AddGnss(const GNSS& gnss) {this_frame_ = std::make_shared<NavStated>(current_time_);this_gnss_ = gnss;if (!first_gnss_received_) {if (!gnss.heading_valid_) {// 要求首个GNSS必须有航向return;}std::cout << "first gnss time:" << std::fixed << std::setprecision(8) << gnss.unix_time_ << std::endl;// 首个gnss信号，将初始pose设置为该gnss信号this_frame_->timestamp_ = gnss.unix_time_;this_frame_->p_ = gnss.utm_pose_.translation();this_frame_->R_ = gnss.utm_pose_.so3();this_frame_->v_.setZero();this_frame_->bg_ = options_.preinteg_options_.init_bg_;this_frame_->ba_ = options_.preinteg_options_.init_ba_;pre_integ_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);last_frame_ = this_frame_;last_gnss_ = this_gnss_;first_gnss_received_ = true;current_time_ = gnss.unix_time_;return;}//* 省略 *// 
}

2. 当 IMU 数据到达时，使用预积分器来累计 IMU 的积分信息。

void GinsPreInteg::AddImu(const IMU& imu) {if (first_gnss_received_ && first_imu_received_) {std::cout << "---IMU 预积分" << std::endl;pre_integ_->Integrate(imu, imu.timestamp_ - last_imu_.timestamp_);}first_imu_received_ = true;last_imu_ = imu;current_time_ = imu.timestamp_;
}

3. 当 ODOM 数据到达时，我们将它记录为最近时刻的速度观测并保留它的读数。

void GinsPreInteg::AddOdom(const sad::Odom& odom) {last_odom_ = odom;last_odom_set_ = true;
}

4. 在 GNSS 数据到达时，先使用 last_imu_ 数据预积分到 GNSS 时刻。这里 last_imu_ 比较特殊，该数据使用了两次，这是第二次使用（第一次是前面的预积分），使用该数据预积分到当前 GNSS 的时刻：

void GinsPreInteg::AddGnss(const GNSS& gnss) {this_frame_ = std::make_shared<NavStated>(current_time_);this_gnss_ = gnss;//* 省略 *//// 积分到GNSS时刻// 这部分为什么可以积分到GNSS时刻 ???// 这个特殊的 last_imu_ 数据使用了两次，这是第二次使用（第一次是前面的预积分）。预积分到当前GNSS的时刻pre_integ_->Integrate(last_imu_, gnss.unix_time_ - current_time_);current_time_ = gnss.unix_time_;// last_frame_ 和 this_frame_，使用 IMU 预积分的预测值来作为优化的初始值（this_frame_）*this_frame_ = pre_integ_->Predict(*last_frame_, options_.gravity_);Optimize();last_frame_ = this_frame_;last_gnss_ = this_gnss_;
}

5. 使用 IMU 预积分的预测值来作为优化的初始值（ $j$ 时刻优化的初始值，代码中为 this_frame_），构建前一个时刻的 GNSS 与当前时刻的 GNSS 间的图优化问题。

注意：①优化完成后将优化后的值重新赋值给 last_frame_ 和 this_frame_，并且使用优化后的 $b_g,b_a$ 重置预积分实例对象。②这里没有先验因子的信息矩阵进行更新处理，后续第 8 章会探究先验因子信息矩阵的设置及更新。

void GinsPreInteg::Optimize() {if (pre_integ_->dt_ < 1e-3) {// 未得到积分return;}// 创建可变尺寸的 BlockSolverusing BlockSolverType = g2o::BlockSolverX;using LinearSolverType = g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType>;auto* solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));g2o::SparseOptimizer optimizer;optimizer.setAlgorithm(solver);// 上时刻顶点， pose, v, bg, baauto v0_pose = new VertexPose();v0_pose->setId(0);v0_pose->setEstimate(last_frame_->GetSE3());optimizer.addVertex(v0_pose);auto v0_vel = new VertexVelocity();v0_vel->setId(1);v0_vel->setEstimate(last_frame_->v_);optimizer.addVertex(v0_vel);auto v0_bg = new VertexGyroBias();v0_bg->setId(2);v0_bg->setEstimate(last_frame_->bg_);optimizer.addVertex(v0_bg);auto v0_ba = new VertexAccBias();v0_ba->setId(3);v0_ba->setEstimate(last_frame_->ba_);optimizer.addVertex(v0_ba);// 本时刻顶点，pose, v, bg, ba// this_frame_ 是从 起始点last_frame_ 开始通过 IMU预积分 得到的预测值，在这里作为图优化的初始值。auto v1_pose = new VertexPose();v1_pose->setId(4);v1_pose->setEstimate(this_frame_->GetSE3());optimizer.addVertex(v1_pose);auto v1_vel = new VertexVelocity();v1_vel->setId(5);v1_vel->setEstimate(this_frame_->v_);optimizer.addVertex(v1_vel);auto v1_bg = new VertexGyroBias();v1_bg->setId(6);v1_bg->setEstimate(this_frame_->bg_);optimizer.addVertex(v1_bg);auto v1_ba = new VertexAccBias();v1_ba->setId(7);v1_ba->setEstimate(this_frame_->ba_);optimizer.addVertex(v1_ba);// 预积分边auto edge_inertial = new EdgeInertial(pre_integ_, options_.gravity_);edge_inertial->setVertex(0, v0_pose);edge_inertial->setVertex(1, v0_vel);edge_inertial->setVertex(2, v0_bg);edge_inertial->setVertex(3, v0_ba);edge_inertial->setVertex(4, v1_pose);edge_inertial->setVertex(5, v1_vel);auto* rk = new g2o::RobustKernelHuber();rk->setDelta(200.0);edge_inertial->setRobustKernel(rk);optimizer.addEdge(edge_inertial);// 两个零偏随机游走auto* edge_gyro_rw = new EdgeGyroRW();edge_gyro_rw->setVertex(0, v0_bg);edge_gyro_rw->setVertex(1, v1_bg);edge_gyro_rw->setInformation(options_.bg_rw_info_);optimizer.addEdge(edge_gyro_rw);auto* edge_acc_rw = new EdgeAccRW();edge_acc_rw->setVertex(0, v0_ba);edge_acc_rw->setVertex(1, v1_ba);edge_acc_rw->setInformation(options_.ba_rw_info_);optimizer.addEdge(edge_acc_rw);// 上时刻先验auto* edge_prior = new EdgePriorPoseNavState(*last_frame_, prior_info_);edge_prior->setVertex(0, v0_pose);edge_prior->setVertex(1, v0_vel);edge_prior->setVertex(2, v0_bg);edge_prior->setVertex(3, v0_ba);optimizer.addEdge(edge_prior);// GNSS边auto edge_gnss0 = new EdgeGNSS(v0_pose, last_gnss_.utm_pose_);edge_gnss0->setInformation(options_.gnss_info_);optimizer.addEdge(edge_gnss0);auto edge_gnss1 = new EdgeGNSS(v1_pose, this_gnss_.utm_pose_);edge_gnss1->setInformation(options_.gnss_info_);optimizer.addEdge(edge_gnss1);// Odom边EdgeEncoder3D* edge_odom = nullptr;Vec3d vel_world = Vec3d::Zero();Vec3d vel_odom = Vec3d::Zero();if (last_odom_set_) {// velocity obsdouble velo_l =options_.wheel_radius_ * last_odom_.left_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;double velo_r =options_.wheel_radius_ * last_odom_.right_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;double average_vel = 0.5 * (velo_l + velo_r);vel_odom = Vec3d(average_vel, 0.0, 0.0);vel_world = this_frame_->R_ * vel_odom;edge_odom = new EdgeEncoder3D(v1_vel, vel_world);edge_odom->setInformation(options_.odom_info_);optimizer.addEdge(edge_odom);// 重置odom数据到达标志位，等待最新的odom数据last_odom_set_ = false;}optimizer.setVerbose(options_.verbose_);optimizer.initializeOptimization();optimizer.optimize(20);if (options_.verbose_) {// 获取结果，统计各类误差LOG(INFO) << "chi2/error: ";LOG(INFO) << "preintegration: " << edge_inertial->chi2() << "/" << edge_inertial->error().transpose();// LOG(INFO) << "gnss0: " << edge_gnss0->chi2() << ", " << edge_gnss0->error().transpose();LOG(INFO) << "gnss1: " << edge_gnss1->chi2() << ", " << edge_gnss1->error().transpose();LOG(INFO) << "bias: " << edge_gyro_rw->chi2() << "/" << edge_acc_rw->error().transpose();LOG(INFO) << "prior: " << edge_prior->chi2() << "/" << edge_prior->error().transpose();if (edge_odom) {LOG(INFO) << "body vel: " << (v1_pose->estimate().so3().inverse() * v1_vel->estimate()).transpose();LOG(INFO) << "meas: " << vel_odom.transpose();LOG(INFO) << "odom: " << edge_odom->chi2() << "/" << edge_odom->error().transpose();}}last_frame_->R_ = v0_pose->estimate().so3();last_frame_->p_ = v0_pose->estimate().translation();last_frame_->v_ = v0_vel->estimate();last_frame_->bg_ = v0_bg->estimate();last_frame_->ba_ = v0_ba->estimate();this_frame_->R_ = v1_pose->estimate().so3();this_frame_->p_ = v1_pose->estimate().translation();this_frame_->v_ = v1_vel->estimate();this_frame_->bg_ = v1_bg->estimate();this_frame_->ba_ = v1_ba->estimate();// 重置integ// 更新 bg、ba，重置预积分options_.preinteg_options_.init_bg_ = this_frame_->bg_;options_.preinteg_options_.init_ba_ = this_frame_->ba_;pre_integ_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);
}

最后运行基于图优化的GINS：

    /// 设置各类回调函数io.SetIMUProcessFunc([&](const sad::IMU& imu) {if (imu_init.InitSuccess()){std::cout << "IMU 处理函数!    " << "imu_inited:" << imu_inited << "    gnss_inited:" << gnss_inited << std::endl;}/// IMU 处理函数// IMU 静止初始化if (!imu_init.InitSuccess()) {imu_init.AddIMU(imu);return;}/// 需要IMU初始化if (!imu_inited) {// 读取初始零偏，设置GINSsad::GinsPreInteg::Options options;options.preinteg_options_.init_bg_ = imu_init.GetInitBg();options.preinteg_options_.init_ba_ = imu_init.GetInitBa();options.gravity_ = imu_init.GetGravity();gins.SetOptions(options);imu_inited = true;std::cout << "imu_inited = true" <<std::endl;std::cout << "imu time:" << std::fixed << std::setprecision(8) << imu.timestamp_ << std::endl;return;}if (!gnss_inited) {/// 等待有效的RTK数据return;}// 与 ESKF 不同之处！！！  /// GNSS 也接收到之后，再开始进行预测gins.AddImu(imu);auto state = gins.GetState();save_result(fout, state);if (ui) {ui->UpdateNavState(state);usleep(5e2);}}).SetGNSSProcessFunc([&](const sad::GNSS& gnss) {if (imu_init.InitSuccess()){std::cout << "GNSS 处理函数!    " << "imu_inited:" << imu_inited << "    gnss_inited:" << gnss_inited << std::endl;}/// GNSS 处理函数if (!imu_inited) {return;}// heading_valid_ 数据由 GNSS 构造函数确定sad::GNSS gnss_convert = gnss;if (!sad::ConvertGps2UTM(gnss_convert, antenna_pos, FLAGS_antenna_angle) || !gnss_convert.heading_valid_) {return;}/// 去掉原点if (!first_gnss_set) {origin = gnss_convert.utm_pose_.translation();first_gnss_set = true;std::cout << "gnss_inited = true" <<std::endl;std::cout << "gnss time:" << std::fixed << std::setprecision(8) << gnss_convert.unix_time_ << std::endl;}gnss_convert.utm_pose_.translation() -= origin;// 与 ESKF 不同之处！！！gins.AddGnss(gnss_convert);auto state = gins.GetState();save_result(fout, state);if (ui) {ui->UpdateNavState(state);usleep(1e3);}gnss_inited = true;}).SetOdomProcessFunc([&](const sad::Odom& odom) {if (imu_init.InitSuccess()){std::cout << "ODOM 处理函数!    " << "imu_inited:" << imu_inited << "    gnss_inited:" << gnss_inited << std::endl;}imu_init.AddOdom(odom);if (imu_inited && gnss_inited) {// 与 ESKF 不同之处！！！gins.AddOdom(odom);}}).Go();

3.2 运行结果

IMU 静止初始化结果：

I0118 16:14:15.636049 40021 static_imu_init.cc:86] mean acce: -0.612774 0.0203702 009.80743
I0118 16:14:15.636137 40021 static_imu_init.cc:109] IMU 初始化成功，初始化时间= 9.99001, bg = -0.000306222 00.000156394 -8.89245e-06, ba = -0.00103346 3.43548e-05 000.0165404, gyro sq = 05.5781e-07 4.46347e-07 7.20772e-07, acce sq = 03.5829e-05 3.60427e-05 4.34496e-05, grav = 000.611741 -0.0203359 00-9.79089, norm: 9.81
I0118 16:14:15.636152 40021 static_imu_init.cc:113] mean gyro: -0.000306222 00.000156394 -8.89245e-06 acce: -0.00103346 3.43548e-05 000.0165404

某一轮优化过程：

GNSS 处理函数!    imu_inited:1    gnss_inited:1
iteration= 0	 chi2= 11.939758	 time= 2.537e-05	 cumTime= 2.537e-05	 edges= 7	 schur= 0	 lambda= 6666674.355996	 levenbergIter= 1
iteration= 1	 chi2= 11.937747	 time= 1.0831e-05	 cumTime= 3.6201e-05	 edges= 7	 schur= 0	 lambda= 4444449.570664	 levenbergIter= 1
iteration= 2	 chi2= 11.934778	 time= 1.069e-05	 cumTime= 4.6891e-05	 edges= 7	 schur= 0	 lambda= 2962966.380443	 levenbergIter= 1
iteration= 3	 chi2= 11.930420	 time= 1.0512e-05	 cumTime= 5.7403e-05	 edges= 7	 schur= 0	 lambda= 1975310.920295	 levenbergIter= 1
iteration= 4	 chi2= 11.924066	 time= 1.0654e-05	 cumTime= 6.8057e-05	 edges= 7	 schur= 0	 lambda= 1213094.557098	 levenbergIter= 1
iteration= 5	 chi2= 11.914094	 time= 1.074e-05	 cumTime= 7.8797e-05	 edges= 7	 schur= 0	 lambda= 549789.382063	 levenbergIter= 1
iteration= 6	 chi2= 11.893194	 time= 1.048e-05	 cumTime= 8.9277e-05	 edges= 7	 schur= 0	 lambda= 183263.127354	 levenbergIter= 1
iteration= 7	 chi2= 11.834372	 time= 1.0341e-05	 cumTime= 9.9618e-05	 edges= 7	 schur= 0	 lambda= 61087.709118	 levenbergIter= 1
iteration= 8	 chi2= 11.669598	 time= 1.0288e-05	 cumTime= 0.000109906	 edges= 7	 schur= 0	 lambda= 20362.569706	 levenbergIter= 1
iteration= 9	 chi2= 11.239074	 time= 1.0601e-05	 cumTime= 0.000120507	 edges= 7	 schur= 0	 lambda= 6787.523235	 levenbergIter= 1
iteration= 10	 chi2= 10.292144	 time= 1.0354e-05	 cumTime= 0.000130861	 edges= 7	 schur= 0	 lambda= 2262.507745	 levenbergIter= 1
iteration= 11	 chi2= 8.562370	 time= 1.046e-05	 cumTime= 0.000141321	 edges= 7	 schur= 0	 lambda= 754.169248	 levenbergIter= 1
iteration= 12	 chi2= 5.795209	 time= 1.0682e-05	 cumTime= 0.000152003	 edges= 7	 schur= 0	 lambda= 251.389749	 levenbergIter= 1
iteration= 13	 chi2= 3.136536	 time= 1.0396e-05	 cumTime= 0.000162399	 edges= 7	 schur= 0	 lambda= 83.796583	 levenbergIter= 1
iteration= 14	 chi2= 2.276110	 time= 1.071e-05	 cumTime= 0.000173109	 edges= 7	 schur= 0	 lambda= 27.932194	 levenbergIter= 1
iteration= 15	 chi2= 2.216088	 time= 1.0856e-05	 cumTime= 0.000183965	 edges= 7	 schur= 0	 lambda= 9.310731	 levenbergIter= 1
iteration= 16	 chi2= 2.215426	 time= 1.0525e-05	 cumTime= 0.00019449	 edges= 7	 schur= 0	 lambda= 6.207154	 levenbergIter= 1
iteration= 17	 chi2= 2.215425	 time= 1.051e-05	 cumTime= 0.000205	 edges= 7	 schur= 0	 lambda= 4.138103	 levenbergIter= 1
iteration= 18	 chi2= 2.215425	 time= 1.0608e-05	 cumTime= 0.000215608	 edges= 7	 schur= 0	 lambda= 2.758735	 levenbergIter= 1
iteration= 19	 chi2= 2.215425	 time= 1.0528e-05	 cumTime= 0.000226136	 edges= 7	 schur= 0	 lambda= 1.839157	 levenbergIter= 1
I0118 16:15:22.336377 40077 gins_pre_integ.cc:240] chi2/error: 
I0118 16:15:22.336380 40077 gins_pre_integ.cc:241] preintegration: 0.00249247/05.70395e-08 -2.69427e-08 -6.17484e-07 -0.000170989 -9.10803e-05 09.26133e-07 -1.31586e-05 -6.98776e-06 07.65849e-08
I0118 16:15:22.336386 40077 gins_pre_integ.cc:243] gnss1: 0.0543654, 00.00120472 -0.00234341 00.00125066 000.0156347 -0.00404241 -0.00189687
I0118 16:15:22.336392 40077 gins_pre_integ.cc:244] bias: 0/0 0 0
I0118 16:15:22.336395 40077 gins_pre_integ.cc:245] prior: 1.69277/000.00167301 0-0.00260842 00-0.0103468 -7.47095e-05 00.000265061 00.000486168 000-0.128167 00-0.0195928 00.000358022 -1.37342e-07 0001.857e-07 06.22816e-07 01.71906e-06 09.29327e-07 00-9.148e-09
I0118 16:15:22.336407 40077 gins_pre_integ.cc:247] body vel: 0000.612737 000.0214819 -0.00161771
I0118 16:15:22.336410 40077 gins_pre_integ.cc:248] meas: 0.584907 00000000 00000000
I0118 16:15:22.336413 40077 gins_pre_integ.cc:249] odom: 0.405799/00000.031448 000.00505169 -1.74854e-05

实时运行结果：

3.3 总结

3.3.1 图优化和 ESKF 的区别

1. 相比 ESKF，基于预积分的图优化方案可以累积 IMU 读数。累积多少时间，或者每次迭代优化取多少次，都可以人为选择。而 ESKF 默认只能迭代一次，预测也只依据单个时刻的IMU 数据。
2. 预积分边（或者用因子图优化的方法，称 IMU 因子或预积分因子）是一个很灵活的因子。它关联的六个顶点都可以发生变化。为了保持状态不发生随意改变，预积分因子通常要配合其他因子一起使用。在我们的案例中，两端的 GNSS 因子可以限制位姿的变化，ODOM 因子可以限制速度的改变，两个零偏因子会限制零偏的变化量，但不限制零偏的绝对值。
3. 先验因子会让整个估计变得更平滑。严格来说，先验因子的协方差矩阵还需要使用边缘化来操作。因为本章主要介绍预积分原理，所以我们给先验因子设定了固定大小的信息矩阵，来简化程序中的一些实现。本书的第 8 章中，我们会谈论先验因子信息矩阵的设定和代码实现方式。读者可以尝试去除本因子，看看轨迹估计会产生什么影响。
4. 图优化让我们很方便地设置核函数，回顾各个因子占据的误差大小，进而确定优化过程主要受哪一部分影响。例如，我们可以分析正常情况下 RTK 观测应该产生多少残差，而异常情况下应该产生多少残差，从而确定 GNSS 是否给出了正确的位姿读数。后面我们还会向读者介绍如何来控制优化流程以实现更鲁棒的效果。读者可以打开本程序的调试输出，查看这些信息。
5. 由于引入了更多计算，图优化的耗时明显要高于滤波器方案。不过，智能汽车的算力相比以往有了明显的增加，目前图优化在一些实时计算里也可以很好地使用了。

3.3.2 总结

本章介绍了预积分的基本原理，包括它的观测模型、噪声模型、雅可比推导方式以及针对零偏的处理方式。读者在实践当中也可以灵活应用预积分：

若不考虑优化，那么预积分和直接积分完全等同；预积分可以用于预测后续状态。
用于优化时，预积分可以方便地建模两帧间的相对运动。如果固定 IMU 零偏，还可以大幅简化预积分模型。如果考虑零偏，那么需要针对零偏的更新，来更新预积分的观测；
预积分模型可以很容易地与其他图优化模型进行融合，在同一个问题中进行优化。也可以很方便地设置积分时间、优化帧数等参数，相比于滤波器方案更加自由。