From 850c8a792105fd2d6ceca59b3000de0ea7ba7b75 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Sun, 12 Jul 2015 20:51:01 -0700
Subject: [PATCH] Fixed BearingRangeFactor

---
 gtsam/sam/BearingRangeFactor.h             | 92 +++-------------------
 gtsam/sam/tests/testBearingRangeFactor.cpp |  2 -
 2 files changed, 13 insertions(+), 81 deletions(-)

diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h
index 6ef1d7e55..1f4122255 100644
--- a/gtsam/sam/BearingRangeFactor.h
+++ b/gtsam/sam/BearingRangeFactor.h
@@ -20,50 +20,11 @@
 #pragma once
 
 #include <gtsam/nonlinear/SerializableExpressionFactor.h>
-#include <gtsam/base/Testable.h>
+#include <gtsam/geometry/BearingRange.h>
 #include <boost/concept/assert.hpp>
 
 namespace gtsam {
 
-// forward declaration of Bearing functor, assumed partially specified
-template <typename A1, typename A2>
-struct Bearing;
-
-// forward declaration of Range functor, assumed partially specified
-template <typename A1, typename A2>
-struct Range;
-
-template <typename B, typename R>
-struct BearingRange : public ProductManifold<B, R> {
-  BearingRange() {}
-  BearingRange(const ProductManifold<B, R>& br) : ProductManifold<B, R>(br) {}
-  BearingRange(const B& b, const R& r) : ProductManifold<B, R>(b, r) {}
-
- private:
-  /// Serialization function
-  template <class ARCHIVE>
-  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
-    ar& boost::serialization::make_nvp("bearing", this->first);
-    ar& boost::serialization::make_nvp("range", this->second);
-  }
-
-  friend class boost::serialization::access;
-};
-
-template <typename B, typename R>
-struct traits<BearingRange<B, R> >
-    : internal::ManifoldTraits<BearingRange<B, R> > {
-  static void Print(const BearingRange<B, R>& m, const std::string& str = "") {
-    traits<B>::Print(m.first, str);
-    traits<R>::Print(m.second, str);
-  }
-  static bool Equals(const BearingRange<B, R>& m1, const BearingRange<B, R>& m2,
-                     double tol = 1e-8) {
-    return traits<B>::Equals(m1.first, m2.first, tol) &&
-           traits<R>::Equals(m1.second, m2.second, tol);
-  }
-};
-
 /**
  * Binary factor for a bearing/range measurement
  * @addtogroup SLAM
@@ -72,25 +33,21 @@ template <typename A1, typename A2,
           typename B = typename Bearing<A1, A2>::result_type,
           typename R = typename Range<A1, A2>::result_type>
 class BearingRangeFactor
-    : public SerializableExpressionFactor2<BearingRange<B, R>, A1, A2> {
- public:
-  typedef BearingRange<B, R> T;
+    : public SerializableExpressionFactor2<BearingRange<A1, A2>, A1, A2> {
+ private:
+  typedef BearingRange<A1, A2> T;
   typedef SerializableExpressionFactor2<T, A1, A2> Base;
   typedef BearingRangeFactor<A1, A2> This;
-  typedef boost::shared_ptr<This> shared_ptr;
-
- private:
-  /** concept check by type */
-  BOOST_CONCEPT_ASSERT((IsTestable<B>));
-  BOOST_CONCEPT_ASSERT((IsTestable<R>));
 
  public:
+  typedef boost::shared_ptr<This> shared_ptr;
+
   /// default constructor
   BearingRangeFactor() {}
 
   /// primary constructor
   BearingRangeFactor(Key key1, Key key2, const B& measuredBearing,
-                     const R measuredRange, const SharedNoiseModel& model)
+                     const R& measuredRange, const SharedNoiseModel& model)
       : Base(key1, key2, model, T(measuredBearing, measuredRange)) {
     this->initialize(expression(key1, key2));
   }
@@ -105,37 +62,14 @@ class BearingRangeFactor
 
   // Return measurement expression
   virtual Expression<T> expression(Key key1, Key key2) const {
-    Expression<A1> a1_(key1);
-    Expression<A2> a2_(key2);
-    return Expression<T>(This::Predict, a1_, a2_);
+    return Expression<T>(T::Measure, Expression<A1>(key1), Expression<A2>(key2));
   }
 
-  /** Print */
-  virtual void print(
-      const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-    std::cout << s << "BearingRangeFactor(" << keyFormatter(this->keys_[0])
-              << "," << keyFormatter(this->keys_[1]) << ")\n";
-    traits<B>::Print(this->measured_.first, "measured bearing: ");
-    traits<R>::Print(this->measured_.second, "measured range: ");
-    this->noiseModel_->print("noise model:\n");
-  }
-
-  /// Prediction function that stacks measurements
-  static T Predict(const A1& pose, const A2& point,
-                   typename MakeOptionalJacobian<T, A1>::type H1,
-                   typename MakeOptionalJacobian<T, A2>::type H2) {
-    typename MakeJacobian<B, A1>::type HB1;
-    typename MakeJacobian<B, A2>::type HB2;
-    typename MakeJacobian<R, A1>::type HR1;
-    typename MakeJacobian<R, A2>::type HR2;
-
-    B b = Bearing<A1, A2>()(pose, point, H1 ? &HB1 : 0, H2 ? &HB2 : 0);
-    R r = Range<A1, A2>()(pose, point, H1 ? &HR1 : 0, H2 ? &HR2 : 0);
-
-    if (H1) *H1 << HB1, HR1;
-    if (H2) *H2 << HB2, HR2;
-    return T(b, r);
+  /// print
+  virtual void print(const std::string& s = "",
+                     const KeyFormatter& kf = DefaultKeyFormatter) const {
+    std::cout << s << "BearingRangeFactor" << std::endl;
+    Base::print(s, kf);
   }
 
 };  // BearingRangeFactor
diff --git a/gtsam/sam/tests/testBearingRangeFactor.cpp b/gtsam/sam/tests/testBearingRangeFactor.cpp
index 8f12988b4..dfc782838 100644
--- a/gtsam/sam/tests/testBearingRangeFactor.cpp
+++ b/gtsam/sam/tests/testBearingRangeFactor.cpp
@@ -34,12 +34,10 @@ Key poseKey(1);
 Key pointKey(2);
 
 typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
-BearingRangeFactor2D::T measurement2D(1, 2);
 static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5));
 BearingRangeFactor2D factor2D(poseKey, pointKey, 1, 2, model2D);
 
 typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
-BearingRangeFactor3D::T measurement3D(Pose3().bearing(Point3(1, 0, 0)), 0);
 static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5));
 BearingRangeFactor3D factor3D(poseKey, pointKey,
                               Pose3().bearing(Point3(1, 0, 0)), 1, model3D);