Made new bias tests by Krunal compile. reinstated backwards compatible method.

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<const Params>& p, const Vector3& biasHat)
-      : PreintegratedRotation(p), biasHat_(biasHat) {
+  PreintegratedAhrsMeasurements(const boost::shared_ptr<Params>& p,
+      const Vector3& biasHat) :
+      PreintegratedRotation(p), biasHat_(biasHat) {
     resetIntegration();
   }
 

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

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<const Params>& p,
+  PreintegratedCombinedMeasurements(const boost::shared_ptr<Params>& p,
                                     const imuBias::ConstantBias& biasHat)
       : PreintegrationBase(p, biasHat) {
     preintMeasCov_.setZero();
   }
 
-  const Params& p() const { return *boost::static_pointer_cast<const Params>(p_);}
+  Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
 
   /// print
   void print(const std::string& s = "Preintegrated Measurements:") const;

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
 //------------------------------------------------------------------------------

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<const Params>& p,
+  PreintegratedImuMeasurements(const boost::shared_ptr<Params>& p,
       const imuBias::ConstantBias& biasHat) :
-      PreintegrationBase(p,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

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<const Params> p_;
+  boost::shared_ptr<Params> p_;
 
   /// Default constructor for serialization
   PreintegratedRotation() {}
 
  public:
   /// Default constructor, resets integration to zero
-  explicit PreintegratedRotation(const boost::shared_ptr<const Params>& p) : p_(p) {
+  explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) : p_(p) {
     resetIntegration();
   }
 

gtsam/navigation/PreintegrationBase.cpp

@@ -103,8 +103,7 @@ void PreintegrationBase::updatePreintegratedJacobians(
 }
 
 std::pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
-    const Vector3& measuredAcc, const Vector3& measuredOmega,
-    boost::optional<const Pose3&> 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<Vector3, Vector3> 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<Vector3, Vector3> 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);
 }
 
 //------------------------------------------------------------------------------

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<const Params>& p,
+  PreintegrationBase(const boost::shared_ptr<Params>& 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<const Params>(p_);
+    return *boost::static_pointer_cast<Params>(p_);
   }
 
   /// getters