diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 9c2dedea1..c54137c90 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -59,8 +59,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). Vector3 correctedAcc, correctedOmega; - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, - &correctedAcc, &correctedOmega); + boost::tie(correctedAcc, correctedOmega) = correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, body_P_sensor); const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 1589f1a1b..eca91f06e 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -54,8 +54,7 @@ void PreintegratedImuMeasurements::integrateMeasurement( OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { Vector3 correctedAcc, correctedOmega; - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, - &correctedAcc, &correctedOmega); + boost::tie(correctedAcc, correctedOmega) = correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, body_P_sensor); // rotation increment computed from the current rotation rate measurement const Vector3 integratedOmega = correctedOmega * deltaT; diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index d8293cf78..57c9c8e7c 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -102,23 +102,30 @@ void PreintegrationBase::updatePreintegratedJacobians( update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); } -void PreintegrationBase::correctMeasurementsByBiasAndSensorPose( +std::pair PreintegrationBase::correctMeasurementsByBiasAndSensorPose( const Vector3& measuredAcc, const Vector3& measuredOmega, - Vector3* correctedAcc, Vector3* correctedOmega) { - *correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - *correctedOmega = biasHat_.correctGyroscope(measuredOmega); + boost::optional body_P_sensor) const { + // Correct for bias in the sensor frame + Vector3 s_correctedAcc, s_correctedOmega; + s_correctedAcc = biasHat_.correctAccelerometer(measuredAcc); + s_correctedOmega = biasHat_.correctGyroscope(measuredOmega); - // Then compensate for sensor-body displacement: we express the quantities + // Compensate for sensor-body displacement if needed: we express the quantities // (originally in the IMU frame) into the body frame - if (p().body_P_sensor) { - Matrix3 body_R_sensor = p().body_P_sensor->rotation().matrix(); - *correctedOmega = body_R_sensor * (*correctedOmega); // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(*correctedOmega); - *correctedAcc = body_R_sensor * (*correctedAcc) - - body_omega_body__cross * body_omega_body__cross - * p().body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } + // Equations below assume the "body" frame is the CG + if (body_P_sensor) { + Matrix3 bRs = body_P_sensor->rotation().matrix(); + Vector3 b_correctedOmega = bRs * s_correctedOmega; // rotation rate vector in the body frame + Matrix3 body_omega_body__cross = skewSymmetric(b_correctedOmega); + Vector3 b_arm = body_P_sensor->translation().vector(); + Vector3 b_velocity_bs = b_correctedOmega.cross(b_arm); // magnitude: omega * arm + // Subtract out the the centripetal acceleration from the measured one + // to get linear acceleration vector in the body frame: + Vector3 b_correctedAcc = bRs * s_correctedAcc + - b_correctedOmega.cross(b_velocity_bs); + return std::make_pair(b_correctedAcc, b_correctedOmega); + } else + return std::make_pair(correctedAcc, s_correctedOmega); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 8182693ed..4f53041f0 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -177,11 +177,11 @@ public: void updatePreintegratedJacobians(const Vector3& correctedAcc, const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT); - void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, - const Vector3& measuredOmega, Vector3* correctedAcc, - Vector3* correctedOmega); + std::pair + correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, + const Vector3& measuredOmega) const; - /// Given the estimate of the bias, return a NavState tangent vector + /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H = boost::none) const; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index eb6b85bae..fcbecac2a 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -958,6 +958,141 @@ TEST(ImuFactor, PredictArbitrary) { EXPECT(assert_equal(Vector(expectedV), v2, 1e-7)); } +/* ************************************************************************* */ +TEST(ImuFactor, bodyPSensorNoBias) { + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) + + // Measurements + Vector3 n_gravity(0, 0, -9.81); // z-up nav frame + Vector3 omegaCoriolis(0, 0, 0); + // Sensor frame is z-down + // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame + Vector3 s_omegaMeas_ns(0, 0.1, M_PI / 10); + // Acc measurement is acceleration of sensor in the sensor frame, when stationary, + // table exerts an equal and opposite force w.r.t gravity + Vector3 s_accMeas(0, 0, -9.81); + double dt = 0.001; + + // Rotate sensor (z-down) to body (same as navigation) i.e. z-up + Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0)); + + ImuFactor::PreintegratedMeasurements pim(bias, Z_3x3, Z_3x3, Z_3x3, true); + + for (int i = 0; i < 1000; ++i) + pim.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt, body_P_sensor); + + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, n_gravity, omegaCoriolis); + + // Predict + Pose3 x1; + Vector3 v1(0, 0, 0); + PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, n_gravity, + omegaCoriolis); + + Pose3 expectedPose(Rot3().ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0)); + EXPECT(assert_equal(expectedPose, poseVelocity.pose)); + + Vector3 expectedVelocity(0, 0, 0); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); +} + +/* ************************************************************************* */ +#include +#include +#include +#include +#include + +TEST(ImuFactor, bodyPSensorWithBias) { + using noiseModel::Diagonal; + typedef imuBias::ConstantBias Bias; + + int numFactors = 80; + Vector6 noiseBetweenBiasSigma; + noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6, + 3.0e-6, 3.0e-6); + SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma); + + // Measurements + Vector3 n_gravity(0, 0, -9.81); + Vector3 omegaCoriolis(0, 0, 0); + + // Sensor frame is z-down + // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame + Vector3 measuredOmega(0, 0.01, 0); + // Acc measurement is acceleration of sensor in the sensor frame, when stationary, + // table exerts an equal and opposite force w.r.t gravity + Vector3 measuredAcc(0, 0, -9.81); + + Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3()); + + Matrix3 accCov = 1e-7 * I_3x3; + Matrix3 gyroCov = 1e-8 * I_3x3; + Matrix3 integrationCov = 1e-9 * I_3x3; + double deltaT = 0.005; + + // Specify noise values on priors + Vector6 priorNoisePoseSigmas( + (Vector(6) << 0.001, 0.001, 0.001, 0.01, 0.01, 0.01).finished()); + Vector3 priorNoiseVelSigmas((Vector(3) << 0.1, 0.1, 0.1).finished()); + Vector6 priorNoiseBiasSigmas( + (Vector(6) << 0.1, 0.1, 0.1, 0.5e-1, 0.5e-1, 0.5e-1).finished()); + SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas); + SharedDiagonal priorNoiseVel = Diagonal::Sigmas(priorNoiseVelSigmas); + SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas); + Vector3 zeroVel(0, 0, 0); + + // Create a factor graph with priors on initial pose, vlocity and bias + NonlinearFactorGraph graph; + Values values; + + PriorFactor priorPose(X(0), Pose3(), priorNoisePose); + graph.add(priorPose); + values.insert(X(0), Pose3()); + + PriorFactor priorVel(V(0), zeroVel, priorNoiseVel); + graph.add(priorVel); + values.insert(V(0), zeroVel); + + // The key to this test is that we specify the bias, in the sensor frame, as known a priori + // We also create factors below that encode our assumption that this bias is constant over time + // In theory, after optimization, we should recover that same bias estimate + Bias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot) + PriorFactor priorBiasFactor(B(0), priorBias, priorNoiseBias); + graph.add(priorBiasFactor); + values.insert(B(0), priorBias); + + // Now add IMU factors and bias noise models + Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0)); + for (int i = 1; i < numFactors; i++) { + ImuFactor::PreintegratedMeasurements pim = + ImuFactor::PreintegratedMeasurements(zeroBias, accCov, gyroCov, + integrationCov, true); + for (int j = 0; j < 200; ++j) + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT, + body_P_sensor); + + // Create factors + graph.add( + ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim, n_gravity, + omegaCoriolis)); + graph.add(BetweenFactor(B(i - 1), B(i), zeroBias, biasNoiseModel)); + + values.insert(X(i), Pose3()); + values.insert(V(i), zeroVel); + values.insert(B(i), priorBias); + } + + // Finally, optimize, and get bias at last time step + Values results = LevenbergMarquardtOptimizer(graph, values).optimize(); + Bias biasActual = results.at(B(numFactors - 1)); + + // And compare it with expected value (our prior) + Bias biasExpected(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); + EXPECT(assert_equal(biasExpected, biasActual, 1e-3)); +} + /* ************************************************************************* */ int main() { TestResult tr;