diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 954ac9656..86614f7dd 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -74,7 +74,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); #else Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr - PreintegrationBase::update(measuredAcc, measuredOmega, dt, + PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &D_incrR_integratedOmega, &A, &B, &C); #endif diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index d1280688d..c549dd359 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -59,7 +59,7 @@ void PreintegratedImuMeasurements::integrateMeasurement( PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); #else Matrix3 D_incrR_integratedOmega; - PreintegrationBase::update(measuredAcc, measuredOmega, dt, + PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &D_incrR_integratedOmega, &A, &B, &C); #endif diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index f4951305e..000403d4a 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -193,7 +193,7 @@ Vector9 PreintegrationBase::UpdatePreintegrated( //------------------------------------------------------------------------------ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, - double dt, Matrix9* A, + const double dt, Matrix9* A, Matrix93* B, Matrix93* C) { // Correct for bias in the sensor frame Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); @@ -202,10 +202,10 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, // Possibly correct for sensor pose Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; if (p().body_P_sensor) - boost::tie(acc, omega) = correctMeasurementsBySensorPose( acc, omega, + boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); - // Do update + // Do update deltaTij_ += dt; preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C); @@ -227,7 +227,7 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { - // NOTE(frank): integrateMeasuremtn always needs to compute the derivatives, + // NOTE(frank): integrateMeasurement always needs to compute the derivatives, // even when not of interest to the caller. Provide scratch space here. Matrix9 A; Matrix93 B, C; @@ -251,78 +251,42 @@ Vector9 PreintegrationBase::biasCorrectedDelta( #else //------------------------------------------------------------------------------ -pair PreintegrationBase::correctMeasurementsByBiasAndSensorPose( - const Vector3& measuredAcc, const Vector3& measuredOmega, - OptionalJacobian<3, 3> D_correctedAcc_measuredAcc, - OptionalJacobian<3, 3> D_correctedAcc_measuredOmega, - OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const { +void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, + Matrix3* D_incrR_integratedOmega, Matrix9* A, Matrix93* B, Matrix93* C) { // Correct for bias in the sensor frame - Vector3 unbiasedAcc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 unbiasedOmega = biasHat_.correctGyroscope(measuredOmega); + Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 omega = biasHat_.correctGyroscope(measuredOmega); - return correctMeasurementsBySensorPose(unbiasedAcc, unbiasedOmega, - D_correctedAcc_measuredAcc, D_correctedAcc_measuredOmega, - D_correctedOmega_measuredOmega); -} - -//------------------------------------------------------------------------------ -NavState PreintegrationBase::updatedDeltaXij(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double dt, - OptionalJacobian<9, 9> D_updated_current, - OptionalJacobian<9, 3> D_updated_measuredAcc, - OptionalJacobian<9, 3> D_updated_measuredOmega) const { - - Vector3 correctedAcc, correctedOmega; - Matrix3 D_correctedAcc_measuredAcc, // - D_correctedAcc_measuredOmega, // - D_correctedOmega_measuredOmega; - bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega && p().body_P_sensor; - boost::tie(correctedAcc, correctedOmega) = - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, - (needDerivs ? &D_correctedAcc_measuredAcc : 0), - (needDerivs ? &D_correctedAcc_measuredOmega : 0), - (needDerivs ? &D_correctedOmega_measuredOmega : 0)); - // Do update in one fell swoop - Matrix93 D_updated_correctedAcc, D_updated_correctedOmega; - NavState updated = deltaXij_.update(correctedAcc, correctedOmega, dt, D_updated_current, - (needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc), - (needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega)); - if (needDerivs) { - *D_updated_measuredAcc = D_updated_correctedAcc * D_correctedAcc_measuredAcc; - *D_updated_measuredOmega = D_updated_correctedOmega * D_correctedOmega_measuredOmega; - if (!p().body_P_sensor->translation().vector().isZero()) { - *D_updated_measuredOmega += D_updated_correctedAcc * D_correctedAcc_measuredOmega; - } - } - return updated; -} - -//------------------------------------------------------------------------------ -void PreintegrationBase::update(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double dt, - Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current, - Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) { + // Possibly correct for sensor pose + Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; + if (p().body_P_sensor) + boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, + D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); // Save current rotation for updating Jacobians const Rot3 oldRij = deltaXij_.attitude(); // Do update deltaTij_ += dt; - deltaXij_ = updatedDeltaXij(measuredAcc, measuredOmega, dt, - D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional + deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional + + if (p().body_P_sensor) { + // More complicated derivatives in case of non-trivial sensor pose + *C *= D_correctedOmega_omega; + if (!p().body_P_sensor->translation().isZero()) + *C += *B* D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + } // Update Jacobians - // TODO(frank): we are repeating some computation here: accessible in F ? - Vector3 correctedAcc, correctedOmega; - boost::tie(correctedAcc, correctedOmega) = - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega); - + // TODO(frank): Try same simplification as in new approach Matrix3 D_acc_R; - oldRij.rotate(correctedAcc, D_acc_R); + oldRij.rotate(acc, D_acc_R); const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; - const Vector3 integratedOmega = correctedOmega * dt; + const Vector3 integratedOmega = omega * dt; const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! const Matrix3 incrRt = incrR.transpose(); delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 13a7461b4..0a15012f7 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -193,7 +193,7 @@ public: // Update integrated vector on tangent manifold preintegrated with acceleration // Static, functional version. static Vector9 UpdatePreintegrated(const Vector3& a_body, - const Vector3& w_body, double dt, + const Vector3& w_body, const double dt, const Vector9& preintegrated, OptionalJacobian<9, 9> A = boost::none, OptionalJacobian<9, 3> B = boost::none, @@ -202,13 +202,11 @@ public: /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame /// Modifies preintegrated_ in place after correcting for bias and possibly sensor pose - void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double deltaT, + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B, Matrix93* C); // Version without derivatives - void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double deltaT); + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt); /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far @@ -217,28 +215,11 @@ public: #else - /// Subtract estimate and correct for sensor pose - /// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc) - /// Ignore D_correctedOmega_measuredAcc as it is trivially zero - std::pair correctMeasurementsByBiasAndSensorPose( - const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, - OptionalJacobian<3, 3> D_correctedAcc_measuredAcc = boost::none, - OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none, - OptionalJacobian<3, 3> D_correctedOmega_measuredOmega = boost::none) const; - - /// Calculate the updated preintegrated measurement, does not modify - /// It takes measured quantities in the j frame - NavState updatedDeltaXij(const Vector3& j_measuredAcc, - const Vector3& j_measuredOmega, const double dt, - OptionalJacobian<9, 9> D_updated_current = boost::none, - OptionalJacobian<9, 3> D_updated_measuredAcc = boost::none, - OptionalJacobian<9, 3> D_updated_measuredOmega = boost::none) const; - /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame - void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, - const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current, - Matrix93* D_udpated_measuredAcc, Matrix93* D_updated_measuredOmega); + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, + Matrix3* D_incrR_integratedOmega, + Matrix9* A, Matrix93* B, Matrix93* C); /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far