Merge pull request #93 from sandro-berchier/feature/pim_to_ahrs

Added non-default const. for AHRS pre-int. and params

gtsam/navigation/AHRSFactor.h

@@ -55,6 +55,26 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
     resetIntegration();
   }
 
+  /**
+   *  Non-Default constructor, initialize with measurements
+   *  @param p: Parameters for AHRS pre-integration
+   *  @param bias_hat: Current estimate of acceleration and rotation rate biases
+   *  @param deltaTij: Delta time in pre-integration
+   *  @param deltaRij: Delta rotation in pre-integration
+   *  @param delRdelBiasOmega: Jacobian of rotation wrt. to gyro bias
+   *  @param preint_meas_cov: Pre-integration covariance
+   */
+  PreintegratedAhrsMeasurements(
+      const boost::shared_ptr<Params>& p,
+      const Vector3& bias_hat,
+      double deltaTij,
+      const Rot3& deltaRij,
+      const Matrix3& delRdelBiasOmega,
+      const Matrix3& preint_meas_cov) :
+      biasHat_(bias_hat),
+      PreintegratedRotation(p, deltaTij, deltaRij, delRdelBiasOmega),
+      preintMeasCov_(preint_meas_cov) {}
+
   const Params& p() const { return *boost::static_pointer_cast<const Params>(p_);}
   const Vector3& biasHat() const { return biasHat_; }
   const Matrix3& preintMeasCov() const { return preintMeasCov_; }

gtsam/navigation/PreintegratedRotation.h

@@ -35,6 +35,13 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
 
   PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {}
 
+  PreintegratedRotationParams(const Matrix3& gyroscope_covariance,
+                              boost::optional<Vector3> omega_coriolis)
+    : gyroscopeCovariance(gyroscope_covariance) {
+      if (omega_coriolis)
+        omegaCoriolis.reset(omega_coriolis.get());
+  }
+
   virtual ~PreintegratedRotationParams() {}
 
   virtual void print(const std::string& s) const;

gtsam/navigation/tests/testAHRSFactor.cpp

@@ -119,6 +119,34 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
   DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
 }
 
+//******************************************************************************
+TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) {
+  Matrix3 gyroscopeCovariance = Matrix3::Ones()*0.4;
+  Vector3 omegaCoriolis(0.1, 0.5, 0.9);
+  PreintegratedRotationParams params(gyroscopeCovariance, omegaCoriolis);
+  Vector3 bias(1.0,2.0,3.0); ///< Current estimate of angular rate bias
+  Rot3 deltaRij(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0));
+  double deltaTij = 0.02;
+  Matrix3 delRdelBiasOmega = Matrix3::Ones()*0.5;
+  Matrix3 preintMeasCov = Matrix3::Ones()*0.2;
+  PreintegratedAhrsMeasurements actualPim(
+    boost::make_shared<PreintegratedRotationParams>(params),
+    bias,
+    deltaTij,
+    deltaRij,
+    delRdelBiasOmega,
+    preintMeasCov);
+  EXPECT(assert_equal(gyroscopeCovariance,
+      actualPim.p().getGyroscopeCovariance(), 1e-6));
+  EXPECT(assert_equal(omegaCoriolis,
+      actualPim.p().getOmegaCoriolis().get(), 1e-6));
+  EXPECT(assert_equal(bias, actualPim.biasHat(), 1e-6));
+  DOUBLES_EQUAL(deltaTij, actualPim.deltaTij(), 1e-6);
+  EXPECT(assert_equal(deltaRij, Rot3(actualPim.deltaRij()), 1e-6));
+  EXPECT(assert_equal(delRdelBiasOmega, actualPim.delRdelBiasOmega(), 1e-6));
+  EXPECT(assert_equal(preintMeasCov, actualPim.preintMeasCov(), 1e-6));
+}
+
 /* ************************************************************************* */
 TEST(AHRSFactor, Error) {
   // Linearization point