网站 当前时间 代码,洪涛怎么样海城市建设网站,看上去高端的网站,菜鸟必读 网站被入侵后需做的检测 2目录 1 预积分的定义 2 预积分的测量模型 ( 预积分的测量值可由 IMU 的测量值积分得到 ) 2.1 旋转部分 2.2 速度部分 2.3 平移部分 2.4 将预积分测量和误差式代回最初的定义式 3 预积分的噪声模型和协方差矩阵 3.1 旋转部分 3.2 速度部分 3.3 平移部分 3.4 噪声项合并 4 零偏的…目录 1 预积分的定义 2 预积分的测量模型 ( 预积分的测量值可由 IMU 的测量值积分得到 ) 2.1 旋转部分 2.2 速度部分 2.3 平移部分 2.4 将预积分测量和误差式代回最初的定义式 3 预积分的噪声模型和协方差矩阵 3.1 旋转部分 3.2 速度部分 3.3 平移部分 3.4 噪声项合并 4 零偏的更新 5 图优化中的预积分边 5.1 预积分边的残差 5.2 预积分残差相对于状态变量的雅可比矩阵 5.2.1 旋转部分 5.2.2 速度部分 5.2.3 平移部分 6 预积分流程 7 预积分的程序实现 7.1 程序实现 7.2 测试 1 测试在恒定角速度运转下的预积分情况 2. 测试在恒定加速度运行下的预积分情况 在基于 ESKF 的 GINS 中我们将两个 GNSS 观测之间的 IMU 数据进行积分作为 ESKF 的预测过程。这种做法把 IMU 数据看成某种一次性的使用方式将它们积分到当前状态估计值上然后用观测数据更新当时的状态估计值。这种做法和此时的状态估计值有关。但是如果状态量发生了改变能否重复利用这些 IMU 数据呢从物理意义上看IMU 反映的是两个时刻间车辆的角度变化量和速度变化量。如果我们希望 IMU 的计算与当时的状态估计值无关在算法上应该如何处理呢这就是本章要讨论的内容。 本章介绍一种十分常见的 IMU 数据处理方法预积分Pre-integraion。与传统 IMU 的运动学积分不同预积分可以将一段时间内的 IMU 测量数据累计起来建立预积分测量同时还能保证测量值与状态变量无关。如果以吃饭来比喻的话ESKF 像是一口口地吃菜而预积分则是从锅里先把菜一块块夹到碗里然后再把碗里的菜一口气吃掉。至于用多大的碗每次夹多少次菜再一起吃形式上就比较自由了。无论是 LIO 系统还是 VIO 系统预积分已经成为诸多与 IMU 紧耦合的标准方法但是原理相对传统 ESKF 的预测过程会更加复杂一些。下面我们来推导其基本原理然后实现一个预积分系统。 1 预积分的定义 在一个 IMU 系统里我们考虑它的五个变量旋转 R、平移 p、角速度 ω、线速度 v 与加速度 a。根据第 2 章介绍的运动学这些变量的运动学关系可以写成如下运动学方程 在 到 时间内对运动学方程进行欧拉积分得 IMU 测量方程其中 为 IMU 测量的高斯噪声 如下 IMU 测量方程带入积分后的运动学方程其中 为离散化后的 IMU 测量噪声 如下 其中噪声项满足 以上过程与我们在 IMU 测量方程和噪声方程第 3 章中已有描述。当然我们完全可以用这种约束来构建图优化对 IMU 相关的问题进行求解。但是这组方程刻画的时间太短仅包含单个 IMU 数据。或者说IMU 的测量频率太高。我们并不希望优化过程随着 IMU 数据进行调用那样太浪费计算资源。我们更希望将这些 IMU 测量值组合在一起处理即在关键帧之间对 IMU 数据进行预积分然后按照 关键帧 的频率调用优化过程。 假设从离散时间 和 之间的 IMU 数据被累计起来这个过程可以持续若干秒钟。这种被累计起来的观测被称为预积分。如果我们使用不同形式的运动学第 2 章中介绍的运动学得到的预积分形式也是不同的 。本书主要使用 SO(3) t 的方式来推导预积分。那么累积离散时间 和 之间的 IMU 数据令累积的时间为得到 上述式子只是累积形式是传统意义上的直接积分。直接积分的缺点是其描述的过程和状态量有关。如果我们对 i 时刻的状态进行优化那么, , . . . , 时刻的状态也会跟着发生改变这个积分就必须重新计算这是非常不便的。为此定义状态变化量 如下 对上述累积式稍加改变尽量将 IMU 读数放在一侧状态量放到另一侧可得如下式子该式称为预积分的定义式 1. 我们不妨考虑从 时刻出发此时这三个量都为零。在 时刻我们计算出 , 和 。而在 时刻时由于这三个式子都是累乘或累加的形式只需在 , 时刻的结果之上加上第 时刻的测量值即可。这在计算层面带来了很大的便利。进一步我们还会发现这种性质对后续计算各种雅可比矩阵都非常方便。2. 从等号最右侧来看上述所有计算都和 的取值无关。即使它们的状态估计值发生改变IMU 积分量也无需重新计算。3. 不过如果零偏 或 发生变化那么上述式子理论上还是需要重新计算。然而我们也可以通过“修正”而非“重新计算”的思路来调整我们的预积分量。4. 请注意预积分量并没有直接的物理含义。尽管符号上用了 , 之类的样子但它并不表示某两个速度或位置上的偏差。它只是如此定义而已。当然从量纲上来说应该与角度、速度、位移对应。5. 同样地由于预积分量不是直接的物理量这种“预积分测量模型”的噪声也必须从原始的 IMU 噪声推导而来。 2 预积分的测量模型 ( 预积分的测量值可由 IMU 的测量值积分得到 ) 由前面的讨论可见预积分内部带有 IMU 的零偏量因此不可避免地会依赖此时的零偏量估计。为了处理这种依赖我们对预积分定义作一些工程上的调整
1. 我们首先认为 时刻的零偏是固定的并且在整个预积分计算过程中也都是固定的。2. 假设预积分的测量值是随零偏线性变化的一阶线性化模型舍弃对零偏量的高阶项。3. 当零偏估计发生改变时在原先预积分测量值的基础上使用线性模型进行修正得到新的预积分测量值。 首先我们固定 时刻的零偏估计来分析预积分的噪声。无论是图优化还是滤波器技术都需要知道某个测量量究竟含有多大的噪声。 2.1 旋转部分 定义预积分旋转测量值 如下 预积分定义式中的旋转部分经过变换可得如下式其中 为预积分旋转的测量噪声 上式中的 可根据《自动驾驶与机器人中的SLAM技术》ch2基础数学知识 中公式计算得到 2.2 速度部分 定义预积分速度测量值 如下 预积分定义式中的速度部分经过变换可得如下式其中 为预积分速度的测量噪声 2.3 平移部分 定义预积分平移测量值 如下 预积分定义式中的平移部分经过变换可得如下式其中 为预积分平移的测量噪声 2.4 将预积分测量和误差式代回最初的定义式 将上面的预积分测量及误差式 重新代入最初的预积分定义式中得 这个式子归纳了前面我们讨论的内容显示了预积分的几大优点
1. 它的左侧是可以通过 IMU 的测量值积分得到的预积分测量值右侧是根据状态变量推断出来的预测值再加上或乘上一个随机噪声。2. 左侧变量的定义方式非常适合程序实现。 可以通过 时刻 IMU 读数得到 可以由 时刻 IMU 读数和 算得而 又可以通过 时刻 IMU 读数和 和 计算结果得到。另一方面如果知道了 时刻的预积分测量值又很容易根据 时刻 IMU 读数计算出 时刻的预积分测量值。这是由预积分测量值的累加/累乘定义方式决定的。3. 从右侧看来也很容易根据 和 时刻的状态变量来推测预积分测量值的大小从而写出误差公式形成最小二乘。 将上式变化可以得到预测公式。通过预测公式可以使用 时刻到 时刻的预积分测量值和 时刻状态推测 时刻状态 忽略测量噪声得 接下来继续推导预积分的噪声模型。 3 预积分的噪声模型和协方差矩阵 由于噪声项的定义比较复杂本节会使用同样的思路来处理各种噪声项。我们会将复杂的噪声项线性化保留一阶项系数然后推导线性模型下的协方差矩阵变化。这是一种非常常见的处理思路对许多复杂模型都很有效。 3.1 旋转部分 旋转部分的测量噪声如下其中 为 时刻离散化后的 IMU 陀螺仪测量噪声 由上式可知作为随机变量的 只和随机变量 有关而其他的都是确定的观测量。 对上式两侧取 得 我们将其变换为递推形式即从 时刻噪声推断得到 时刻噪声 注意上式中 。 3.2 速度部分 速度部分的测量噪声如下其中 为 时刻离散化后的 IMU 加速度计测量噪声 我们将其变换为递推形式即从 时刻噪声推断得到 时刻噪声 3.3 平移部分 平移部分的测量噪声如下其中 为 时刻离散化后的 IMU 加速度计测量噪声 我们将其变换为递推形式即从 时刻噪声推断得到 时刻噪声 3.4 噪声项合并 为了方便表示将 3 种噪声项的递推形式合并为一个。令 为预积分测量噪声、 为 时刻离散化后的 IMU 测量噪声 那么从 到 的递推形式为 其中系数矩阵 为 如果我们以协方差的形式来记录噪声那么每次增加 IMU 观测时噪声应该呈现出逐渐增大的关系 这里的 阵接近单位矩阵 因此可以看成将预积分测量噪声累加起来。陀螺仪的噪声通过 矩阵进入到旋转的观测量中而加速度计的噪声则主要进入速度与平移估计中。这种累加关系很容易在程序中实现出来。后面我们会在实验章节中看到它们的实现。注意如果预积分定义的残差项顺序发生改变我们也需要调整这里的系统矩阵行列关系以保持一致性。 4 零偏的更新 先前的讨论都假设了在 时刻的 IMU 零偏固定不变当然这都是为了方便后续的计算。然而在实际的图优化中我们经常会对状态变量优化变量进行更新导致零偏发生改变。理论上来讲如果 IMU零偏发生了变化预积分就应该重新计算因为预积分的每一步都用到了 时刻的 IMU 零偏。但是实际操作过程中我们也可以选用一种取巧的做法假设预积分的测量值是随零偏线性变化的当零偏估计发生改变时在原先预积分测量值的基础上使用线性模型进行修正得到新的预积分测量值。具体来说我们把预积分测量值看成 的函数那么当 更新了 之后预积分观测应作如下的修正下式即为预积分测量值的修正公式 只需求出上式中 预积分观测值 相对于 的雅可比矩阵即可在零偏更新时对预积分观测值进行修正。 预积分观测值 相对于 的雅可比矩阵如下 将其变换为递推形式即从 时刻雅可比矩阵推断得到 时刻雅可比矩阵 5 图优化中的预积分边 现在我们定义了预积分的测量模型推导了它的噪声模型和协方差矩阵并说明了随着零偏量更新预积分观测值应该怎么更新。事实上我们已经可以把预积分观测作为图优化的因子Factor或者边Edge了。 在 IMU 相关的应用中通常把每个时刻的状态建模为包含旋转、平移、线速度、IMU 零偏的变量构成状态变量集合 预积分模型构建了关键帧 与关键帧 之间的相对运动约束。 5.1 预积分边的残差 不同文献当中对残差的具体定义并不完全一致残差的实际定义是相当灵活的。注意更换残差定义时对应的噪声协方差可能会发生改变。 预积分本身的观测模型已经在预积分定义式中介绍我们可以用 时刻、 时刻的状态变量值与预积分的观测量作差得到残差的定义公式。 通常我们会把 统一写成一个 9 维的残差变量。它表面上关联两个时刻的旋转、平移、线速度但由于预积分观测内部含有 IMU 零偏所以实际也和 时刻两个零偏有关。在优化过程中如果对 时刻的零偏进行更新那么预积分观测量也应该线性地发生改变从而影响残差项的取值。所以尽管在这个残差项里似乎不含有 它们显然是和残差相关的。因此如果我们把预积分残差看作同一个那么它与状态顶点的关联应该如下图所示。除了预积分因子本身之外还需要约束 IMU 的随机游走因此在 IMU 的不同时刻零偏会存在一个约束因子。 关于图优化中顶点的形式有以下两种
把所有状态变量放在同一个顶点选择“散装的形式”即对旋转、平移、线速度和两个零偏分别构造顶点共构造 5 个顶点然后求解这几个顶点之间的雅可比。如果采用这种做法那么雅可比矩阵的数量会变多但单个雅可比矩阵的维度可以降低单个雅可比通常为 3 × 3而预积分观测量对状态变量的雅可比会变为 9 × 15且有很多零块。由于视觉和激光的观测约束通常只和 相关如果需要构造视觉和激光雷达的紧耦合 LIO 系统则可以避免雅可比矩阵当中的一些零矩阵块。 总而言之两种做法各有各的好处本书的代码实现部分使用了散装做法即把各状态量写成单独的顶点分别来约束它们。 5.2 预积分残差相对于状态变量的雅可比矩阵 最后我们来讨论预积分残差相对状态变量的雅可比矩阵。 5.2.1 旋转部分 旋转部分的残差如下其与状态变量 有关 假设优化初始的零偏为 在某一步迭代时我们当前估计出来的零偏修正为 而当前修正得到的预积分旋转观测量为 残差为 。预积分旋转残差相对状态变量的雅可比矩阵如下 上式中的 为 为 。 5.2.2 速度部分 速度部分的残差如下其与状态变量 有关 预积分速度残差相对状态变量的雅可比矩阵如下 5.2.3 平移部分 平移部分的残差如下其与状态变量 有关 预积分平移残差相对状态变量的雅可比矩阵如下 6 预积分流程 在一个关键帧组成的系统中我们可以从任意一个时刻的关键帧出发开始预积分并且在任一时刻停止预积分过程。之后我们可以把预积分的观测量、噪声以及各种累计雅可比取出来用于约束两个关键帧的状态。按照先前的讨论在开始预积分之后当 时刻一个新的 IMU 数据到来时我们的程序应该完成以下任务
1. 在上一个数据基础上利用下式计算三个预积分观测量其中 右乘更新 2. 计算三个噪声量的协方差矩阵作为后续图优化的信息矩阵 其中 。
3. 计算预积分观测量相对于零偏的雅可比矩阵当零偏更新时进行预积分测量值的修正 4.增量积分时间 。5.可以使用 时刻到 时刻的预积分测量值 推测 时刻状态 7 预积分的程序实现 7.1 程序实现 一个预积分类应该存储以下数据
• 预积分的观测量 • 预积分开始时的 IMU 零偏 , • 在积分时期内的测量噪声 。• 各积分量对 IMU 零偏的雅可比矩阵。 • 整个积分时间 。
/*** IMU 预积分器** 调用Integrate来插入新的IMU读数然后通过Get函数得到预积分的值* 雅可比也可以通过本类获得可用于构建g2o的边类*/
class IMUPreintegration {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW/// 参数配置项/// 初始的零偏需要设置其他可以不改struct Options {Options() {}Vec3d init_bg_ Vec3d::Zero(); // 初始零偏Vec3d init_ba_ Vec3d::Zero(); // 初始零偏double noise_gyro_ 1e-2; // 陀螺噪声标准差double noise_acce_ 1e-1; // 加计噪声标准差};public:double dt_ 0; // 整体预积分时间Mat9d cov_ Mat9d::Zero(); // 累计噪声矩阵Mat6d noise_gyro_acce_ Mat6d::Zero(); // 测量噪声矩阵// 零偏Vec3d bg_ Vec3d::Zero();Vec3d ba_ Vec3d::Zero();// 预积分观测量SO3 dR_;Vec3d dv_ Vec3d::Zero();Vec3d dp_ Vec3d::Zero();// 雅可比矩阵Mat3d dR_dbg_ Mat3d::Zero();Mat3d dV_dbg_ Mat3d::Zero();Mat3d dV_dba_ Mat3d::Zero();Mat3d dP_dbg_ Mat3d::Zero();Mat3d dP_dba_ Mat3d::Zero();
}; 注意 IMU 零偏相关的噪声项并不直接和预积分类有关我们将它们挪到优化类当中。 单个 IMU 的积分函数实现如下
void IMUPreintegration::Integrate(const IMU imu, double dt) {// 去掉零偏的测量Vec3d gyr imu.gyro_ - bg_; // 陀螺Vec3d acc imu.acce_ - ba_; // 加计// 更新dv, dp, 见p105 (4.9), (4.13), (4.16) // 等号右侧 dp_ dv_ 为 i 时刻预积分观测量 等号左侧 dp_ dv_ 为 j 时刻预积分观测量Vec3d omega gyr * dt; // 转动量SO3 deltaR SO3::exp(omega); // exp后SO3 new_dR dR_ * deltaR;Vec3d new_dv dv_ dR_ * acc * dt;Vec3d new_dp dp_ dv_ * dt 0.5f * dR_.matrix() * acc * dt * dt;Mat3d rightJ SO3::jr(omega); // 右雅可比Mat3d acc_hat SO3::hat(acc);double dt2 dt * dt;// 运动方程雅可比矩阵系数A,B阵见p108 (4.29)Eigen::Matrixdouble, 9, 9 A;A.setIdentity();Eigen::Matrixdouble, 9, 6 B;B.setZero();A.block3, 3(0, 0) deltaR.matrix().transpose(); // A.block3, 3(3, 0) -dR_.matrix() * dt * acc_hat;A.block3, 3(6, 0) -0.5f * dR_.matrix() * acc_hat * dt2;A.block3, 3(6, 3) dt * Mat3d::Identity();B.block3, 3(0, 0) rightJ * dt;B.block3, 3(3, 3) dR_.matrix() * dt;B.block3, 3(6, 3) 0.5f * dR_.matrix() * dt2;// 更新噪声项cov_ A * cov_ * A.transpose() B * noise_gyro_acce_ * B.transpose(); // 更新各雅可比见式p111 (4.39)dR_dbg_ deltaR.matrix().transpose() * dR_dbg_ - rightJ * dt; // p111 (4.39a)dV_dba_ dV_dba_ - dR_.matrix() * dt; // (4.39b)dV_dbg_ dV_dbg_ - dR_.matrix() * dt * acc_hat * dR_dbg_; // (4.39c)dP_dba_ dP_dba_ dV_dba_ * dt - 0.5f * dR_.matrix() * dt2; // (4.39d)dP_dbg_ dP_dbg_ dV_dbg_ * dt - 0.5f * dR_.matrix() * dt2 * acc_hat * dR_dbg_; // (4.39e)// 更新预积分测量值dR_ new_dR; dv_ new_dv; dp_ new_dp; // my 增量积分时间dt_ dt;
} 注意如果不进行优化预积分和直接积分的效果是完全一致的都是将 IMU 的数据积分起来。 在预积分之后我们也可以像 ESKF 一样从起始状态向最终状态进行预测。预测函数实现是非常简单的
NavStated IMUPreintegration::Predict(const sad::NavStated start, const Vec3d grav) const {// grav 存在默认值// std::cout grav: grav.transpose() std::endl;// p105 (4.18) 变形版 忽略噪声后 i 时刻状态和 j 时刻状态的关系式, 这里的 dt_ 是整体预积分时间SO3 Rj start.R_ * dR_;Vec3d vj start.R_ * dv_ start.v_ grav * dt_;// pj 有点问题Vec3d pj start.R_ * dp_ start.p_ start.v_ * dt_ 0.5f * grav * dt_ * dt_;auto state NavStated(start.timestamp_ dt_, Rj, pj, vj);state.bg_ bg_;state.ba_ ba_;return state;
} 与 ESKF 不同的是
预积分可以对多个 IMU 数据进行预测可以从任意起始时刻向后预测 ESKF 通常只在当前状态下针对单个 IMU 数据向下一个时刻预测 7.2 测试 1 测试在恒定角速度运转下的预积分情况
TEST(PREINTEGRATION_TEST, ROTATION_TEST) {// 测试在恒定角速度运转下的预积分情况double imu_time_span 0.01; // IMU测量间隔Vec3d constant_omega(0, 0, M_PI); // 角速度为180度/s转1秒应该等于转180度Vec3d gravity(0, 0, -9.8); // Z 向上重力方向为负sad::NavStated start_status(0), end_status(1.0);sad::IMUPreintegration pre_integ;// 对比直接积分Sophus::SO3d R;Vec3d t Vec3d::Zero();Vec3d v Vec3d::Zero();for (int i 1; i 100; i) {double time imu_time_span * i;Vec3d acce -gravity; // 水平放置时加速度计应该测量到一个向上的力为 -gpre_integ.Integrate(sad::IMU(time, constant_omega, acce), imu_time_span);sad::NavStated this_status pre_integ.Predict(start_status, gravity);// p101 (4.4)t t v * imu_time_span 0.5 * gravity * imu_time_span * imu_time_span 0.5 * (R * acce) * imu_time_span * imu_time_span;v v gravity * imu_time_span (R * acce) * imu_time_span;R R * Sophus::SO3d::exp(constant_omega * imu_time_span);// 验证在简单情况下直接积分和预积分结果相等// Google Test框架中的断言assertions用于比较预期值和实际值是否在允许的误差范围内。如果不满足条件则报 FailureEXPECT_NEAR(t[0], this_status.p_[0], 1e-2);EXPECT_NEAR(t[1], this_status.p_[1], 1e-2);EXPECT_NEAR(t[2], this_status.p_[2], 1e-2);EXPECT_NEAR(v[0], this_status.v_[0], 1e-2);EXPECT_NEAR(v[1], this_status.v_[1], 1e-2);EXPECT_NEAR(v[2], this_status.v_[2], 1e-2);EXPECT_NEAR(R.unit_quaternion().x(), this_status.R_.unit_quaternion().x(), 1e-4);EXPECT_NEAR(R.unit_quaternion().y(), this_status.R_.unit_quaternion().y(), 1e-4);EXPECT_NEAR(R.unit_quaternion().z(), this_status.R_.unit_quaternion().z(), 1e-4);EXPECT_NEAR(R.unit_quaternion().w(), this_status.R_.unit_quaternion().w(), 1e-4);}end_status pre_integ.Predict(start_status, gravity);LOG(INFO) preinteg result: ;LOG(INFO) end rotation: \n end_status.R_.matrix();LOG(INFO) end trans: \n end_status.p_.transpose();LOG(INFO) end v: \n end_status.v_.transpose();LOG(INFO) direct integ result: ;LOG(INFO) end rotation: \n R.matrix();LOG(INFO) end trans: \n t.transpose();LOG(INFO) end v: \n v.transpose();SUCCEED();
} 测试结果
./test_preintegration --gtest_filterPREINTEGRATION_TEST.ROTATION_TESTNote: Google Test filter PREINTEGRATION_TEST.ROTATION_TEST
[] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
[----------] 1 test from PREINTEGRATION_TEST
[ RUN ] PREINTEGRATION_TEST.ROTATION_TEST
I0116 22:22:12.217562 490210 test_preintegration.cc:74] preinteg result:
I0116 22:22:12.217624 490210 test_preintegration.cc:75] end rotation:
000000000-1 03.1225e-16 00000000000
-3.1225e-16 000000000-1 00000000000
00000000000 00000000000 00000000001
I0116 22:22:12.217639 490210 test_preintegration.cc:76] end trans:
000000000000 000000000000 -4.44089e-15
I0116 22:22:12.217643 490210 test_preintegration.cc:77] end v:
000000000000 000000000000 -1.77636e-15
I0116 22:22:12.217648 490210 test_preintegration.cc:79] direct integ result:
I0116 22:22:12.217648 490210 test_preintegration.cc:80] end rotation:
000000000-1 03.1225e-16 00000000000
-3.1225e-16 000000000-1 00000000000
00000000000 00000000000 00000000001
I0116 22:22:12.217655 490210 test_preintegration.cc:81] end trans:
0 0 0
I0116 22:22:12.217658 490210 test_preintegration.cc:82] end v:
0 0 0
[ OK ] PREINTEGRATION_TEST.ROTATION_TEST (0 ms)
[----------] 1 test from PREINTEGRATION_TEST (0 ms total)[----------] Global test environment tear-down
[] 1 test from 1 test suite ran. (0 ms total)
[ PASSED ] 1 test.2. 测试在恒定加速度运行下的预积分情况
TEST(PREINTEGRATION_TEST, ACCELERATION_TEST) {// 测试在恒定加速度运行下的预积分情况double imu_time_span 0.01; // IMU测量间隔Vec3d gravity(0, 0, -9.8); // Z 向上重力方向为负Vec3d constant_acce(0.1, 0, 0); // x 方向上的恒定加速度sad::NavStated start_status(0), end_status(1.0);sad::IMUPreintegration pre_integ;// 对比直接积分Sophus::SO3d R;Vec3d t Vec3d::Zero();Vec3d v Vec3d::Zero();for (int i 1; i 100; i) {double time imu_time_span * i;Vec3d acce constant_acce - gravity;pre_integ.Integrate(sad::IMU(time, Vec3d::Zero(), acce), imu_time_span);sad::NavStated this_status pre_integ.Predict(start_status, gravity);t t v * imu_time_span 0.5 * gravity * imu_time_span * imu_time_span 0.5 * (R * acce) * imu_time_span * imu_time_span;v v gravity * imu_time_span (R * acce) * imu_time_span;// 验证在简单情况下直接积分和预积分结果相等EXPECT_NEAR(t[0], this_status.p_[0], 1e-2);EXPECT_NEAR(t[1], this_status.p_[1], 1e-2);EXPECT_NEAR(t[2], this_status.p_[2], 1e-2);EXPECT_NEAR(v[0], this_status.v_[0], 1e-2);EXPECT_NEAR(v[1], this_status.v_[1], 1e-2);EXPECT_NEAR(v[2], this_status.v_[2], 1e-2);EXPECT_NEAR(R.unit_quaternion().x(), this_status.R_.unit_quaternion().x(), 1e-4);EXPECT_NEAR(R.unit_quaternion().y(), this_status.R_.unit_quaternion().y(), 1e-4);EXPECT_NEAR(R.unit_quaternion().z(), this_status.R_.unit_quaternion().z(), 1e-4);EXPECT_NEAR(R.unit_quaternion().w(), this_status.R_.unit_quaternion().w(), 1e-4);}end_status pre_integ.Predict(start_status, gravity);LOG(INFO) preinteg result: ;LOG(INFO) end rotation: \n end_status.R_.matrix();LOG(INFO) end trans: \n end_status.p_.transpose();LOG(INFO) end v: \n end_status.v_.transpose();LOG(INFO) direct integ result: ;LOG(INFO) end rotation: \n R.matrix();LOG(INFO) end trans: \n t.transpose();LOG(INFO) end v: \n v.transpose();SUCCEED();
} 测试结果 ./test_preintegration --gtest_filterPREINTEGRATION_TEST.ACCELERATION_TESTNote: Google Test filter PREINTEGRATION_TEST.ACCELERATION_TEST
[] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
[----------] 1 test from PREINTEGRATION_TEST
[ RUN ] PREINTEGRATION_TEST.ACCELERATION_TEST
I0116 22:26:44.789176 490325 test_preintegration.cc:126] preinteg result:
I0116 22:26:44.789251 490325 test_preintegration.cc:127] end rotation:
1 0 0
0 1 0
0 0 1
I0116 22:26:44.789265 490325 test_preintegration.cc:128] end trans:
000000000.05 000000000000 -4.44089e-15
I0116 22:26:44.789271 490325 test_preintegration.cc:129] end v:
0000000000.1 000000000000 -1.77636e-15
I0116 22:26:44.789274 490325 test_preintegration.cc:131] direct integ result:
I0116 22:26:44.789276 490325 test_preintegration.cc:132] end rotation:
1 0 0
0 1 0
0 0 1
I0116 22:26:44.789283 490325 test_preintegration.cc:133] end trans:
0.05 0000 0000
I0116 22:26:44.789286 490325 test_preintegration.cc:134] end v:
0.1 000 000
[ OK ] PREINTEGRATION_TEST.ACCELERATION_TEST (0 ms)
[----------] 1 test from PREINTEGRATION_TEST (0 ms total)[----------] Global test environment tear-down
[] 1 test from 1 test suite ran. (0 ms total)
[ PASSED ] 1 test.
