Switched to Rot3 and added Pose3 version

gtsam/navigation/AttitudeFactor.cpp

@@ -39,7 +39,40 @@ bool AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const {
 }
 
 //***************************************************************************
-Vector AttitudeFactor::evaluateError(const Pose3& p,
+Vector AttitudeFactor::evaluateError(const Rot3& nRb,
+    boost::optional<Matrix&> H) const {
+  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, 3);
+    H->block < 2, 3 > (0, 0) = D_e_nRef * D_nRef_R;
+    return e;
+  } else {
+    Sphere2 nRef = nRb * bRef_;
+    return nZ_.error(nRef);
+  }
+}
+
+//***************************************************************************
+void Pose3AttitudeFactor::print(const string& s,
+    const KeyFormatter& keyFormatter) const {
+  cout << s << "Pose3AttitudeFactor 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 Pose3AttitudeFactor::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 Pose3AttitudeFactor::evaluateError(const Pose3& p,
     boost::optional<Matrix&> H) const {
   const Rot3& nRb = p.rotation();
   if (H) {

gtsam/navigation/AttitudeFactor.h

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- *  @file   AttitudeFactor.h
+ *  @file   Pose3AttitudeFactor.h
  *  @author Frank Dellaert
  *  @brief  Header file for Attitude factor
  *  @date   January 28, 2014
@@ -24,18 +24,18 @@
 namespace gtsam {
 
 /**
- * Prior on the attitude of a Pose3.
+ * Prior on the attitude of a Rot3.
  * 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<Pose3> {
+class AttitudeFactor: public NoiseModelFactor1<Rot3> {
 
 private:
 
-  typedef NoiseModelFactor1<Pose3> Base;
+  typedef NoiseModelFactor1<Rot3> Base;
 
   Sphere2 nZ_, bRef_; ///< Position measurement in
 
@@ -54,6 +54,80 @@ public:
   virtual ~AttitudeFactor() {
   }
 
+  /**
+   * @brief Constructor
+   * @param key of the Rot3 variable that will be constrained
+   * @param nZ measured direction in navigation frame
+   * @param model Gaussian noise model
+   * @param bRef reference direction in body frame (default Z-axis)
+   */
+  AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model,
+      const Sphere2& bRef = Sphere2(0, 0, 1)) :
+      Base(model, key), nZ_(nZ), bRef_(bRef) {
+  }
+
+  /// @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(new This(*this)));
+  }
+
+  /** implement functions needed for Testable */
+
+  /** print */
+  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+      DefaultKeyFormatter) const;
+
+  /** 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,
+      boost::optional<Matrix&> H = boost::none) const;
+
+private:
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar
+        & boost::serialization::make_nvp("NoiseModelFactor1",
+            boost::serialization::base_object<Base>(*this));
+    ar & BOOST_SERIALIZATION_NVP(nZ_);
+    ar & BOOST_SERIALIZATION_NVP(bRef_);
+  }
+};
+
+/**
+ * Version of AttitudeFactor for Pose3 (this should be easier)
+ * @addtogroup Navigation
+ */
+class Pose3AttitudeFactor: public NoiseModelFactor1<Pose3> {
+
+private:
+
+  typedef NoiseModelFactor1<Pose3> Base;
+
+  Sphere2 nZ_, bRef_; ///< Position measurement in
+
+public:
+
+  /// shorthand for a smart pointer to a factor
+  typedef boost::shared_ptr<Pose3AttitudeFactor> shared_ptr;
+
+  /// Typedef to this class
+  typedef Pose3AttitudeFactor This;
+
+  /** default constructor - only use for serialization */
+  Pose3AttitudeFactor() {
+  }
+
+  virtual ~Pose3AttitudeFactor() {
+  }
+
   /**
    * @brief Constructor
    * @param key of the Pose3 variable that will be constrained
@@ -61,8 +135,8 @@ public:
    * @param model Gaussian noise model
    * @param bRef reference direction in body frame (default Z-axis)
    */
-  AttitudeFactor(Key key, const Sphere2& nZ,
+  Pose3AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model,
-      const SharedNoiseModel& model, const Sphere2& bRef=Sphere2(0,0,1)) :
+      const Sphere2& bRef = Sphere2(0, 0, 1)) :
       Base(model, key), nZ_(nZ), bRef_(bRef) {
   }
 

gtsam/navigation/tests/testAttitudeFactor.cpp

@@ -40,13 +40,47 @@ TEST( AttitudeFactor, Constructor ) {
   AttitudeFactor factor(key, nDown, model, bZ);
   EXPECT(assert_equal(factor0,factor,1e-5));
 
+  // Create a linearization point at the zero-error point
+  Rot3 nRb;
+  EXPECT(assert_equal(zero(2),factor.evaluateError(nRb),1e-5));
+
+  // Calculate numerical derivatives
+  Matrix expectedH = numericalDerivative11<Rot3>(
+      boost::bind(&AttitudeFactor::evaluateError, &factor, _1, boost::none),
+      nRb);
+
+  // Use the factor to calculate the derivative
+  Matrix actualH;
+  factor.evaluateError(nRb, actualH);
+
+  // Verify we get the expected error
+  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+}
+
+// *************************************************************************
+TEST( Pose3AttitudeFactor, Constructor ) {
+
+  // Example: pitch and roll of aircraft in an ENU Cartesian frame.
+  // If pitch and roll are zero for an aerospace frame,
+  // that means Z is pointing down, i.e., direction of Z = (0,0,-1)
+  Sphere2 bZ(0, 0, 1); // reference direction is body Z axis
+  Sphere2 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement"
+
+  // Factor
+  Key key(1);
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
+  Pose3AttitudeFactor factor0(key, nDown, model);
+  Pose3AttitudeFactor factor(key, nDown, model, bZ);
+  EXPECT(assert_equal(factor0,factor,1e-5));
+
   // Create a linearization point at the zero-error point
   Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0));
   EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5));
 
   // Calculate numerical derivatives
   Matrix expectedH = numericalDerivative11<Pose3>(
-      boost::bind(&AttitudeFactor::evaluateError, &factor, _1, boost::none), T);
+      boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1,
+          boost::none), T);
 
   // Use the factor to calculate the derivative
   Matrix actualH;