Moved BearingRangeFactor to SAM

examples/PlanarSLAMExample.cpp

@@ -42,7 +42,7 @@
 // Also, we will initialize the robot at the origin using a Prior factor.
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
 
 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
 // are nonlinear factors, we will need a Nonlinear Factor Graph.

examples/SolverComparer.cpp

@@ -32,7 +32,7 @@
 */
 
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/geometry/Pose2.h>

gtsam.h

@@ -2290,7 +2290,7 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {
 typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
 
 
-#include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
 template<POSE, POINT, BEARING>
 virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
   BearingRangeFactor(size_t poseKey, size_t pointKey, const BEARING& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);

gtsam/sam/BearingRangeFactor.h

@@ -0,0 +1,148 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file BearingRangeFactor.h
+ * @date Apr 1, 2010
+ * @author Kai Ni
+ * @brief a single factor contains both the bearing and the range to prevent
+ * handle to pair bearing and range factors
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/SerializableExpressionFactor.h>
+#include <gtsam/base/Testable.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
+ */
+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;
+  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:
+  /// default constructor
+  BearingRangeFactor() {}
+
+  /// primary constructor
+  BearingRangeFactor(Key key1, Key key2, const B& measuredBearing,
+                     const R measuredRange, const SharedNoiseModel& model)
+      : Base(key1, key2, model, T(measuredBearing, measuredRange)) {
+    this->initialize(expression(key1, key2));
+  }
+
+  virtual ~BearingRangeFactor() {}
+
+  /// @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)));
+  }
+
+  // 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_);
+  }
+
+  /** 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);
+  }
+
+};  // BearingRangeFactor
+
+/// traits
+template <typename A1, typename A2, typename B, typename R>
+struct traits<BearingRangeFactor<A1, A2, B, R> >
+    : public Testable<BearingRangeFactor<A1, A2, B, R> > {};
+
+}  // namespace gtsam

gtsam/sam/tests/testBearingRangeFactor.cpp

@@ -0,0 +1,109 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file  testBearingRangeFactor.cpp
+ *  @brief Unit tests for BearingRangeFactor Class
+ *  @author Frank Dellaert
+ *  @date July 2015
+ */
+
+#include <gtsam/sam/BearingRangeFactor.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/nonlinear/factorTesting.h>
+#include <gtsam/nonlinear/expressionTesting.h>
+#include <gtsam/base/serialization.h>
+#include <gtsam/base/serializationTestHelpers.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using namespace serializationTestHelpers;
+
+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);
+
+/* ************************************************************************* */
+// Export Noisemodels
+// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
+BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
+
+/* ************************************************************************* */
+TEST(BearingRangeFactor, Serialization2D) {
+  EXPECT(equalsObj<BearingRangeFactor2D>(factor2D));
+  EXPECT(equalsXML<BearingRangeFactor2D>(factor2D));
+  EXPECT(equalsBinary<BearingRangeFactor2D>(factor2D));
+}
+
+/* ************************************************************************* */
+TEST(BearingRangeFactor, 2D) {
+  // Serialize the factor
+  std::string serialized = serializeXML(factor2D);
+
+  // And de-serialize it
+  BearingRangeFactor2D factor;
+  deserializeXML(serialized, factor);
+
+  // Set the linearization point
+  Values values;
+  values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
+  values.insert(pointKey, Point2(-4.0, 11.0));
+
+  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey),
+                                      values, 1e-7, 1e-5);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+TEST(BearingRangeFactor, Serialization3D) {
+  EXPECT(equalsObj<BearingRangeFactor3D>(factor3D));
+  EXPECT(equalsXML<BearingRangeFactor3D>(factor3D));
+  EXPECT(equalsBinary<BearingRangeFactor3D>(factor3D));
+}
+
+/* ************************************************************************* */
+TEST(BearingRangeFactor, 3D) {
+  // Serialize the factor
+  std::string serialized = serializeXML(factor3D);
+
+  // And de-serialize it
+  BearingRangeFactor3D factor;
+  deserializeXML(serialized, factor);
+
+  // Set the linearization point
+  Values values;
+  values.insert(poseKey, Pose3());
+  values.insert(pointKey, Point3(1, 0, 0));
+
+  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey),
+                                      values, 1e-7, 1e-5);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/slam/BearingRangeFactor.h

@@ -9,130 +9,13 @@
 
  * -------------------------------------------------------------------------- */
 
-/**
- * @file BearingRangeFactor.h
- * @date Apr 1, 2010
- * @author Kai Ni
- * @brief a single factor contains both the bearing and the range to prevent
- * handle to pair bearing and range factors
- */
-
 #pragma once
 
-#include <gtsam/nonlinear/NonlinearFactor.h>
+#ifdef _MSC_VER
-#include <gtsam/base/OptionalJacobian.h>
+#pragma message( \
-#include <gtsam/base/Testable.h>
+    "BearingRangeFactor is now an ExpressionFactor in SAM directory")
+#else
+#warning "BearingRangeFactor is now an ExpressionFactor in SAM directory"
+#endif
 
-#include <boost/concept/assert.hpp>
+#include <gtsam/sam/BearingRangeFactor.h>
-
-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;
-
-/**
- * Binary factor for a bearing/range measurement
- * @addtogroup SLAM
- */
-template <typename A1, typename A2,
-          typename BEARING = typename Bearing<A1, A2>::result_type,
-          typename RANGE = typename Range<A1, A2>::result_type>
-class BearingRangeFactor : public NoiseModelFactor2<A1, A2> {
- public:
-  typedef BearingRangeFactor<A1, A2> This;
-  typedef NoiseModelFactor2<A1, A2> Base;
-  typedef boost::shared_ptr<This> shared_ptr;
-
- private:
-  // the measurement
-  BEARING measuredBearing_;
-  RANGE measuredRange_;
-
-  /** concept check by type */
-  BOOST_CONCEPT_ASSERT((IsTestable<BEARING>));
-  BOOST_CONCEPT_ASSERT((IsTestable<RANGE>));
-
- public:
-  BearingRangeFactor() {} /* Default constructor */
-  BearingRangeFactor(Key poseKey, Key pointKey, const BEARING& measuredBearing,
-                     const RANGE measuredRange, const SharedNoiseModel& model)
-      : Base(model, poseKey, pointKey),
-        measuredBearing_(measuredBearing),
-        measuredRange_(measuredRange) {}
-
-  virtual ~BearingRangeFactor() {}
-
-  /// @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)));
-  }
-
-  /** Print */
-  virtual void print(
-      const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-    std::cout << s << "BearingRangeFactor(" << keyFormatter(this->key1()) << ","
-              << keyFormatter(this->key2()) << ")\n";
-    measuredBearing_.print("measured bearing: ");
-    std::cout << "measured range: " << measuredRange_ << std::endl;
-    this->noiseModel_->print("noise model:\n");
-  }
-
-  /** equals */
-  virtual bool equals(const NonlinearFactor& expected, RANGE tol = 1e-9) const {
-    const This* e = dynamic_cast<const This*>(&expected);
-    return e != NULL && Base::equals(*e, tol) &&
-           fabs(this->measuredRange_ - e->measuredRange_) < tol &&
-           this->measuredBearing_.equals(e->measuredBearing_, tol);
-  }
-
-  /** h(x)-z -> between(z,h(x)) for BEARING manifold */
-  Vector evaluateError(const A1& pose, const A2& point,
-                       boost::optional<Matrix&> H1,
-                       boost::optional<Matrix&> H2) const {
-    typename MakeJacobian<BEARING, A1>::type HB1;
-    typename MakeJacobian<BEARING, A2>::type HB2;
-    typename MakeJacobian<RANGE, A1>::type HR1;
-    typename MakeJacobian<RANGE, A2>::type HR2;
-
-    BEARING y1 = Bearing<A1, A2>()(pose, point, H1 ? &HB1 : 0, H2 ? &HB2 : 0);
-    Vector e1 = traits<BEARING>::Logmap(traits<BEARING>::Between(measuredBearing_,y1));
-
-    RANGE y2 = Range<A1, A2>()(pose, point, H1 ? &HR1 : 0, H2 ? &HR2 : 0);
-    Vector e2 = traits<RANGE>::Logmap(traits<RANGE>::Between(measuredRange_, y2));
-
-    if (H1) {
-      H1->resize(HB1.RowsAtCompileTime + HR1.RowsAtCompileTime, HB1.ColsAtCompileTime);
-      *H1 << HB1, HR1;
-    }
-    if (H2) {
-      H2->resize(HB2.RowsAtCompileTime + HR2.RowsAtCompileTime, HB2.ColsAtCompileTime);
-      *H2 << HB2, HR2;
-    }
-    return concatVectors(2, &e1, &e2);
-  }
-
-  /** return the measured */
-  const std::pair<BEARING, RANGE> measured() const {
-    return std::make_pair(measuredBearing_, measuredRange_);
-  }
-
- private:
-  /** Serialization function */
-  friend typename boost::serialization::access;
-  template <typename ARCHIVE>
-  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
-    ar& boost::serialization::make_nvp(
-        "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
-    ar& BOOST_SERIALIZATION_NVP(measuredBearing_);
-    ar& BOOST_SERIALIZATION_NVP(measuredRange_);
-  }
-};  // BearingRangeFactor
-
-}  // namespace gtsam

gtsam/slam/dataset.cpp

@@ -15,7 +15,7 @@
  * @brief utility functions for loading datasets
  */
 
-#include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/geometry/Point3.h>

gtsam_unstable/slam/serialization.cpp

@@ -11,7 +11,7 @@
 #include <gtsam/base/serialization.h>
 
 //#include <gtsam/slam/AntiFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 //#include <gtsam/slam/BoundingConstraint.h>
 #include <gtsam/slam/GeneralSFMFactor.h>

gtsam_unstable/slam/tests/testSerialization.cpp

@@ -17,7 +17,7 @@
 
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
 
 #include <stdlib.h>
 #include <fstream>

tests/testGaussianISAM2.cpp

@@ -9,7 +9,7 @@
 #include <tests/smallExample.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/Values.h>

tests/testGaussianJunctionTreeB.cpp

@@ -16,7 +16,7 @@
  */
 
 #include <tests/smallExample.h>
-#include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/geometry/Pose2.h>

tests/testMarginals.cpp

@@ -31,7 +31,7 @@
 // add in headers for specific factors
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
 
 #include <gtsam/nonlinear/Marginals.h>
 

tests/testNonlinearISAM.cpp

@@ -6,7 +6,7 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearISAM.h>

tests/testSerializationSLAM.cpp

@@ -22,7 +22,7 @@
 
 #include <tests/smallExample.h>
 //#include <gtsam/slam/AntiFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 //#include <gtsam/slam/BoundingConstraint.h>
 #include <gtsam/slam/GeneralSFMFactor.h>

timing/timeIncremental.cpp

@@ -17,7 +17,7 @@
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/ISAM2.h>