Fixed comments

gtsam/navigation/AHRSFactor.h

@@ -1,39 +1,57 @@
-/*
+/* ----------------------------------------------------------------------------
- * ImuFactor.h
+
- *
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- *  Created on: Jun 29, 2014
+ * Atlanta, Georgia 30332-0415
- *      Author: krunal
+ * All Rights Reserved
- */
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file  AHRSFactor.h
+ *  @author Krunal Chande, Luca Carlone
+ **/
 
 #pragma once
 
+/* GTSAM includes */
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/base/LieVector.h>
 #include <gtsam/base/debug.h>
 
+/* External or standard includes */
 #include <ostream>
 
 namespace gtsam {
 
 class AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, imuBias::ConstantBias> {
 public:
+
+  /** Struct to store results of preintegrating IMU measurements.  Can be build
+   * incrementally so as to avoid costly integration at time of factor construction. */
+
+  /** CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates)
+       * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated AHRS factor*/
   class PreintegratedMeasurements {
   public:
-    imuBias::ConstantBias biasHat;
+    imuBias::ConstantBias biasHat;///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer
-    Matrix measurementCovariance;
+    Matrix measurementCovariance;///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3)
 
-    Rot3 deltaRij;
+    Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i)
-    double deltaTij;
+    double deltaTij; ///< Time interval from i to j
-    Matrix3 delRdelBiasOmega;
+    Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
-    Matrix PreintMeasCov;
+    Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
 
-    PreintegratedMeasurements(const imuBias::ConstantBias& bias,
+    /** Default constructor, initialize with no measurements */
-        const Matrix3& measuredOmegaCovariance) :
+    PreintegratedMeasurements(
-          biasHat(bias), measurementCovariance(3,3), deltaTij(0.0),
+        const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
+        const Matrix3& measuredOmegaCovariance ///< Covariance matrix of measured angular rate
+        ) : biasHat(bias), measurementCovariance(3,3), deltaTij(0.0),
           delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(3,3) {
-//      measurementCovariance << integrationErrorCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measurementAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance;
       measurementCovariance <<measuredOmegaCovariance;
       PreintMeasCov = Matrix::Zero(3,3);
     }
@@ -42,6 +60,7 @@ public:
       biasHat(imuBias::ConstantBias()), measurementCovariance(Matrix::Zero(3,3)), deltaTij(0.0),
       delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(3,3)) {}
 
+    /** print */
     void print(const std::string& s = "Preintegrated Measurements: ") const {
       std::cout << s << std::endl;
       biasHat.print(" biasHat");
@@ -51,6 +70,7 @@ public:
       std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl;
     }
 
+    /** equals */
     bool equals(const PreintegratedMeasurements& expected,
         double tol = 1e-9) const {
       return biasHat.equals(expected.biasHat, tol)
@@ -64,14 +84,14 @@ public:
     Matrix MeasurementCovariance() const {
       return measurementCovariance;
     }
-    Matrix deltaRij_() const {
+    Matrix DeltaRij() const {
       return deltaRij.matrix();
     }
-    double deltaTij_() const {
+    double DeltaTij() const {
       return deltaTij;
     }
 
-    Vector biasHat_() const {
+    Vector BiasHat() const {
       return biasHat.vector();
     }
 
@@ -82,43 +102,65 @@ public:
       PreintMeasCov = Matrix::Zero(9, 9);
     }
 
+    /** Add a single Gyroscope measurement to the preintegration. */
     void integrateMeasurement(
-        const Vector3& measuredOmega, double deltaT,
+        const Vector3& measuredOmega,  ///< Measured angular velocity (in body frame)
-        boost::optional<const Pose3&> body_P_sensor = boost::none) {
+        double deltaT, ///< Time step
+        boost::optional<const Pose3&> body_P_sensor = boost::none ///< Sensor frame
+        ) {
+
+      // NOTE: order is important here because each update uses old values.
+      // First we compensate the measurements for the bias
       Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega);
 
+      // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
       if (body_P_sensor) {
         Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
-        correctedOmega = body_R_sensor * correctedOmega;
+        correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
         Matrix3 body_omega_body_cross = skewSymmetric(correctedOmega);
+        // linear acceleration vector in the body frame
       }
-      const Vector3 theta_incr = correctedOmega * deltaT;
+      const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
-      const Rot3 Rincr = Rot3::Expmap(theta_incr);
+      const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
-      const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr);
+      const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
 
+      // Update Jacobians
+      /* ----------------------------------------------------------------------------------------------------------------------- */
       delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega
           - Jr_theta_incr * deltaT;
 
-      //      Matrix3 Z_3x3 = Matrix::Zero();
+      // Update preintegrated measurements covariance
-      //      Matrix3 I_3x3 = Matrix::Identity();
+      /* ----------------------------------------------------------------------------------------------------------------------- */
-      const Vector3 theta_i = Rot3::Logmap(deltaRij);
+      const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3)
       const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i);
 
       Rot3 Rot_j = deltaRij * Rincr;
-      const Vector3 theta_j = Rot3::Logmap(Rot_j);
+      const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
       const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(
           theta_j);
 
+      // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
+      // can be seen as a prediction phase in an EKF framework
       Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix()
                   * Jr_theta_i;
+      // analytic expression corresponding to the following numerical derivative
+      // Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
+
+      // overall Jacobian wrt preintegrated measurements (df/dx)
       Matrix F(3, 3);
       F << H_angles_angles;
 
+      // first order uncertainty propagation
+      // the deltaT allows to pass from continuous time noise to discrete time noise
       PreintMeasCov = F * PreintMeasCov * F.transpose()
                   + measurementCovariance * deltaT;
+
+      // Update preintegrated measurements
+      /* ----------------------------------------------------------------------------------------------------------------------- */
       deltaRij = deltaRij * Rincr;
       deltaTij += deltaT;
     }
+
     /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
     // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
     static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
@@ -155,8 +197,8 @@ private:
 
   PreintegratedMeasurements preintegratedMeasurements_;
   Vector3 gravity_;
-  Vector3 omegaCoriolis_;
+  Vector3 omegaCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
-  boost::optional<Pose3> body_P_sensor_;
+  boost::optional<Pose3> body_P_sensor_;///< The pose of the sensor in the body frame
 
 public:
 
@@ -171,20 +213,24 @@ public:
   AHRSFactor() :
     preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero()) {}
 
-  AHRSFactor(Key rot_i, Key rot_j, Key bias,
+  AHRSFactor(
-      const PreintegratedMeasurements& preintegratedMeasurements,
+      Key rot_i, ///< previous rot key
-      const Vector3& omegaCoriolis,
+      Key rot_j,  ///< current rot key
-      boost::optional<const Pose3&> body_P_sensor = boost::none) :
+      Key bias,///< previous bias key
-        Base(
+      const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated measurements
-            noiseModel::Gaussian::Covariance(
+      const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame
-                preintegratedMeasurements.PreintMeasCov), rot_i, rot_j, bias), preintegratedMeasurements_(
+      boost::optional<const Pose3&> body_P_sensor = boost::none  ///< The Pose of the sensor frame in the body frame
-                    preintegratedMeasurements), omegaCoriolis_(
+      ) :
-                        omegaCoriolis), body_P_sensor_(body_P_sensor) {
+      Base(
+          noiseModel::Gaussian::Covariance(
+              preintegratedMeasurements.PreintMeasCov), rot_i, rot_j, bias), preintegratedMeasurements_(
+          preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
+          body_P_sensor) {
   }
 
   virtual ~AHRSFactor() {}
 
-
+  /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(
@@ -193,6 +239,9 @@ public:
         );
     }
 
+  /** implement functions needed for Testable */
+
+  /** print */
   virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const {
     std::cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << ","
@@ -206,6 +255,7 @@ public:
       this->body_P_sensor_->print("  sensor pose in body frame: ");
   }
 
+  /** equals */
   virtual bool equals(const NonlinearFactor& expected,
       double tol = 1e-9) const {
     const This *e = dynamic_cast<const This*>(&expected);
@@ -226,6 +276,9 @@ public:
     return omegaCoriolis_;
   }
 
+  /** implement functions needed to derive from Factor */
+
+  /** vector of errors */
   Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
       const imuBias::ConstantBias& bias,
       boost::optional<Matrix&> H1 = boost::none,
@@ -321,7 +374,6 @@ public:
 
     // Predict state at time j
     /* ---------------------------------------------------------------------------------------------------- */
-
     const Rot3 deltaRij_biascorrected =
         preintegratedMeasurements.deltaRij.retract(
             preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr,
@@ -349,7 +401,6 @@ private:
     ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
     ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
   }
-};
+}; // AHRSFactor
-// AHRSFactor
 typedef AHRSFactor::PreintegratedMeasurements AHRSFactorPreintegratedMeasurements;
 } //namespace gtsam

gtsam/navigation/ImuFactor.h

@@ -77,8 +77,8 @@ namespace gtsam {
       PreintegratedMeasurements(
           const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
           const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
-          const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
+          const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measured Angular Rate
-          const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
+          const Matrix3& integrationErrorCovariance, ///< Covariance matrix of integration errors
           const bool use2ndOrderIntegration = false ///< Controls the order of integration
       ) : biasHat(bias), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
       delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
@@ -95,7 +95,7 @@ namespace gtsam {
       biasHat(imuBias::ConstantBias()), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
       delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
       delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
-      delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9)
+      delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9), use2ndOrderIntegration_(false)
       {
           measurementCovariance =  Matrix::Zero(9,9);
           PreintMeasCov = Matrix::Zero(9,9);
@@ -324,7 +324,7 @@ namespace gtsam {
 #endif
 
     /** Default constructor - only use for serialization */
-    ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {}
+    ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()), use2ndOrderCoriolis_(false){}
 
     /** Constructor */
     ImuFactor(

gtsam/navigation/tests/testAHRSFactor.cpp

@@ -12,7 +12,7 @@
 /**
  * @file    testImuFactor.cpp
  * @brief   Unit test for ImuFactor
- * @author  Luca Carlone, Stephen Williams, Richard Roberts
+ * @author  Krunal Chande, Luca Carlone
  */
 
 #include <gtsam/navigation/AHRSFactor.h>

gtsam/navigation/tests/testImuFactor.cpp

@@ -441,60 +441,60 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
   EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
 }
 
-#include <gtsam/linear/GaussianFactorGraph.h>
+//#include <gtsam/linear/GaussianFactorGraph.h>
-/* ************************************************************************* */
+///* ************************************************************************* */
-TEST( ImuFactor, LinearizeTiming)
+//TEST( ImuFactor, LinearizeTiming)
-{
+//{
-  // Linearization point
+//  // Linearization point
-  Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
+//  Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
-  LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
+//  LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
-  Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
+//  Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
-  LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
+//  LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
-  imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012));
+//  imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012));
-
+//
-  // Pre-integrator
+//  // Pre-integrator
-  imuBias::ConstantBias biasHat(Vector3(0, 0, 0.10), Vector3(0, 0, 0.10));
+//  imuBias::ConstantBias biasHat(Vector3(0, 0, 0.10), Vector3(0, 0, 0.10));
-  Vector3 gravity; gravity << 0, 0, 9.81;
+//  Vector3 gravity; gravity << 0, 0, 9.81;
-  Vector3 omegaCoriolis; omegaCoriolis << 0.0001, 0, 0.01;
+//  Vector3 omegaCoriolis; omegaCoriolis << 0.0001, 0, 0.01;
-  ImuFactor::PreintegratedMeasurements pre_int_data(biasHat, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
+//  ImuFactor::PreintegratedMeasurements pre_int_data(biasHat, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
-
+//
-  // Pre-integrate Measurements
+//  // Pre-integrate Measurements
-  Vector3 measuredAcc(0.1, 0.0, 0.0);
+//  Vector3 measuredAcc(0.1, 0.0, 0.0);
-  Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0);
+//  Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0);
-  double deltaT = 0.5;
+//  double deltaT = 0.5;
-  for(size_t i = 0; i < 50; ++i) {
+//  for(size_t i = 0; i < 50; ++i) {
-    pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
+//    pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
-  }
+//  }
-
+//
-  // Create factor
+//  // Create factor
-  noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.preintegratedMeasurementsCovariance());
+//  noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.MeasurementCovariance());
-  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model);
+//  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
-
+//
-  Values values;
+//  Values values;
-  values.insert(X(1), x1);
+//  values.insert(X(1), x1);
-  values.insert(X(2), x2);
+//  values.insert(X(2), x2);
-  values.insert(V(1), v1);
+//  values.insert(V(1), v1);
-  values.insert(V(2), v2);
+//  values.insert(V(2), v2);
-  values.insert(B(1), bias);
+//  values.insert(B(1), bias);
-
+//
-  Ordering ordering;
+//  Ordering ordering;
-  ordering.push_back(X(1));
+//  ordering.push_back(X(1));
-  ordering.push_back(V(1));
+//  ordering.push_back(V(1));
-  ordering.push_back(X(2));
+//  ordering.push_back(X(2));
-  ordering.push_back(V(2));
+//  ordering.push_back(V(2));
-  ordering.push_back(B(1));
+//  ordering.push_back(B(1));
-
+//
-  GaussianFactorGraph graph;
+//  GaussianFactorGraph graph;
-  gttic_(LinearizeTiming);
+//  gttic_(LinearizeTiming);
-  for(size_t i = 0; i < 100000; ++i) {
+//  for(size_t i = 0; i < 100000; ++i) {
-    GaussianFactor::shared_ptr g = factor.linearize(values, ordering);
+//    GaussianFactor::shared_ptr g = factor.linearize(values, ordering);
-    graph.push_back(g);
+//    graph.push_back(g);
-  }
+//  }
-  gttoc_(LinearizeTiming);
+//  gttoc_(LinearizeTiming);
-  tictoc_finishedIteration_();
+//  tictoc_finishedIteration_();
-  std::cout << "Linear Error: " << graph.error(values.zeroVectors(ordering)) << std::endl;
+//  std::cout << "Linear Error: " << graph.error(values.zeroVectors(ordering)) << std::endl;
-  tictoc_print_();
+//  tictoc_print_();
-}
+//}
 
 
 /* ************************************************************************* */

gtsam/slam/DroneDynamicsFactor.h

@@ -16,7 +16,7 @@
  * @author Duy-Nguyen Ta
  * @date Sep 29, 2014
  */
-
+// Implementation is incorrect use DroneDynamicsVelXYFactor instead.
 #pragma once
 
 #include <boost/lexical_cast.hpp>

gtsam/slam/tests/testDistanceFactor.cpp

@@ -13,7 +13,7 @@
  *  @file  testDistanceFactor.cpp
  *  @brief Unit tests for DistanceFactor Class
  *  @author Duy-Nguyen Ta
- *  @date Oct 2012
+ *  @date Oct 2014
  */
 
 #include <CppUnitLite/TestHarness.h>

gtsam/slam/tests/testDroneDynamicsFactor.cpp

@@ -12,8 +12,8 @@
 /**
  *  @file  testRangeFactor.cpp
  *  @brief Unit tests for DroneDynamicsFactor Class
- *  @author Stephen Williams
+ *  @author Duy-Nguyen Ta
- *  @date Oct 2012
+ *  @date Oct 2014
  */
 
 #include <CppUnitLite/TestHarness.h>

gtsam/slam/tests/testDroneDynamicsVelXYFactor.cpp

@@ -12,8 +12,8 @@
 /**
  *  @file  testRangeFactor.cpp
  *  @brief Unit tests for DroneDynamicsVelXYFactor Class
- *  @author Stephen Williams
+ *  @author Duy-Nguyen Ta
- *  @date Oct 2012
+ *  @date Oct 2014
  */
 
 #include <CppUnitLite/TestHarness.h>