From 8fbab3bde85246e4226117bfafc76610692ec11b Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 29 Jan 2014 13:34:52 -0500
Subject: [PATCH] Changed API slightly and made very clear what is measurement
 (typically in nav frame) and what is reference direction (typically in body
 frame)

---
 gtsam/navigation/AttitudeFactor.cpp           | 22 +++++++++----------
 gtsam/navigation/AttitudeFactor.h             | 16 +++++++-------
 gtsam/navigation/tests/testAttitudeFactor.cpp |  8 ++++---
 3 files changed, 24 insertions(+), 22 deletions(-)

diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp
index ddfedd1b9..de8eaf3bf 100644
--- a/gtsam/navigation/AttitudeFactor.cpp
+++ b/gtsam/navigation/AttitudeFactor.cpp
@@ -26,33 +26,33 @@ namespace gtsam {
 void AttitudeFactor::print(const string& s,
     const KeyFormatter& keyFormatter) const {
   cout << s << "AttitudeFactor on " << keyFormatter(this->key()) << "\n";
-  z_.print("  measured direction: ");
-  ref_.print("  reference direction: ");
+  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->z_.equals(e->z_, tol)
-      && this->ref_.equals(e->ref_, tol);
+  return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
+      && this->bRef_.equals(e->bRef_, tol);
 }
 
 //***************************************************************************
 Vector AttitudeFactor::evaluateError(const Pose3& p,
     boost::optional<Matrix&> H) const {
-  const Rot3& R = p.rotation();
+  const Rot3& nRb = p.rotation();
   if (H) {
-    Matrix D_q_R, D_e_q;
-    Sphere2 q = R.rotate(z_, D_q_R);
-    Vector e = ref_.error(q, D_e_q);
+    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_q * D_q_R;
+    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 q = R * z_;
-    return ref_.error(q);
+    Sphere2 nRef = nRb * bRef_;
+    return nZ_.error(nRef);
   }
 }
 
diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h
index 6f97f539a..e622a7e37 100644
--- a/gtsam/navigation/AttitudeFactor.h
+++ b/gtsam/navigation/AttitudeFactor.h
@@ -37,7 +37,7 @@ private:
 
   typedef NoiseModelFactor1<Pose3> Base;
 
-  Sphere2 z_, ref_; ///< Position measurement in
+  Sphere2 nZ_, bRef_; ///< Position measurement in
 
 public:
 
@@ -57,13 +57,13 @@ public:
   /**
    * @brief Constructor
    * @param key of the Pose3 variable that will be constrained
-   * @param z measured direction in body frame
-   * @param ref reference direction in navigation frame
+   * @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& z, const Sphere2& ref,
-      const SharedNoiseModel& model) :
-      Base(model, key), z_(z), ref_(ref) {
+  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
@@ -96,8 +96,8 @@ private:
     ar
         & boost::serialization::make_nvp("NoiseModelFactor1",
             boost::serialization::base_object<Base>(*this));
-    ar & BOOST_SERIALIZATION_NVP(z_);
-    ar & BOOST_SERIALIZATION_NVP(ref_);
+    ar & BOOST_SERIALIZATION_NVP(nZ_);
+    ar & BOOST_SERIALIZATION_NVP(bRef_);
   }
 };
 
diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp
index d079e0058..d8d87ecc0 100644
--- a/gtsam/navigation/tests/testAttitudeFactor.cpp
+++ b/gtsam/navigation/tests/testAttitudeFactor.cpp
@@ -30,13 +30,15 @@ TEST( AttitudeFactor, 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); // body Z axis
-  Sphere2 nDown(0, 0, -1); // down, in ENU navigation frame
+  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(3, 0.25);
-  AttitudeFactor factor(key, bZ, nDown, model);
+  AttitudeFactor factor0(key, nDown, model);
+  AttitudeFactor 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));