diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index f9f846790..6c79ea137 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -49,8 +49,9 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation * Default constructor, initialize with no measurements * @param bias Current estimate of acceleration and rotation rate biases */ - PreintegratedAhrsMeasurements(const boost::shared_ptr& p, const Vector3& biasHat) - : PreintegratedRotation(p), biasHat_(biasHat) { + PreintegratedAhrsMeasurements(const boost::shared_ptr& p, + const Vector3& biasHat) : + PreintegratedRotation(p), biasHat_(biasHat) { resetIntegration(); } diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index c54137c90..c6bc8282a 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -59,7 +59,8 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). Vector3 correctedAcc, correctedOmega; - boost::tie(correctedAcc, correctedOmega) = correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, body_P_sensor); + boost::tie(correctedAcc, correctedOmega) = + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega); 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/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 192cc3d99..1289f22c6 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -115,13 +115,13 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase { * Default constructor, initializes the class with no measurements * @param bias Current estimate of acceleration and rotation rate biases */ - PreintegratedCombinedMeasurements(const boost::shared_ptr& p, + PreintegratedCombinedMeasurements(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat) : PreintegrationBase(p, biasHat) { preintMeasCov_.setZero(); } - const Params& p() const { return *boost::static_pointer_cast(p_);} + Params& p() const { return *boost::static_pointer_cast(p_);} /// print void print(const std::string& s = "Preintegrated Measurements:") const; diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index eca91f06e..187261685 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -54,7 +54,8 @@ void PreintegratedImuMeasurements::integrateMeasurement( OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { Vector3 correctedAcc, correctedOmega; - boost::tie(correctedAcc, correctedOmega) = correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, body_P_sensor); + boost::tie(correctedAcc, correctedOmega) = + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega); // rotation increment computed from the current rotation rate measurement const Vector3 integratedOmega = correctedOmega * deltaT; @@ -123,6 +124,14 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements( resetIntegration(); } +//------------------------------------------------------------------------------ +void PreintegratedImuMeasurements::integrateMeasurement( + const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + const Pose3& body_P_sensor) { + p_->body_P_sensor = body_P_sensor; + integrateMeasurement(measuredAcc, measuredOmega, deltaT); +} + //------------------------------------------------------------------------------ // ImuFactor methods //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index f66d28828..980f7d3f3 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -76,9 +76,9 @@ public: * @param bias Current estimate of acceleration and rotation rate biases * @param p Parameters, typically fixed in a single application */ - PreintegratedImuMeasurements(const boost::shared_ptr& p, - const imuBias::ConstantBias& biasHat) : - PreintegrationBase(p,biasHat) { + PreintegratedImuMeasurements(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat) : + PreintegrationBase(p, biasHat) { preintMeasCov_.setZero(); } @@ -115,6 +115,11 @@ public: const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration = true); + /// @deprecated version of integrateMeasurement with body_P_sensor + /// Use parameters instead + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double deltaT, const Pose3& body_P_sensor); + private: /// Serialization function diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index a93c54127..df521341f 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -60,14 +60,14 @@ class PreintegratedRotation { Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias /// Parameters - boost::shared_ptr p_; + boost::shared_ptr p_; /// Default constructor for serialization PreintegratedRotation() {} public: /// Default constructor, resets integration to zero - explicit PreintegratedRotation(const boost::shared_ptr& p) : p_(p) { + explicit PreintegratedRotation(const boost::shared_ptr& p) : p_(p) { resetIntegration(); } diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 57c9c8e7c..da6b03019 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -103,8 +103,7 @@ void PreintegrationBase::updatePreintegratedJacobians( } std::pair PreintegrationBase::correctMeasurementsByBiasAndSensorPose( - const Vector3& measuredAcc, const Vector3& measuredOmega, - boost::optional body_P_sensor) const { + const Vector3& measuredAcc, const Vector3& measuredOmega) const { // Correct for bias in the sensor frame Vector3 s_correctedAcc, s_correctedOmega; s_correctedAcc = biasHat_.correctAccelerometer(measuredAcc); @@ -113,11 +112,10 @@ std::pair PreintegrationBase::correctMeasurementsByBiasAndSens // Compensate for sensor-body displacement if needed: we express the quantities // (originally in the IMU frame) into the body frame // Equations below assume the "body" frame is the CG - if (body_P_sensor) { - Matrix3 bRs = body_P_sensor->rotation().matrix(); + if (p().body_P_sensor) { + Matrix3 bRs = p().body_P_sensor->rotation().matrix(); + Vector3 b_arm = p().body_P_sensor->translation().vector(); 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: @@ -125,7 +123,7 @@ std::pair PreintegrationBase::correctMeasurementsByBiasAndSens - b_correctedOmega.cross(b_velocity_bs); return std::make_pair(b_correctedAcc, b_correctedOmega); } else - return std::make_pair(correctedAcc, s_correctedOmega); + return std::make_pair(s_correctedAcc, s_correctedOmega); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 4f53041f0..e41114b07 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -122,7 +122,7 @@ public: * @param bias Current estimate of acceleration and rotation rate biases * @param p Parameters, typically fixed in a single application */ - PreintegrationBase(const boost::shared_ptr& p, + PreintegrationBase(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat) : PreintegratedRotation(p), biasHat_(biasHat) { resetIntegration(); @@ -132,7 +132,7 @@ public: void resetIntegration(); const Params& p() const { - return *boost::static_pointer_cast(p_); + return *boost::static_pointer_cast(p_); } /// getters