广东网站建设方案报价,做好网站 怎么要版权,制作视频网站教程,设置wordpress首页显示文章摘要前言#xff1a;预积分图优化的结构 1 预积分的图优化顶点 这里使用 《自动驾驶与机器人中的SLAM技术》ch4#xff1a;预积分学 中提到的散装的形式来实现预积分的顶点部分#xff0c;所以每个状态被分为位姿#xff08;#xff09;、速度、陀螺零偏、加计零偏四种顶点预积分图优化的结构 1 预积分的图优化顶点 这里使用 《自动驾驶与机器人中的SLAM技术》ch4预积分学 中提到的散装的形式来实现预积分的顶点部分所以每个状态被分为位姿、速度、陀螺零偏、加计零偏四种顶点共 15 维。后三者实际上都是 的变量可以直接使用继承来实现。g2o 部分可参考 G2O (General Graph Optimization) 。 下文中提到的顶点的维度为 优化变量的维度。 1.1 旋转在前平移在后的位姿顶点6维 位姿顶点的顺序为旋转在前平移在后雅可比矩阵的顺序要与之对应。
/*** 旋转在前平移在后的的SO3t类型pose6自由度存储时伪装为g2o::VertexSE3供g2o_viewer查看*/
class VertexPose : public g2o::BaseVertex6, 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::Mapconst Vec3d(update_[0]) 使用 update_[0] 获取指向数组中第 1 个元素的地址从该地址开始将 update_ 数组连续的 3 个元素映射为一个 Eigen::Vector3d 对象// Eigen::Mapconst Vec3d(update_[3]) 使用 update_[3] 获取指向数组中第 4 个元素的地址从该地址开始将 update_ 数组连续的 3 个元素映射为一个 Eigen::Vector3d 对象// Eigen::Map: 这是一个模板类用于将现有的内存块映射为 Eigen 对象。这种映射是零开销的因为它不会复制数据而是直接使用提供的内存。_estimate.so3() _estimate.so3() * SO3::exp(Eigen::Mapconst Vec3d(update_[0])); // 旋转部分右乘更新_estimate.translation() Eigen::Mapconst Vec3d(update_[3]); // 平移部分updateCache();}
}; 1.2 速度顶点3维
/*** 速度顶点单纯的Vec3d*/
class VertexVelocity : public g2o::BaseVertex3, 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::Mapconst 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维多元边 注意预积分边需要从外部传入 预积分对象、重力矢量和权重因子为信息矩阵添加的乘积因子。其中需要从预积分类中获取 整体预积分时间、预积分的协方差矩阵其逆乘以权重因子作为信息矩阵、预积分陀螺仪零偏计算 、雅可比矩阵和零偏更新时修正后的预积分观测量。 其中残差的定义及残差对状态变量的雅可比矩阵见 《自动驾驶与机器人中的SLAM技术》ch4预积分学 中的推导。 计算 预积分残差对状态变量的雅可比矩阵 的结构如下图所示 代码实现如下
/// 与预积分相关的vertex, edge
/*** 预积分边多元边* 连接6个顶点上一帧的pose, v, bg, ba下一帧的pose, v* 观测量为9维即预积分残差, 顺序R, v, p* information从预积分类中获取构造函数中计算* 旋转在前平移在后*/
class EdgeInertial : public g2o::BaseMultiEdge9, Vec9d {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW/*** 构造函数中需要指定预积分类对象* param preinteg 预积分对象指针* param gravity 重力矢量* param weight 权重。为信息矩阵添加的乘积因子*/EdgeInertial(std::shared_ptrIMUPreintegration 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::Matrixdouble, 24, 24 GetHessian() {linearizeOplus();Eigen::Matrixdouble, 9, 24 J;J.block9, 6(0, 0) _jacobianOplus[0];J.block9, 3(0, 6) _jacobianOplus[1];J.block9, 3(0, 9) _jacobianOplus[2];J.block9, 3(0, 12) _jacobianOplus[3];J.block9, 6(0, 15) _jacobianOplus[4];J.block9, 3(0, 21) _jacobianOplus[5];return J.transpose() * information() * J;}private:const double dt_;std::shared_ptrIMUPreintegration preint_ nullptr;Vec3d grav_;
};
EdgeInertial::EdgeInertial(std::shared_ptrIMUPreintegration 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_castconst VertexPose*(_vertices[0]);auto* v1 dynamic_castconst VertexVelocity*(_vertices[1]);auto* bg1 dynamic_castconst VertexGyroBias*(_vertices[2]);auto* ba1 dynamic_castconst VertexAccBias*(_vertices[3]);auto* p2 dynamic_castconst VertexPose*(_vertices[4]);auto* v2 dynamic_castconst 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_castconst VertexPose*(_vertices[0]);auto* v1 dynamic_castconst VertexVelocity*(_vertices[1]);auto* bg1 dynamic_castconst VertexGyroBias*(_vertices[2]);auto* ba1 dynamic_castconst VertexAccBias*(_vertices[3]);auto* p2 dynamic_castconst VertexPose*(_vertices[4]);auto* v2 dynamic_castconst 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].block3, 3(0, 0) -invJr * (R2.inverse() * R1).matrix();// dv/dR1, p115 (4.47)_jacobianOplus[0].block3, 3(3, 0) SO3::hat(R1T * (vj - vi - grav_ * dt_));// dp/dR1, p115 (4.48d_jacobianOplus[0].block3, 3(6, 0) SO3::hat(R1T * (pj - pi - vi * dt_ - 0.5 * grav_ * dt_ * dt_));/// 残差对p1, 9x3// dp/dp1, p115 (4.48a)_jacobianOplus[0].block3, 3(6, 3) -R1T.matrix();/// 残差对v1, 9x3_jacobianOplus[1].setZero();// dv/dv1, p114 (4.46a)_jacobianOplus[1].block3, 3(3, 0) -R1T.matrix();// dp/dv1, p115 (4.48c)_jacobianOplus[1].block3, 3(6, 0) -R1T.matrix() * dt_;/// 残差对bg1_jacobianOplus[2].setZero();// dR/dbg1, p114 (4.45)_jacobianOplus[2].block3, 3(0, 0) -invJr * eR.inverse().matrix() * SO3::jr((dR_dbg * dbg).eval()) * dR_dbg;// dv/dbg1 p111 (4.38)前添加负号_jacobianOplus[2].block3, 3(3, 0) -dv_dbg;// dp/dbg1 p111 (4.38)前添加负号_jacobianOplus[2].block3, 3(6, 0) -dp_dbg;/// 残差对ba1_jacobianOplus[3].setZero();// dv/dba1 p111 (4.38)前添加负号_jacobianOplus[3].block3, 3(3, 0) -dv_dba;// dp/dba1 p111 (4.38)前添加负号_jacobianOplus[3].block3, 3(6, 0) -dp_dba;/// 残差对pose2_jacobianOplus[4].setZero();// dr/dr2, p114 (4.43)_jacobianOplus[4].block3, 3(0, 0) invJr;// dp/dp2, p115 (4.48b)_jacobianOplus[4].block3, 3(6, 3) R1T.matrix();/// 残差对v2_jacobianOplus[5].setZero();// dv/dv2, p114 (4,46b)_jacobianOplus[5].block3, 3(3, 0) R1T.matrix(); // OK
} 2.2 零偏随机游走边3维双元边 2.2.1 陀螺仪随机游走边 残差定义 残差对状态变量的雅可比矩阵 /*** 陀螺仪随机游走边双元边*/
class EdgeGyroRW : public g2o::BaseBinaryEdge3, 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_castconst VertexGyroBias*(_vertices[0]);const auto* VG2 dynamic_castconst VertexGyroBias*(_vertices[1]);_error VG2-estimate() - VG1-estimate();}virtual void linearizeOplus() {// 残差对 bg1, 3x3_jacobianOplusXi -Mat3d::Identity();// 残差对 bg2, 3x3_jacobianOplusXj.setIdentity();}Eigen::Matrixdouble, 6, 6 GetHessian() {linearizeOplus();Eigen::Matrixdouble, 3, 6 J;J.block3, 3(0, 0) _jacobianOplusXi;J.block3, 3(0, 3) _jacobianOplusXj;return J.transpose() * information() * J;}
}; 2.2.2 加速度计随机游走边 残差定义 残差对状态变量的雅可比矩阵 /*** 加速度计随机游走边双元边*/
class EdgeAccRW : public g2o::BaseBinaryEdge3, 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_castconst VertexAccBias*(_vertices[0]);const auto* VA2 dynamic_castconst VertexAccBias*(_vertices[1]);_error VA2-estimate() - VA1-estimate();}virtual void linearizeOplus() {// 残差对 ba1, 3x3_jacobianOplusXi -Mat3d::Identity();// 残差对 ba2, 3x3_jacobianOplusXj.setIdentity();}Eigen::Matrixdouble, 6, 6 GetHessian() {linearizeOplus();Eigen::Matrixdouble, 3, 6 J;J.block3, 3(0, 0) _jacobianOplusXi;J.block3, 3(0, 3) _jacobianOplusXj;return J.transpose() * information() * J;}
}; 2.3 GNSS 观测边单边 GNSS 方案分为双天线方案和单天线方案。 2.3.1 双天线方案6维 旋转位移 注意双天线 GNSS 边需要从外部传入 GNSS 测量值。 残差定义 残差对状态变量的雅可比矩阵 时刻的 GNSS 残差和雅可比矩阵同上。
/*** 6 自由度的GNSS* 误差的旋转在前平移在后*/
class EdgeGNSS : public g2o::BaseUnaryEdge6, 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.head3() (_measurement.so3().inverse() * v-estimate().so3()).log();_error.tail3() v-estimate().translation() - _measurement.translation();};void linearizeOplus() override {VertexPose* v (VertexPose*)_vertices[0];// jacobian 6x6_jacobianOplusXi.setZero();_jacobianOplusXi.block3, 3(0, 0) (_measurement.so3().inverse() * v-estimate().so3()).jr_inv(); // dR/dR_jacobianOplusXi.block3, 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 边需要从外部传入 传感器外参 和 GNSS 测量值此时是 而不是双天线的 。 残差定义 残差对状态变量的雅可比矩阵 时刻的 GNSS 残差和雅可比矩阵同上。
/*** 只有平移的GNSS* 此时需要提供RTK外参 TBG才能正确施加约束*/
class EdgeGNSSTransOnly : public g2o::BaseUnaryEdge3, 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.block3, 3(0, 0) (_measurement.so3().inverse() * v-estimate().so3()).jr_inv(); // dR/dR// _jacobianOplusXi.block3, 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维多元边 注意先验信息边需要从外部传入 上一轮优化 时刻到 时刻得到的 时刻关键帧状态这里加上下标 区分和 先验信息矩阵上一轮优化完成后利用边缘化对 Hessian 矩阵的方法求出的 时刻关键帧状态的协方差矩阵作为当前轮优化时 时刻状态的先验因子使用。 残差定义 残差对状态变量的雅可比矩阵 /*** 先验信息边多元变* 对上一帧IMU pvq bias的先验* info 由外部指定通过时间窗口边缘化给出** 顶点顺序pose, v, bg, ba* 残差顺序R, p, v, bg, ba15维* 残差的旋转在前平移在后*/
class EdgePriorPoseNavState : public g2o::BaseMultiEdge15, 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::Matrixdouble, 15, 15 GetHessian() {linearizeOplus();Eigen::Matrixdouble, 15, 15 J;J.block15, 6(0, 0) _jacobianOplus[0];J.block15, 3(0, 6) _jacobianOplus[1];J.block15, 3(0, 9) _jacobianOplus[2];J.block15, 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_castconst VertexPose*(_vertices[0]);auto* vv dynamic_castconst VertexVelocity*(_vertices[1]);auto* vg dynamic_castconst VertexGyroBias*(_vertices[2]);auto* va dynamic_castconst 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_castconst 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].block3, 3(0, 0) SO3::jr_inv(er); // dr/dr_jacobianOplus[0].block3, 3(3, 3) Mat3d::Identity(); // dp/dp_jacobianOplus[1].setZero();_jacobianOplus[1].block3, 3(6, 0) Mat3d::Identity(); // dv/dv_jacobianOplus[2].setZero();_jacobianOplus[2].block3, 3(9, 0) Mat3d::Identity(); // dbg/dbg_jacobianOplus[3].setZero();_jacobianOplus[3].block3, 3(12, 0) Mat3d::Identity(); // dba/dba
} 2.5 轮速计边3维单元边 注意轮速计边需要从外部传入 经过转换后的 世界坐标系下ODOM 观测的是车体坐标系下的车头朝向的速度 。 残差定义 残差对状态变量的雅可比矩阵 /*** 3维 轮速计观测边* 轮速观测世界速度在自车坐标系下矢量, 3维情况下假设自车不会有y和z方向速度*/
class EdgeEncoder3D : public g2o::BaseUnaryEdge3, 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 静止初始化算法来获取初始的零偏和重力方向随后初始化各类信息矩阵、、GNSS、ODOM、先验因子的信息矩阵然后使用首个带姿态的 GNSS 数据来获取初始位置与姿态初始速度为 0初始的零偏等于预积分类中的零偏即 IMU 静止初始化得到的零偏。如果 IMU 和 GNSS 都有效就开始进行预测和优化过程。2. 当 IMU 数据到达时使用预积分器来累计 IMU 的积分信息。3. 当 ODOM 数据到达时我们将它记录为最近时刻的速度观测并保留它的读数。4. 在 GNSS 数据到达时先使用 last_imu_ 数据预积分到 GNSS 时刻。5. 使用 IMU 预积分的预测值来作为优化的初始值 时刻优化的初始值代码中为 this_frame_构建前一个时刻的 GNSS 与当前时刻的 GNSS 间的图优化问题。当然也可以用 GNSS 的观测值来计算两种方式的优化初值会不太一样但在本例中结果相似。该问题的节点和边定义如下 • 节点前一时刻与当前时刻的位姿、速度、两个零偏共 8 个顶点。 • 边两个时刻间的预积分观测边两个时刻的 GNSS 的观测边前一个时刻的先验边两个零偏随机游走边速度观测边。这样一共有 7 个边。 3.1 代码实现
1. 使用 IMU 静止初始化算法来获取初始的零偏和重力方向随后初始化各类信息矩阵、、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 静止初始化完成后 使用静止初始化得到的 初始化预积分类的 然后初始化各类信息矩阵、、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;} 初始化各类信息矩阵的代码如下、、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_sharedIMUPreintegration(options_.preinteg_options_);// 设置 ODOM 的信息矩阵double o2 1.0 / (options_.odom_var_ * options_.odom_var_);options_.odom_info_.diagonal() o2, o2, o2;// 先验因子的信息矩阵prior_info_.block6, 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_sharedNavStated(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_sharedIMUPreintegration(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_sharedNavStated(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 预积分的预测值来作为优化的初始值 时刻优化的初始值代码中为 this_frame_构建前一个时刻的 GNSS 与当前时刻的 GNSS 间的图优化问题。 注意①优化完成后将优化后的值重新赋值给 last_frame_ 和 this_frame_并且使用优化后的 重置预积分实例对象。②这里没有先验因子的信息矩阵进行更新处理后续第 8 章会探究先验因子信息矩阵的设置及更新。
void GinsPreInteg::Optimize() {if (pre_integ_-dt_ 1e-3) {// 未得到积分return;}// 创建可变尺寸的 BlockSolverusing BlockSolverType g2o::BlockSolverX;using LinearSolverType g2o::LinearSolverEigenBlockSolverType::PoseMatrixType;auto* solver new g2o::OptimizationAlgorithmLevenberg(g2o::make_uniqueBlockSolverType(g2o::make_uniqueLinearSolverType()));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_sharedIMUPreintegration(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 零偏还可以大幅简化预积分模型。如果考虑零偏那么需要针对零偏的更新来更新预积分的观测预积分模型可以很容易地与其他图优化模型进行融合在同一个问题中进行优化。也可以很方便地设置积分时间、优化帧数等参数相比于滤波器方案更加自由。
