Created common base class

gtsam/navigation/AttitudeFactor.cpp

@@ -23,23 +23,7 @@ using namespace std;
 namespace gtsam {
 
 //***************************************************************************
-void AttitudeFactor::print(const string& s,
+Vector AttitudeFactor::attitudeError(const Rot3& nRb,
-    const KeyFormatter& keyFormatter) const {
-  cout << s << "AttitudeFactor on " << keyFormatter(this->key()) << "\n";
-  nZ_.print("  measured direction in nav frame: ");
-  bRef_.print("  reference direction in body frame: ");
-  this->noiseModel_->print("  noise model: ");
-}
-
-//***************************************************************************
-bool AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const {
-  const This* e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
-      && this->bRef_.equals(e->bRef_, tol);
-}
-
-//***************************************************************************
-Vector AttitudeFactor::evaluateError(const Rot3& nRb,
     boost::optional<Matrix&> H) const {
   if (H) {
     Matrix D_nRef_R, D_e_nRef;
@@ -54,6 +38,23 @@ Vector AttitudeFactor::evaluateError(const Rot3& nRb,
   }
 }
 
+//***************************************************************************
+void Rot3AttitudeFactor::print(const string& s,
+    const KeyFormatter& keyFormatter) const {
+  cout << s << "Rot3AttitudeFactor on " << keyFormatter(this->key()) << "\n";
+  nZ_.print("  measured direction in nav frame: ");
+  bRef_.print("  reference direction in body frame: ");
+  this->noiseModel_->print("  noise model: ");
+}
+
+//***************************************************************************
+bool Rot3AttitudeFactor::equals(const NonlinearFactor& expected,
+    double tol) const {
+  const This* e = dynamic_cast<const This*>(&expected);
+  return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
+      && this->bRef_.equals(e->bRef_, tol);
+}
+
 //***************************************************************************
 void Pose3AttitudeFactor::print(const string& s,
     const KeyFormatter& keyFormatter) const {
@@ -71,24 +72,6 @@ bool Pose3AttitudeFactor::equals(const NonlinearFactor& expected,
       && this->bRef_.equals(e->bRef_, tol);
 }
 
-//***************************************************************************
-Vector Pose3AttitudeFactor::evaluateError(const Pose3& p,
-    boost::optional<Matrix&> H) const {
-  const Rot3& nRb = p.rotation();
-  if (H) {
-    Matrix D_nRef_R, D_e_nRef;
-    Sphere2 nRef = nRb.rotate(bRef_, D_nRef_R);
-    Vector e = nZ_.error(nRef, D_e_nRef);
-    H->resize(2, 6);
-    H->block < 2, 3 > (0, 0) = D_e_nRef * D_nRef_R;
-    H->block < 2, 3 > (0, 3) << Matrix::Zero(2, 3);
-    return e;
-  } else {
-    Sphere2 nRef = nRb * bRef_;
-    return nZ_.error(nRef);
-  }
-}
-
 //***************************************************************************
 
 }/// namespace gtsam

gtsam/navigation/AttitudeFactor.h

@@ -24,34 +24,60 @@
 namespace gtsam {
 
 /**
- * Prior on the attitude of a Rot3.
+ * Base class for prior on attitude
  * Example:
  * - measurement is direction of gravity in body frame bF
  * - reference is direction of gravity in navigation frame nG
  * This factor will give zero error if nRb * bF == nG
  * @addtogroup Navigation
  */
-class AttitudeFactor: public NoiseModelFactor1<Rot3> {
+class AttitudeFactor {
 
-private:
+protected:
 
-  typedef NoiseModelFactor1<Rot3> Base;
+  const Sphere2 nZ_, bRef_; ///< Position measurement in
-
-  Sphere2 nZ_, bRef_; ///< Position measurement in
 
 public:
 
-  /// shorthand for a smart pointer to a factor
-  typedef boost::shared_ptr<AttitudeFactor> shared_ptr;
-
-  /// Typedef to this class
-  typedef AttitudeFactor This;
-
   /** default constructor - only use for serialization */
   AttitudeFactor() {
   }
 
-  virtual ~AttitudeFactor() {
+  /**
+   * @brief Constructor
+   * @param nZ measured direction in navigation frame
+   * @param bRef reference direction in body frame (default Z-axis)
+   */
+  AttitudeFactor(const Sphere2& nZ, const Sphere2& bRef = Sphere2(0, 0, 1)) :
+      nZ_(nZ), bRef_(bRef) {
+  }
+
+  /** vector of errors */
+  Vector attitudeError(const Rot3& p,
+      boost::optional<Matrix&> H = boost::none) const;
+};
+
+/**
+ * Version of AttitudeFactor for Rot3
+ * @addtogroup Navigation
+ */
+class Rot3AttitudeFactor: public NoiseModelFactor1<Rot3>, public AttitudeFactor {
+
+  typedef NoiseModelFactor1<Rot3> Base;
+
+public:
+
+  /// shorthand for a smart pointer to a factor
+  typedef boost::shared_ptr<Rot3AttitudeFactor> shared_ptr;
+
+  /// Typedef to this class
+  typedef Rot3AttitudeFactor This;
+
+  /** default constructor - only use for serialization */
+  Rot3AttitudeFactor() {
+  }
+
+  virtual ~Rot3AttitudeFactor() {
   }
 
   /**
@@ -61,9 +87,9 @@ public:
    * @param model Gaussian noise model
    * @param bRef reference direction in body frame (default Z-axis)
    */
-  AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model,
+  Rot3AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model,
       const Sphere2& bRef = Sphere2(0, 0, 1)) :
-      Base(model, key), nZ_(nZ), bRef_(bRef) {
+      Base(model, key), AttitudeFactor(nZ, bRef) {
   }
 
   /// @return a deep copy of this factor
@@ -72,8 +98,6 @@ public:
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
-  /** implement functions needed for Testable */
-
   /** print */
   virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const;
@@ -81,11 +105,11 @@ public:
   /** equals */
   virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
 
-  /** implement functions needed to derive from Factor */
-
   /** vector of errors */
-  Vector evaluateError(const Rot3& p,
+  virtual Vector evaluateError(const Rot3& nRb, //
-      boost::optional<Matrix&> H = boost::none) const;
+      boost::optional<Matrix&> H = boost::none) const {
+    return attitudeError(nRb, H);
+  }
 
 private:
 
@@ -102,17 +126,14 @@ private:
 };
 
 /**
- * Version of AttitudeFactor for Pose3 (this should be easier)
+ * Version of AttitudeFactor for Pose3
  * @addtogroup Navigation
  */
-class Pose3AttitudeFactor: public NoiseModelFactor1<Pose3> {
+class Pose3AttitudeFactor: public NoiseModelFactor1<Pose3>,
-
+    public AttitudeFactor {
-private:
 
   typedef NoiseModelFactor1<Pose3> Base;
 
-  Sphere2 nZ_, bRef_; ///< Position measurement in
-
 public:
 
   /// shorthand for a smart pointer to a factor
@@ -137,7 +158,7 @@ public:
    */
   Pose3AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model,
       const Sphere2& bRef = Sphere2(0, 0, 1)) :
-      Base(model, key), nZ_(nZ), bRef_(bRef) {
+      Base(model, key), AttitudeFactor(nZ, bRef) {
   }
 
   /// @return a deep copy of this factor
@@ -146,8 +167,6 @@ public:
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
-  /** implement functions needed for Testable */
-
   /** print */
   virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const;
@@ -155,11 +174,17 @@ public:
   /** equals */
   virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
 
-  /** implement functions needed to derive from Factor */
-
   /** vector of errors */
-  Vector evaluateError(const Pose3& p,
+  virtual Vector evaluateError(const Pose3& nTb, //
-      boost::optional<Matrix&> H = boost::none) const;
+      boost::optional<Matrix&> H = boost::none) const {
+    Vector e = attitudeError(nTb.rotation(), H);
+    if (H) {
+      Matrix H23 = *H;
+      *H = Matrix::Zero(2, 6);
+      H->block<2,3>(0,0) = H23;
+    }
+    return e;
+  }
 
 private:
 

gtsam/navigation/tests/testAttitudeFactor.cpp

@@ -11,7 +11,7 @@
 
 /**
  * @file    testAttitudeFactor.cpp
- * @brief   Unit test for AttitudeFactor
+ * @brief   Unit test for Rot3AttitudeFactor
  * @author  Frank Dellaert
  * @date   January 22, 2014
  */
@@ -25,7 +25,7 @@ using namespace std;
 using namespace gtsam;
 
 // *************************************************************************
-TEST( AttitudeFactor, Constructor ) {
+TEST( Rot3AttitudeFactor, Constructor ) {
 
   // Example: pitch and roll of aircraft in an ENU Cartesian frame.
   // If pitch and roll are zero for an aerospace frame,
@@ -36,8 +36,8 @@ TEST( AttitudeFactor, Constructor ) {
   // Factor
   Key key(1);
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
-  AttitudeFactor factor0(key, nDown, model);
+  Rot3AttitudeFactor factor0(key, nDown, model);
-  AttitudeFactor factor(key, nDown, model, bZ);
+  Rot3AttitudeFactor factor(key, nDown, model, bZ);
   EXPECT(assert_equal(factor0,factor,1e-5));
 
   // Create a linearization point at the zero-error point
@@ -46,7 +46,7 @@ TEST( AttitudeFactor, Constructor ) {
 
   // Calculate numerical derivatives
   Matrix expectedH = numericalDerivative11<Rot3>(
-      boost::bind(&AttitudeFactor::evaluateError, &factor, _1, boost::none),
+      boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none),
       nRb);
 
   // Use the factor to calculate the derivative