moved RangeFactor to SAM

examples/RangeISAMExample_plaza2.cpp

@@ -39,7 +39,7 @@
 // have been provided with the library for solving robotics SLAM problems.
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/RangeFactor.h>
+#include <gtsam/sam/RangeFactor.h>
 #include <gtsam/slam/dataset.h>
 
 // Standard headers, added last, so we know headers above work on their own

gtsam.h

@@ -2262,7 +2262,7 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor {
 };
 
 
-#include <gtsam/slam/RangeFactor.h>
+#include <gtsam/sam/RangeFactor.h>
 template<POSE, POINT>
 virtual class RangeFactor : gtsam::NoiseModelFactor {
   RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);

gtsam/sam/RangeFactor.h

@@ -0,0 +1,145 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  RangeFactor.h
+ *  @brief Serializable factor induced by a range measurement between two points
+ *and/or poses
+ *  @date July 2015
+ *  @author Frank Dellaert
+ **/
+
+#pragma once
+
+#include <gtsam/nonlinear/SerializableExpressionFactor.h>
+#include <gtsam/geometry/concepts.h>
+#include <boost/lexical_cast.hpp>
+
+namespace gtsam {
+
+/**
+ * Binary factor for a range measurement
+ * @addtogroup SAM
+ */
+template <class A1, class A2 = A1>
+struct RangeFactor : public SerializableExpressionFactor2<double, A1, A2> {
+ private:
+  typedef RangeFactor<A1, A2> This;
+  typedef SerializableExpressionFactor2<double, A1, A2> Base;
+
+  // Concept requirements for this factor
+  GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(A1, A2)
+
+ public:
+  /// default constructor
+  RangeFactor() {}
+
+  RangeFactor(Key key1, Key key2, double measured,
+              const SharedNoiseModel& model)
+      : Base(key1, key2, model, measured) {
+    this->initialize(expression(key1, key2));
+  }
+
+  /// @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<double> expression(Key key1, Key key2) const {
+    Expression<A1> t1_(key1);
+    Expression<A2> t2_(key2);
+    return Expression<double>(t1_, &A1::range, t2_);
+  }
+
+  /// print
+  void print(const std::string& s = "",
+             const KeyFormatter& kf = DefaultKeyFormatter) const {
+    std::cout << s << "RangeFactor" << std::endl;
+    Base::print(s, kf);
+  }
+};  // \ RangeFactor
+
+/// traits
+template <class A1, class A2>
+struct traits<RangeFactor<A1, A2> > : public Testable<RangeFactor<A1, A2> > {};
+
+/**
+ * Binary factor for a range measurement, with a transform applied
+ * @addtogroup SAM
+ */
+template <class A1, class A2 = A1>
+class RangeFactorWithTransform
+    : public SerializableExpressionFactor2<double, A1, A2> {
+ private:
+  typedef RangeFactorWithTransform<A1, A2> This;
+  typedef SerializableExpressionFactor2<double, A1, A2> Base;
+
+  A1 body_T_sensor_;  ///< The pose of the sensor in the body frame
+
+  // Concept requirements for this factor
+  GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(A1, A2)
+
+ public:
+  //// Default constructor
+  RangeFactorWithTransform() {}
+
+  RangeFactorWithTransform(Key key1, Key key2, double measured,
+                           const SharedNoiseModel& model,
+                           const A1& body_T_sensor)
+      : Base(key1, key2, model, measured), body_T_sensor_(body_T_sensor) {
+    this->initialize(expression(key1, key2));
+  }
+
+  virtual ~RangeFactorWithTransform() {}
+
+  /// @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<double> expression(Key key1, Key key2) const {
+    Expression<A1> body_T_sensor__(body_T_sensor_);
+    Expression<A1> nav_T_body_(key1);
+    Expression<A1> nav_T_sensor_(traits<A1>::Compose, nav_T_body_,
+                                 body_T_sensor__);
+    Expression<A2> t2_(key2);
+    return Expression<double>(nav_T_sensor_, &A1::range, t2_);
+  }
+
+  /** print contents */
+  void print(const std::string& s = "",
+             const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    std::cout << s << "RangeFactorWithTransform" << std::endl;
+    this->body_T_sensor_.print("  sensor pose in body frame: ");
+    Base::print(s, keyFormatter);
+  }
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "Base", boost::serialization::base_object<Base>(*this));
+    ar& BOOST_SERIALIZATION_NVP(body_T_sensor_);
+  }
+};  // \ RangeFactorWithTransform
+
+/// traits
+template <class A1, class A2>
+struct traits<RangeFactorWithTransform<A1, A2> >
+    : public Testable<RangeFactorWithTransform<A1, A2> > {};
+
+}  // \ namespace gtsam

gtsam/sam/tests/testRangeFactor.cpp

@@ -16,14 +16,15 @@
  *  @date Oct 2012
  */
 
-#include <CppUnitLite/TestHarness.h>
+#include <gtsam/sam/RangeFactor.h>
-#include <gtsam/slam/RangeFactor.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/TestableAssertions.h>
+
+#include <CppUnitLite/TestHarness.h>
 #include <boost/bind.hpp>
 
 using namespace std;

gtsam/slam/RangeFactor.h

@@ -9,189 +9,14 @@
 
  * -------------------------------------------------------------------------- */
 
-/**
- *  @file  RangeFactor.H
- *  @author Frank Dellaert
- **/
-
 #pragma once
 
-#include <gtsam/nonlinear/NonlinearFactor.h>
+#ifdef _MSC_VER
-#include <gtsam/geometry/concepts.h>
+#pragma message("RangeFactor is now an ExpressionFactor in SAM directory")
-#include <boost/lexical_cast.hpp>
+#else
+#warning "RangeFactor is now an ExpressionFactor in SAM directory"
+#endif
 
-namespace gtsam {
 
-/**
+#include <gtsam/sam/RangeFactor.h>
- * Binary factor for a range measurement
- * @addtogroup SLAM
- */
-template<class T1, class T2 = T1>
-class RangeFactor: public NoiseModelFactor2<T1, T2> {
-private:
 
-  double measured_; /** measurement */
-
-  typedef RangeFactor<T1, T2> This;
-  typedef NoiseModelFactor2<T1, T2> Base;
-
-  // Concept requirements for this factor
-  GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(T1, T2)
-
-public:
-
-  RangeFactor() {
-  } /* Default constructor */
-
-  RangeFactor(Key key1, Key key2, double measured,
-      const SharedNoiseModel& model) :
-      Base(model, key1, key2), measured_(measured) {
-  }
-
-  virtual ~RangeFactor() {
-  }
-
-  /// @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)));
-  }
-
-  /** h(x)-z */
-  Vector evaluateError(const T1& t1, const T2& t2, boost::optional<Matrix&> H1 =
-      boost::none, boost::optional<Matrix&> H2 = boost::none) const {
-    double hx;
-    hx = t1.range(t2, H1, H2);
-    return (Vector(1) << hx - measured_).finished();
-  }
-
-  /** return the measured */
-  double measured() const {
-    return measured_;
-  }
-
-  /** equals specialized to this factor */
-  virtual bool equals(const NonlinearFactor& expected,
-      double tol = 1e-9) const {
-    const This *e = dynamic_cast<const This*>(&expected);
-    return e != NULL && Base::equals(*e, tol)
-        && fabs(this->measured_ - e->measured_) < tol;
-  }
-
-  /** print contents */
-  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const {
-    std::cout << s << "RangeFactor, range = " << measured_ << std::endl;
-    Base::print("", keyFormatter);
-  }
-
-private:
-
-  /** Serialization function */
-  friend class boost::serialization::access;
-  template<class 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(measured_);
-  }
-}; // \ RangeFactor
-
-/// traits
-template<class T1, class T2>
-struct traits<RangeFactor<T1,T2> > : public Testable<RangeFactor<T1,T2> > {};
-
-/**
- * Binary factor for a range measurement, with a transform applied
- * @addtogroup SLAM
- */
-template<class POSE, class T2 = POSE>
-class RangeFactorWithTransform: public NoiseModelFactor2<POSE, T2> {
-private:
-
-  double measured_; /** measurement */
-  POSE body_P_sensor_; ///< The pose of the sensor in the body frame
-
-  typedef RangeFactorWithTransform<POSE, T2> This;
-  typedef NoiseModelFactor2<POSE, T2> Base;
-
-  // Concept requirements for this factor
-  GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, T2)
-
-public:
-
-  RangeFactorWithTransform() {
-  } /* Default constructor */
-
-  RangeFactorWithTransform(Key key1, Key key2, double measured,
-      const SharedNoiseModel& model, const POSE& body_P_sensor) :
-      Base(model, key1, key2), measured_(measured), body_P_sensor_(
-          body_P_sensor) {
-  }
-
-  virtual ~RangeFactorWithTransform() {
-  }
-
-  /// @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)));
-  }
-
-  /** h(x)-z */
-  Vector evaluateError(const POSE& t1, const T2& t2,
-      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
-          boost::none) const {
-    double hx;
-    if (H1) {
-      Matrix H0;
-      hx = t1.compose(body_P_sensor_, H0).range(t2, H1, H2);
-      *H1 = *H1 * H0;
-    } else {
-      hx = t1.compose(body_P_sensor_).range(t2, H1, H2);
-    }
-    return (Vector(1) << hx - measured_).finished();
-  }
-
-  /** return the measured */
-  double measured() const {
-    return measured_;
-  }
-
-  /** equals specialized to this factor */
-  virtual bool equals(const NonlinearFactor& expected,
-      double tol = 1e-9) const {
-    const This *e = dynamic_cast<const This*>(&expected);
-    return e != NULL && Base::equals(*e, tol)
-        && fabs(this->measured_ - e->measured_) < tol
-        && body_P_sensor_.equals(e->body_P_sensor_);
-  }
-
-  /** print contents */
-  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const {
-    std::cout << s << "RangeFactor, range = " << measured_ << std::endl;
-    this->body_P_sensor_.print("  sensor pose in body frame: ");
-    Base::print("", keyFormatter);
-  }
-
-private:
-
-  /** Serialization function */
-  friend class boost::serialization::access;
-  template<class 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(measured_);
-    ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
-  }
-}; // \ RangeFactorWithTransform
-
-/// traits
-template<class T1, class T2>
-struct traits<RangeFactorWithTransform<T1, T2> > : public Testable<RangeFactorWithTransform<T1, T2> > {};
-
-} // \ namespace gtsam

gtsam/slam/tests/testGeneralSFMFactor.cpp

@@ -17,7 +17,7 @@
  */
 
 #include <gtsam/slam/GeneralSFMFactor.h>
-#include <gtsam/slam/RangeFactor.h>
+#include <gtsam/sam/RangeFactor.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Rot2.h>

gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp

@@ -17,7 +17,7 @@
  */
 
 #include <gtsam/slam/GeneralSFMFactor.h>
-#include <gtsam/slam/RangeFactor.h>
+#include <gtsam/sam/RangeFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/inference/Symbol.h>

gtsam_unstable/dynamics/tests/testIMUSystem.cpp

@@ -9,7 +9,7 @@
 
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/PriorFactor.h>
-#include <gtsam/slam/RangeFactor.h>
+#include <gtsam/sam/RangeFactor.h>
 #include <gtsam_unstable/slam/PartialPriorFactor.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

gtsam_unstable/examples/SmartRangeExample_plaza1.cpp

@@ -39,7 +39,7 @@
 // have been provided with the library for solving robotics SLAM problems.
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/RangeFactor.h>
+#include <gtsam/sam/RangeFactor.h>
 #include <gtsam_unstable/slam/SmartRangeFactor.h>
 
 // Standard headers, added last, so we know headers above work on their own

gtsam_unstable/examples/SmartRangeExample_plaza2.cpp

@@ -39,7 +39,7 @@
 // have been provided with the library for solving robotics SLAM problems.
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/RangeFactor.h>
+#include <gtsam/sam/RangeFactor.h>
 
 // Standard headers, added last, so we know headers above work on their own
 #include <boost/foreach.hpp>

gtsam_unstable/gtsam_unstable.h

@@ -367,7 +367,7 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor {
 
 };
 
-#include <gtsam/slam/RangeFactor.h>
+#include <gtsam/sam/RangeFactor.h>
 template<POSE, POINT>
 virtual class RangeFactor : gtsam::NonlinearFactor {
   RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);

gtsam_unstable/slam/serialization.cpp

@@ -17,7 +17,7 @@
 #include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
-#include <gtsam/slam/RangeFactor.h>
+#include <gtsam/sam/RangeFactor.h>
 #include <gtsam/slam/StereoFactor.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/inference/Symbol.h>

tests/testGaussianFactorGraphB.cpp

@@ -532,7 +532,7 @@ TEST(GaussianFactorGraph, hasConstraints)
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/slam/PriorFactor.h>
-#include <gtsam/slam/RangeFactor.h>
+#include <gtsam/sam/RangeFactor.h>
 
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, conditional_sigma_failure) {

tests/testSerializationSLAM.cpp

@@ -29,7 +29,7 @@
 //#include <gtsam/slam/PartialPriorFactor.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
-#include <gtsam/slam/RangeFactor.h>
+#include <gtsam/sam/RangeFactor.h>
 #include <gtsam/slam/StereoFactor.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/inference/Symbol.h>