Merged in feature/sam_sfm_directories (pull request #174)

[CI Skip] First Push 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/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

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

@@ -1,4 +1,5 @@
 /**
+
  * GTSAM Wrap Module Definition
  *
  * These are the current classes available through the matlab toolbox interface,
@@ -156,7 +157,7 @@ virtual class Value {
   size_t dim() const;
 };
 
-#include <gtsam/base/LieScalar_Deprecated.h>
+#include <gtsam/base/deprecated/LieScalar.h>
 class LieScalar {
   // Standard constructors
   LieScalar();
@@ -185,7 +186,7 @@ class LieScalar {
   static Vector Logmap(const gtsam::LieScalar& p);
 };
 
-#include <gtsam/base/LieVector_Deprecated.h>
+#include <gtsam/base/deprecated/LieVector.h>
 class LieVector {
   // Standard constructors
   LieVector();
@@ -217,7 +218,7 @@ class LieVector {
   void serialize() const;
 };
 
-#include <gtsam/base/LieMatrix_Deprecated.h>
+#include <gtsam/base/deprecated/LieMatrix.h>
 class LieMatrix {
   // Standard constructors
   LieMatrix();
@@ -2262,7 +2263,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);
@@ -2272,20 +2273,16 @@ typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
 typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
-
+typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
-// Commented out by Frank Dec 2014: not poses!
+typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
-// If needed, we need a RangeFactor that takes a camera, extracts the pose
+typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
-// Should be easy with Expressions
+typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
-//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
-//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
-//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
-//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
 
 
-#include <gtsam/slam/BearingFactor.h>
+#include <gtsam/sam/BearingFactor.h>
-template<POSE, POINT, ROTATION>
+template<POSE, POINT, BEARING>
 virtual class BearingFactor : gtsam::NoiseModelFactor {
-  BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel);
+  BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel);
 
   // enabling serialization functionality
   void serialize() const;
@@ -2293,19 +2290,18 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {
 
 typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
 
-
+#include <gtsam/sam/BearingRangeFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
+template<POSE, POINT, BEARING, RANGE>
-template<POSE, POINT, ROTATION>
 virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
-  BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);
+  BearingRangeFactor(size_t poseKey, size_t pointKey,
-
+      const BEARING& measuredBearing, const RANGE& measuredRange,
-  pair<ROTATION, double> measured() const;
+      const gtsam::noiseModel::Base* noiseModel);
 
   // enabling serialization functionality
   void serialize() const;
 };
 
-typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingRangeFactor2D;
+typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRangeFactor2D;
 
 
 #include <gtsam/slam/ProjectionFactor.h>

gtsam/CMakeLists.txt

@@ -9,7 +9,10 @@ set (gtsam_subdirs
     discrete
     linear
     nonlinear
+    sam
+    sfm
     slam
+    smart
 	navigation
 )
 

gtsam/base/CMakeLists.txt

@@ -5,5 +5,8 @@ install(FILES ${base_headers} DESTINATION include/gtsam/base)
 file(GLOB base_headers_tree "treeTraversal/*.h")
 install(FILES ${base_headers_tree} DESTINATION include/gtsam/base/treeTraversal)
 
+file(GLOB deprecated_headers "deprecated/*.h")
+install(FILES ${deprecated_headers} DESTINATION include/gtsam/base/deprecated)
+
 # Build tests
 add_subdirectory(tests)

gtsam/base/GenericValue.h

@@ -194,6 +194,11 @@ public:
 
 };
 
+// traits
+template <typename ValueType>
+struct traits<GenericValue<ValueType> >
+    : public Testable<GenericValue<ValueType> > {};
+
 // define Value::cast here since now GenericValue has been declared
 template<typename ValueType>
 const ValueType& Value::cast() const {

gtsam/base/LieMatrix.cpp

@@ -1,27 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 LieMatrix.cpp
- * @brief A wrapper around Matrix providing Lie compatibility
- * @author Richard Roberts and Alex Cunningham
- */
-
-#include <gtsam/base/LieMatrix_Deprecated.h>
-
-namespace gtsam {
-
-/* ************************************************************************* */
-void LieMatrix::print(const std::string& name) const {
-  gtsam::print(matrix(), name);
-}
-
-}

gtsam/base/LieMatrix.h

@@ -11,7 +11,7 @@
 
 /**
  * @file LieMatrix.h
- * @brief External deprecation warning, see LieMatrix_Deprecated.h for details
+ * @brief External deprecation warning, see deprecated/LieMatrix.h for details
  * @author Paul Drews
  */
 
@@ -23,4 +23,4 @@
 #warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead."
 #endif
 
-#include "gtsam/base/LieMatrix_Deprecated.h"
+#include "gtsam/base/deprecated/LieMatrix.h"

gtsam/base/LieScalar.cpp

@@ -1,17 +0,0 @@
-/*
- * LieScalar.cpp
- *
- *  Created on: Apr 12, 2013
- *      Author: thduynguyen
- */
-
-
-
-
-#include <gtsam/base/LieScalar_Deprecated.h>
-
-namespace gtsam {
-  void LieScalar::print(const std::string& name) const {
-    std::cout << name << ": " << d_ << std::endl;
-  }
-}

gtsam/base/LieScalar.h

@@ -11,7 +11,7 @@
 
 /**
  * @file LieScalar.h
- * @brief External deprecation warning, see LieScalar_Deprecated.h for details
+ * @brief External deprecation warning, see deprecated/LieScalar.h for details
  * @author Kai Ni
  */
 
@@ -23,4 +23,4 @@
   #warning "LieScalar.h is deprecated. Please use double/float instead."
 #endif
 
-#include <gtsam/base/LieScalar_Deprecated.h>
+#include <gtsam/base/deprecated/LieScalar.h>

gtsam/base/LieVector.cpp

@@ -1,40 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 LieVector.cpp
- * @brief Implementations for LieVector functions
- * @author Alex Cunningham
- */
-
-#include <gtsam/base/LieVector_Deprecated.h>
-#include <cstdarg>
-
-using namespace std;
-
-namespace gtsam {
-
-/* ************************************************************************* */
-LieVector::LieVector(size_t m, const double* const data)
-: Vector(m)
-{
-  for(size_t i = 0; i < m; i++)
-    (*this)(i) = data[i];
-}
-
-/* ************************************************************************* */
-void LieVector::print(const std::string& name) const {
-  gtsam::print(vector(), name);
-}
-
-// Does not compile because LieVector is not fixed size.
-// GTSAM_CONCEPT_LIE_INST(LieVector)
-} // \namespace gtsam

gtsam/base/LieVector.h

@@ -11,7 +11,7 @@
 
 /**
  * @file LieVector.h
- * @brief Deprecation warning for LieVector_Deprecated.h, see LieVector_Deprecated.h for details.
+ * @brief Deprecation warning for LieVector, see deprecated/LieVector.h for details.
  * @author Paul Drews
  */
 
@@ -23,4 +23,4 @@
 #warning "LieVector.h is deprecated. Please use Eigen::Vector instead."
 #endif
 
-#include <gtsam/base/LieVector_Deprecated.h>
+#include <gtsam/base/deprecated/LieVector.h>

gtsam/base/Manifold.h

@@ -187,8 +187,8 @@ public:
   typedef Eigen::Matrix<double, dimension, 1> TangentVector;
   typedef OptionalJacobian<dimension, dimension> ChartJacobian;
 
-  /// Default constructor yields identity
+  /// Default constructor needs default constructors to be defined
-  ProductManifold():std::pair<M1,M2>(traits<M1>::Identity(),traits<M2>::Identity()) {}
+  ProductManifold():std::pair<M1,M2>(M1(),M2()) {}
 
   // Construct from two original manifold values
   ProductManifold(const M1& m1, const M2& m2):std::pair<M1,M2>(m1,m2) {}

gtsam/base/OptionalJacobian.h

@@ -171,6 +171,16 @@ public:
 // forward declare
 template <typename T> struct traits;
 
+/**
+ * @brief: meta-function to generate Jacobian
+ * @param T return type
+ * @param A argument type
+ */
+template <class T, class A>
+struct MakeJacobian {
+  typedef Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension> type;
+};
+
 /**
  * @brief: meta-function to generate JacobianTA optional reference
  * Used mainly by Expressions

gtsam/base/deprecated/LieMatrix.h

@@ -60,8 +60,9 @@ struct LieMatrix : public Matrix {
   /// @{
 
   /** print @param s optional string naming the object */
-  GTSAM_EXPORT void print(const std::string& name="") const;
+  GTSAM_EXPORT void print(const std::string& name = "") const {
-
+    gtsam::print(matrix(), name);
+  }
   /** equality up to tolerance */
   inline bool equals(const LieMatrix& expected, double tol=1e-5) const {
     return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol);

gtsam/base/deprecated/LieScalar.h

@@ -48,8 +48,10 @@ namespace gtsam {
 
     /// @name Testable
     /// @{
-    void print(const std::string& name="") const;
+    void print(const std::string& name = "") const {
-    bool equals(const LieScalar& expected, double tol=1e-5) const {
+      std::cout << name << ": " << d_ << std::endl;
+    }
+    bool equals(const LieScalar& expected, double tol = 1e-5) const {
       return fabs(expected.d_ - d_) <= tol;
     }
     /// @}

gtsam/base/deprecated/LieVector.h

@@ -18,6 +18,7 @@
 #pragma once
 
 #include <gtsam/base/VectorSpace.h>
+#include <cstdarg>
 
 namespace gtsam {
 
@@ -50,11 +51,15 @@ struct LieVector : public Vector {
   LieVector(double d) : Vector((Vector(1) << d).finished()) {}
 
   /** constructor with size and initial data, row order ! */
-  GTSAM_EXPORT LieVector(size_t m, const double* const data);
+  GTSAM_EXPORT LieVector(size_t m, const double* const data) : Vector(m) {
+    for (size_t i = 0; i < m; i++) (*this)(i) = data[i];
+  }
 
   /// @name Testable
   /// @{
-  GTSAM_EXPORT void print(const std::string& name="") const;
+  GTSAM_EXPORT void print(const std::string& name="") const {
+    gtsam::print(vector(), name);
+  }
   bool equals(const LieVector& expected, double tol=1e-5) const {
     return gtsam::equal(vector(), expected.vector(), tol);
   }

gtsam/base/serializationTestHelpers.h

@@ -49,15 +49,15 @@ bool equality(const T& input = T()) {
   return input==output;
 }
 
-// This version requires equals
+// This version requires Testable
 template<class T>
 bool equalsObj(const T& input = T()) {
   T output;
   roundtrip<T>(input,output);
-  return input.equals(output);
+  return assert_equal(input, output);
 }
 
-// De-referenced version for pointers
+// De-referenced version for pointers, requires equals method
 template<class T>
 bool equalsDereferenced(const T& input) {
   T output;
@@ -84,15 +84,15 @@ bool equalityXML(const T& input = T()) {
   return input==output;
 }
 
-// This version requires equals
+// This version requires Testable
 template<class T>
 bool equalsXML(const T& input = T()) {
   T output;
   roundtripXML<T>(input,output);
-  return input.equals(output);
+  return assert_equal(input, output);
 }
 
-// This version is for pointers
+// This version is for pointers, requires equals method
 template<class T>
 bool equalsDereferencedXML(const T& input = T()) {
   T output;
@@ -119,15 +119,15 @@ bool equalityBinary(const T& input = T()) {
   return input==output;
 }
 
-// This version requires equals
+// This version requires Testable
 template<class T>
 bool equalsBinary(const T& input = T()) {
   T output;
   roundtripBinary<T>(input,output);
-  return input.equals(output);
+  return assert_equal(input, output);
 }
 
-// This version is for pointers
+// This version is for pointers, requires equals method
 template<class T>
 bool equalsDereferencedBinary(const T& input = T()) {
   T output;

gtsam/base/tests/testLieMatrix.cpp

@@ -15,8 +15,7 @@
  */
 
 #include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/LieMatrix_Deprecated.h>
+#include <gtsam/base/deprecated/LieMatrix.h>
-
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Manifold.h>
 

gtsam/base/tests/testLieScalar.cpp

@@ -15,8 +15,7 @@
  */
 
 #include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/LieScalar_Deprecated.h>
+#include <gtsam/base/deprecated/LieScalar.h>
-
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Manifold.h>
 

gtsam/base/tests/testLieVector.cpp

@@ -15,8 +15,7 @@
  */
 
 #include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/LieVector_Deprecated.h>
+#include <gtsam/base/deprecated/LieVector.h>
-
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Manifold.h>
 

gtsam/base/tests/testTestableAssertions.cpp

@@ -15,8 +15,7 @@
  */
 
 #include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/LieScalar_Deprecated.h>
+#include <gtsam/base/deprecated/LieScalar.h>
-
 #include <gtsam/base/TestableAssertions.h>
 
 using namespace gtsam;

gtsam/geometry/BearingRange.h

@@ -0,0 +1,133 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 BearingRange.h
+ * @date July, 2015
+ * @author Frank Dellaert
+ * @brief Bearing-Range product
+ */
+
+#pragma once
+
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/OptionalJacobian.h>
+#include <boost/concept/assert.hpp>
+
+namespace gtsam {
+
+// Forward declaration of Bearing functor which should be of A1*A2 -> return_type
+// For example Bearing<Pose3,Point3>(pose,point), defined in Pose3.h will return Unit3
+// At time of writing only Pose2 and Pose3 specialize this functor.
+template <typename A1, typename A2>
+struct Bearing;
+
+// Forward declaration of Range functor which should be of A1*A2 -> return_type
+// For example Range<Pose2,Pose2>(T1,T2), defined in Pose2.h  will return double
+// At time of writing Pose2, Pose3, and several Camera variants specialize this for several types
+template <typename A1, typename A2>
+struct Range;
+
+/**
+ * Bearing-Range product for a particular A1,A2 combination will use the functors above to create
+ * a similar functor of type A1*A2 -> pair<Bearing::return_type,Range::return_type>
+ * For example BearingRange<Pose2,Point2>(pose,point) will return pair<Rot2,double>
+ * and BearingRange<Pose3,Point3>(pose,point) will return pair<Unit3,double>
+ */
+template <typename A1, typename A2>
+struct BearingRange
+    : public ProductManifold<typename Bearing<A1, A2>::result_type,
+                             typename Range<A1, A2>::result_type> {
+  typedef typename Bearing<A1, A2>::result_type B;
+  typedef typename Range<A1, A2>::result_type R;
+  typedef ProductManifold<B, R> Base;
+
+  BearingRange() {}
+  BearingRange(const ProductManifold<B, R>& br) : Base(br) {}
+  BearingRange(const B& b, const R& r) : Base(b, r) {}
+
+  /// Prediction function that stacks measurements
+  static BearingRange Measure(
+      const A1& a1, const A2& a2,
+      OptionalJacobian<Base::dimension, traits<A1>::dimension> H1 = boost::none,
+      OptionalJacobian<Base::dimension, traits<A2>::dimension> H2 =
+          boost::none) {
+    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>()(a1, a2, H1 ? &HB1 : 0, H2 ? &HB2 : 0);
+    R r = Range<A1, A2>()(a1, a2, H1 ? &HR1 : 0, H2 ? &HR2 : 0);
+
+    if (H1) *H1 << HB1, HR1;
+    if (H2) *H2 << HB2, HR2;
+    return BearingRange(b, r);
+  }
+
+  void print(const std::string& str = "") const {
+    std::cout << str;
+    traits<B>::Print(this->first, "bearing ");
+    traits<R>::Print(this->second, "range ");
+  }
+  bool equals(const BearingRange<A1, A2>& m2, double tol = 1e-8) const {
+    return traits<B>::Equals(this->first, m2.first, tol) &&
+           traits<R>::Equals(this->second, m2.second, tol);
+  }
+
+ 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;
+};
+
+// Declare this to be both Testable and a Manifold
+template <typename A1, typename A2>
+struct traits<BearingRange<A1, A2> >
+    : Testable<BearingRange<A1, A2> >,
+      internal::ManifoldTraits<BearingRange<A1, A2> > {};
+
+// Helper class for to implement Range traits for classes with a bearing method
+// For example, to specialize Bearing to Pose3 and Point3, using Pose3::bearing, it suffices to say
+//   template <> struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
+// where the third argument is used to indicate the return type
+template <class A1, typename A2, class RT>
+struct HasBearing {
+  typedef RT result_type;
+  RT operator()(
+      const A1& a1, const A2& a2,
+      OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1,
+      OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2) {
+    return a1.bearing(a2, H1, H2);
+  }
+};
+
+// Similar helper class for to implement Range traits for classes with a range method
+// For classes with overloaded range methods, such as SimpleCamera, this can even be templated:
+//   template <typename T> struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};
+template <class A1, typename A2, class RT>
+struct HasRange {
+  typedef RT result_type;
+  RT operator()(
+      const A1& a1, const A2& a2,
+      OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1,
+      OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2) {
+    return a1.range(a2, H1, H2);
+  }
+};
+
+}  // namespace gtsam

gtsam/geometry/CalibratedCamera.h

@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/geometry/BearingRange.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/concepts.h>
 #include <gtsam/base/Manifold.h>
@@ -318,7 +319,7 @@ public:
   }
 
   /// @}
-  /// @name Transformations and mesaurement functions
+  /// @name Transformations and measurement functions
   /// @{
 
   /**
@@ -384,14 +385,16 @@ private:
   /// @}
 };
 
-template<>
+// manifold traits
-struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {
+template <>
-};
+struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
 
-template<>
+template <>
-struct traits<const CalibratedCamera> : public internal::Manifold<
+struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
-    CalibratedCamera> {
-};
 
-}
+// range traits, used in RangeFactor
+template <typename T>
+struct Range<CalibratedCamera, T> : HasRange<CalibratedCamera, T, double> {};
+
+}  // namespace gtsam
 

gtsam/geometry/PinholeCamera.h

@@ -19,6 +19,7 @@
 #pragma once
 
 #include <gtsam/geometry/PinholePose.h>
+#include <gtsam/geometry/BearingRange.h>
 
 namespace gtsam {
 
@@ -263,24 +264,22 @@ public:
   template<class CalibrationB>
   double range(const PinholeCamera<CalibrationB>& camera,
       OptionalJacobian<1, dimension> Dcamera = boost::none,
-      boost::optional<Matrix&> Dother = boost::none) const {
+      OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = boost::none) const {
     Matrix16 Dcamera_, Dother_;
     double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0,
         Dother ? &Dother_ : 0);
     if (Dcamera) {
-      Dcamera->resize(1, 6 + DimK);
       *Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
     }
     if (Dother) {
-      Dother->resize(1, 6 + CalibrationB::dimension);
       Dother->setZero();
-      Dother->block<1, 6>(0, 0) = Dother_;
+      Dother->template block<1, 6>(0, 0) = Dother_;
     }
     return result;
   }
 
   /**
-   * Calculate range to another camera
+   * Calculate range to a calibrated camera
    * @param camera Other camera
    * @return range (double)
    */
@@ -304,14 +303,18 @@ private:
 
 };
 
-template<typename Calibration>
+// manifold traits
-struct traits<PinholeCamera<Calibration> > : public internal::Manifold<
-    PinholeCamera<Calibration> > {
-};
 
-template<typename Calibration>
+template <typename Calibration>
-struct traits<const PinholeCamera<Calibration> > : public internal::Manifold<
+struct traits<PinholeCamera<Calibration> >
-    PinholeCamera<Calibration> > {
+    : public internal::Manifold<PinholeCamera<Calibration> > {};
-};
 
-} // \ gtsam
+template <typename Calibration>
+struct traits<const PinholeCamera<Calibration> >
+    : public internal::Manifold<PinholeCamera<Calibration> > {};
+
+// range traits, used in RangeFactor
+template <typename Calibration, typename T>
+struct Range<PinholeCamera<Calibration>, T> : HasRange<PinholeCamera<Calibration>, T, double> {};
+
+}  // \ gtsam

gtsam/geometry/Point3.h

@@ -22,6 +22,7 @@
 #pragma once
 
 #include <gtsam/base/VectorSpace.h>
+#include <gtsam/dllexport.h>
 #include <boost/serialization/nvp.hpp>
 #include <cmath>
 
@@ -199,4 +200,19 @@ struct traits<Point3> : public internal::VectorSpace<Point3> {};
 
 template<>
 struct traits<const Point3> : public internal::VectorSpace<Point3> {};
-}
+
+template <typename A1, typename A2>
+struct Range;
+
+template <>
+struct Range<Point3, Point3> {
+  typedef double result_type;
+  double operator()(const Point3& p, const Point3& q,
+                    OptionalJacobian<1, 3> H1 = boost::none,
+                    OptionalJacobian<1, 3> H2 = boost::none) {
+    return p.distance(q, H1, H2);
+  }
+};
+
+}  // namespace gtsam
+

gtsam/geometry/Pose2.h

@@ -20,9 +20,11 @@
 
 #pragma once
 
+#include <gtsam/geometry/BearingRange.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/base/Lie.h>
+#include <gtsam/dllexport.h>
 
 namespace gtsam {
 
@@ -289,11 +291,18 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
 typedef std::pair<Point2,Point2> Point2Pair;
 GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
 
-template<>
+template <>
 struct traits<Pose2> : public internal::LieGroup<Pose2> {};
 
-template<>
+template <>
 struct traits<const Pose2> : public internal::LieGroup<Pose2> {};
 
+// bearing and range traits, used in RangeFactor
+template <typename T>
+struct Bearing<Pose2, T> : HasBearing<Pose2, T, Rot2> {};
+
+template <typename T>
+struct Range<Pose2, T> : HasRange<Pose2, T, double> {};
+
 } // namespace gtsam
 

gtsam/geometry/Pose3.cpp

@@ -314,35 +314,47 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
 
 /* ************************************************************************* */
 double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
-    OptionalJacobian<1, 3> H2) const {
+                    OptionalJacobian<1, 3> H2) const {
+  Matrix36 D_local_pose;
+  Matrix3 D_local_point;
+  Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0);
   if (!H1 && !H2) {
-    return transform_to(point).norm();
+    return local.norm();
   } else {
-    Matrix36 D1;
+    Matrix13 D_r_local;
-    Matrix3 D2;
+    const double r = local.norm(D_r_local);
-    Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
+    if (H1) *H1 = D_r_local * D_local_pose;
-    const double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z,
+    if (H2) *H2 = D_r_local * D_local_point;
-        n = sqrt(d2);
+    return r;
-    Matrix13 D_result_d;
-    D_result_d << x / n, y / n, z / n;
-    if (H1) *H1 = D_result_d * D1;
-    if (H2) *H2 = D_result_d * D2;
-    return n;
   }
 }
 
 /* ************************************************************************* */
-double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1,
+double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> H1,
-    OptionalJacobian<1,6> H2) const {
+                    OptionalJacobian<1, 6> H2) const {
-  Matrix13 D2;
+  Matrix13 D_local_point;
-  double r = range(pose.translation(), H1, H2? &D2 : 0);
+  double r = range(pose.translation(), H1, H2 ? &D_local_point : 0);
-  if (H2) {
+  if (H2) *H2 << Matrix13::Zero(), D_local_point * pose.rotation().matrix();
-    Matrix13 H2_ = D2 * pose.rotation().matrix();
-    *H2 << Matrix13::Zero(), H2_;
-  }
   return r;
 }
 
+/* ************************************************************************* */
+Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1,
+                     OptionalJacobian<2, 3> H2) const {
+  Matrix36 D_local_pose;
+  Matrix3 D_local_point;
+  Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0);
+  if (!H1 && !H2) {
+    return Unit3(local);
+  } else {
+    Matrix23 D_b_local;
+    Unit3 b = Unit3::FromPoint3(local, D_b_local);
+    if (H1) *H1 = D_b_local * D_local_pose;
+    if (H2) *H2 = D_b_local * D_local_point;
+    return b;
+  }
+}
+
 /* ************************************************************************* */
 boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
   const size_t n = pairs.size();

gtsam/geometry/Pose3.h

@@ -19,6 +19,7 @@
 
 #include <gtsam/config.h>
 
+#include <gtsam/geometry/BearingRange.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/base/Lie.h>
@@ -262,6 +263,14 @@ public:
   double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
       OptionalJacobian<1, 6> H2 = boost::none) const;
 
+  /**
+   * Calculate bearing to a landmark
+   * @param point 3D location of landmark
+   * @return bearing (Unit3)
+   */
+  Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> H1 = boost::none,
+      OptionalJacobian<2, 3> H2 = boost::none) const;
+
   /// @}
   /// @name Advanced Interface
   /// @{
@@ -323,11 +332,17 @@ GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
 // For MATLAB wrapper
 typedef std::vector<Pose3> Pose3Vector;
 
-template<>
+template <>
 struct traits<Pose3> : public internal::LieGroup<Pose3> {};
 
-template<>
+template <>
 struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
 
+// bearing and range traits, used in RangeFactor
+template <>
+struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
 
-} // namespace gtsam
+template <typename T>
+struct Range<Pose3, T> : HasRange<Pose3, T, double> {};
+
+}  // namespace gtsam

gtsam/geometry/SimpleCamera.h

@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/geometry/BearingRange.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Cal3_S2.h>
 
@@ -125,14 +126,18 @@ public:
 
 };
 
-template<>
+/// Recover camera from 3*4 camera matrix
-struct traits<SimpleCamera> : public internal::Manifold<SimpleCamera> {
+GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);
-};
 
-template<>
+// manifold traits
-struct traits<const SimpleCamera> : public internal::Manifold<SimpleCamera> {
+template <>
-};
+struct traits<SimpleCamera> : public internal::Manifold<SimpleCamera> {};
 
-  /// Recover camera from 3*4 camera matrix
+template <>
-  GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);
+struct traits<const SimpleCamera> : public internal::Manifold<SimpleCamera> {};
-}
+
+// range traits, used in RangeFactor
+template <typename T>
+struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};
+
+}  // namespace gtsam

gtsam/geometry/concepts.h

@@ -59,24 +59,6 @@ private:
   }
 };
 
-/**
- * Range measurement concept
- * Given a pair of Lie variables, there must exist a function to calculate
- * range with derivatives.
- */
-template<class V1, class V2>
-class RangeMeasurementConcept {
-private:
-  static double checkRangeMeasurement(const V1& x, const V2& p) {
-    return x.range(p);
-  }
-
-  static double checkRangeMeasurementDerivatives(const V1& x, const V2& p) {
-    boost::optional<Matrix&> H1, H2;
-    return x.range(p, H1, H2);
-  }
-};
-
 } // \namespace gtsam
 
 /**
@@ -92,7 +74,3 @@ private:
 #define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept<T>;
 #define GTSAM_CONCEPT_POSE_TYPE(T) typedef gtsam::PoseConcept<T> _gtsam_PoseConcept##T;
 
-/** Range Measurement macros */
-#define GTSAM_CONCEPT_RANGE_MEASUREMENT_INST(V1,V2) template class gtsam::RangeMeasurementConcept<V1,V2>;
-#define GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(V1,V2) typedef gtsam::RangeMeasurementConcept<V1,V2> _gtsam_RangeMeasurementConcept##V1##V2;
-

gtsam/geometry/tests/testBearingRange.cpp

@@ -0,0 +1,79 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  testBearingRange.cpp
+ *  @brief Unit tests for BearingRange Class
+ *  @author Frank Dellaert
+ *  @date July 2015
+ */
+
+#include <gtsam/geometry/BearingRange.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/base/serializationTestHelpers.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using namespace serializationTestHelpers;
+
+typedef BearingRange<Pose2, Point2> BearingRange2D;
+BearingRange2D br2D(1, 2);
+
+typedef BearingRange<Pose3, Point3> BearingRange3D;
+BearingRange3D br3D(Pose3().bearing(Point3(1, 0, 0)), 1);
+
+//******************************************************************************
+TEST(BearingRange2D, Concept) {
+  BOOST_CONCEPT_ASSERT((IsManifold<BearingRange2D>));
+}
+
+/* ************************************************************************* */
+TEST(BearingRange, 2D) {
+  BearingRange2D expected(0, 1);
+  BearingRange2D actual = BearingRange2D::Measure(Pose2(), Point2(1, 0));
+  EXPECT(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST(BearingRange, Serialization2D) {
+  EXPECT(equalsObj<BearingRange2D>(br2D));
+  EXPECT(equalsXML<BearingRange2D>(br2D));
+  EXPECT(equalsBinary<BearingRange2D>(br2D));
+}
+
+//******************************************************************************
+TEST(BearingRange3D, Concept) {
+  BOOST_CONCEPT_ASSERT((IsManifold<BearingRange3D>));
+}
+
+/* ************************************************************************* */
+TEST(BearingRange, 3D) {
+  BearingRange3D expected(Unit3(), 1);
+  BearingRange3D actual = BearingRange3D::Measure(Pose3(), Point3(1, 0, 0));
+  EXPECT(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST(BearingRange, Serialization3D) {
+  EXPECT(equalsObj<BearingRange3D>(br3D));
+  EXPECT(equalsXML<BearingRange3D>(br3D));
+  EXPECT(equalsBinary<BearingRange3D>(br3D));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/geometry/tests/testPose3.cpp

@@ -633,6 +633,22 @@ TEST( Pose3, range_pose )
   EXPECT(assert_equal(expectedH2,actualH2));
 }
 
+/* ************************************************************************* */
+Unit3 bearing_proxy(const Pose3& pose, const Point3& point) {
+  return pose.bearing(point);
+}
+TEST( Pose3, bearing )
+{
+  Matrix expectedH1, actualH1, expectedH2, actualH2;
+  EXPECT(assert_equal(Unit3(1,0,0),x1.bearing(l1, actualH1, actualH2),1e-9));
+
+  // Check numerical derivatives
+  expectedH1 = numericalDerivative21(bearing_proxy, x1, l1);
+  expectedH2 = numericalDerivative22(bearing_proxy, x1, l1);
+  EXPECT(assert_equal(expectedH1,actualH1));
+  EXPECT(assert_equal(expectedH2,actualH2));
+}
+
 /* ************************************************************************* */
 TEST( Pose3, unicycle )
 {

gtsam/linear/tests/testSerializationLinear.cpp

@@ -38,6 +38,7 @@ using namespace gtsam::serializationTestHelpers;
 
 /* ************************************************************************* */
 // Export Noisemodels
+// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");

gtsam/nonlinear/Expression.h

@@ -172,6 +172,9 @@ public:
 
 private:
 
+  /// Default constructor, for serialization
+  Expression() {}
+
   /// Keys and dimensions in same order
   typedef std::pair<KeyVector, FastVector<int> > KeysAndDims;
   KeysAndDims keysAndDims() const;

gtsam/nonlinear/ExpressionFactor.h

@@ -23,43 +23,54 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/base/Testable.h>
 #include <numeric>
-
 namespace gtsam {
 
 /**
+
  * Factor that supports arbitrary expressions via AD
  */
-template<class T>
+template<typename T>
 class ExpressionFactor: public NoiseModelFactor {
+  BOOST_CONCEPT_ASSERT((IsTestable<T>));
 
 protected:
 
   typedef ExpressionFactor<T> This;
-
-  T measurement_; ///< the measurement to be compared with the expression
-  Expression<T> expression_; ///< the expression that is AD enabled
-  FastVector<int> dims_; ///< dimensions of the Jacobian matrices
-
   static const int Dim = traits<T>::dimension;
 
+  T measured_;  ///< the measurement to be compared with the expression
+  Expression<T> expression_;  ///< the expression that is AD enabled
+  FastVector<int> dims_;      ///< dimensions of the Jacobian matrices
+
 public:
 
   typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
 
   /// Constructor
-  ExpressionFactor(const SharedNoiseModel& noiseModel, //
+  ExpressionFactor(const SharedNoiseModel& noiseModel,  //
-      const T& measurement, const Expression<T>& expression) :
+                   const T& measurement, const Expression<T>& expression)
-      measurement_(measurement), expression_(expression) {
+      : NoiseModelFactor(noiseModel), measured_(measurement) {
-    if (!noiseModel)
+    initialize(expression);
-      throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
+  }
-    if (noiseModel->dim() != Dim)
-      throw std::invalid_argument(
-          "ExpressionFactor was created with a NoiseModel of incorrect dimension.");
-    noiseModel_ = noiseModel;
 
-    // Get keys and dimensions for Jacobian matrices
+  /// Destructor
-    // An Expression is assumed unmutable, so we do this now
+  virtual ~ExpressionFactor() {}
-    boost::tie(keys_, dims_) = expression_.keysAndDims();
+
+  /** return the measurement */
+  const T& measured() const { return measured_; }
+
+  /// print relies on Testable traits being defined for T
+  void print(const std::string& s, const KeyFormatter& keyFormatter) const {
+    NoiseModelFactor::print(s, keyFormatter);
+    traits<T>::Print(measured_, s + ".measured_");
+  }
+
+  /// equals relies on Testable traits being defined for T
+  bool equals(const NonlinearFactor& f, double tol) const {
+    const ExpressionFactor* p = dynamic_cast<const ExpressionFactor*>(&f);
+    return p && NoiseModelFactor::equals(f, tol) &&
+           traits<T>::Equals(measured_, p->measured_, tol) &&
+           dims_ == p->dims_;
   }
 
   /**
@@ -71,10 +82,10 @@ public:
       boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if (H) {
       const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
-      return traits<T>::Local(measurement_, value);
+      return traits<T>::Local(measured_, value);
     } else {
       const T value = expression_.value(x);
-      return traits<T>::Local(measurement_, value);
+      return traits<T>::Local(measured_, value);
     }
   }
 
@@ -105,7 +116,7 @@ public:
     T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here !
 
     // Evaluate error and set RHS vector b
-    Ab(size()).col(0) = -traits<T>::Local(measurement_, value);
+    Ab(size()).col(0) = -traits<T>::Local(measured_, value);
 
     // Whiten the corresponding system, Ab already contains RHS
     Vector b = Ab(size()).col(0); // need b to be valid for Robust noise models
@@ -119,8 +130,116 @@ public:
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
+
+protected:
+ ExpressionFactor() {}
+ /// Default constructor, for serialization
+
+ /// Constructor for serializable derived classes
+ ExpressionFactor(const SharedNoiseModel& noiseModel, const T& measurement)
+     : NoiseModelFactor(noiseModel), measured_(measurement) {
+   // Not properly initialized yet, need to call initialize
+ }
+
+ /// Initialize with constructor arguments
+ void initialize(const Expression<T>& expression) {
+   if (!noiseModel_)
+     throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
+   if (noiseModel_->dim() != Dim)
+     throw std::invalid_argument(
+         "ExpressionFactor was created with a NoiseModel of incorrect dimension.");
+   expression_ = expression;
+
+   // Get keys and dimensions for Jacobian matrices
+   // An Expression is assumed unmutable, so we do this now
+   boost::tie(keys_, dims_) = expression_.keysAndDims();
+ }
+
+ /// Recreate expression from keys_ and measured_, used in load below.
+ /// Needed to deserialize a derived factor
+ virtual Expression<T> expression() const {
+   throw std::runtime_error("ExpressionFactor::expression not provided: cannot deserialize.");
+ }
+
+private:
+ /// Save to an archive: just saves the base class
+ template <class Archive>
+ void save(Archive& ar, const unsigned int /*version*/) const {
+   ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor);
+   ar << boost::serialization::make_nvp("measured_", this->measured_);
+ }
+
+ /// Load from an archive, creating a valid expression using the overloaded
+ /// [expression] method
+ template <class Archive>
+ void load(Archive& ar, const unsigned int /*version*/) {
+   ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor);
+   ar >> boost::serialization::make_nvp("measured_", this->measured_);
+   this->initialize(expression());
+ }
+
+ // Indicate that we implement save/load separately, and be friendly to boost
+ BOOST_SERIALIZATION_SPLIT_MEMBER()
+
+ friend class boost::serialization::access;
 };
 // ExpressionFactor
 
+/// traits
+template <typename T>
+struct traits<ExpressionFactor<T> > : public Testable<ExpressionFactor<T> > {};
+
+/**
+ * Binary specialization of ExpressionFactor meant as a base class for binary factors
+ * Enforces expression method with two keys, and provides evaluateError
+ * Derived needs to call initialize.
+ */
+template <typename T, typename A1, typename A2>
+class ExpressionFactor2 : public ExpressionFactor<T> {
+ public:
+  /// Destructor
+  virtual ~ExpressionFactor2() {}
+
+  /// Backwards compatible evaluateError, to make existing tests compile
+  Vector evaluateError(const A1& a1, const A2& a2,
+                       boost::optional<Matrix&> H1 = boost::none,
+                       boost::optional<Matrix&> H2 = boost::none) const {
+    Values values;
+    values.insert(this->keys_[0], a1);
+    values.insert(this->keys_[1], a2);
+    std::vector<Matrix> H(2);
+    Vector error = this->unwhitenedError(values, H);
+    if (H1) (*H1) = H[0];
+    if (H2) (*H2) = H[1];
+    return error;
+  }
+
+  /// Recreate expression from given keys_ and measured_, used in load
+  /// Needed to deserialize a derived factor
+  virtual Expression<T> expression(Key key1, Key key2) const {
+    throw std::runtime_error("ExpressionFactor2::expression not provided: cannot deserialize.");
+  }
+
+ protected:
+  /// Default constructor, for serialization
+  ExpressionFactor2() {}
+
+  /// Constructor takes care of keys, but still need to call initialize
+  ExpressionFactor2(Key key1, Key key2,
+                                const SharedNoiseModel& noiseModel,
+                                const T& measurement)
+      : ExpressionFactor<T>(noiseModel, measurement) {
+    this->keys_.push_back(key1);
+    this->keys_.push_back(key2);
+  }
+
+ private:
+  /// Return an expression that predicts the measurement given Values
+  virtual Expression<T> expression() const {
+    return expression(this->keys_[0], this->keys_[1]);
+  }
+};
+// ExpressionFactor2
+
 }// \ namespace gtsam
 

gtsam/nonlinear/expressionTesting.h

@@ -20,44 +20,11 @@
 #pragma once
 
 #include <gtsam/nonlinear/ExpressionFactor.h>
-#include <gtsam/nonlinear/Expression.h>
 #include <gtsam/nonlinear/factorTesting.h>
-#include <gtsam/base/Matrix.h>
 #include <gtsam/base/Testable.h>
-#include <gtsam/linear/VectorValues.h>
-
-#include <CppUnitLite/TestResult.h>
-#include <CppUnitLite/Test.h>
-#include <CppUnitLite/Failure.h>
 
 namespace gtsam {
 
-namespace internal {
-// CPPUnitLite-style test for linearization of a factor
-bool testFactorJacobians(TestResult& result_, const std::string& name_,
-    const NoiseModelFactor& factor, const gtsam::Values& values, double delta,
-    double tolerance) {
-
-  // Create expected value by numerical differentiation
-  JacobianFactor expected = linearizeNumerically(factor, values, delta);
-
-  // Create actual value by linearize
-  boost::shared_ptr<JacobianFactor> actual = //
-      boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
-
-  // Check cast result and then equality
-  return actual && assert_equal(expected, *actual, tolerance);
-}
-}
-
-/// \brief Check the Jacobians produced by a factor against finite differences.
-/// \param factor The factor to test.
-/// \param values Values filled in for testing the Jacobians.
-/// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians
-/// \param tolerance The numerical tolerance to use when comparing Jacobians.
-#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \
-    { EXPECT(gtsam::internal::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance)); }
-
 namespace internal {
 // CPPUnitLite-style test for linearization of an ExpressionFactor
 template<typename T>

gtsam/nonlinear/factorTesting.h

@@ -22,6 +22,10 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/base/numericalDerivative.h>
 
+#include <CppUnitLite/TestResult.h>
+#include <CppUnitLite/Test.h>
+#include <CppUnitLite/Failure.h>
+
 namespace gtsam {
 
 /**
@@ -29,6 +33,10 @@ namespace gtsam {
  * The benefit of this method is that it does not need to know what types are
  * involved to evaluate the factor. If all the machinery of gtsam is working
  * correctly, we should get the correct numerical derivatives out the other side.
+ * NOTE(frank): factor that have non vector-space measurements use between or LocalCoordinates
+ * to evaluate the error, and their derivatives will only be correct for near-zero errors.
+ * This is fixable but expensive, and does not matter in practice as most factors will sit near
+ * zero errors anyway. However, it means that below will only be exact for the correct measurement.
  */
 JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
     const Values& values, double delta = 1e-5) {
@@ -65,4 +73,30 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
   return JacobianFactor(jacobians, -e);
 }
 
+namespace internal {
+// CPPUnitLite-style test for linearization of a factor
+bool testFactorJacobians(TestResult& result_, const std::string& name_,
+    const NoiseModelFactor& factor, const gtsam::Values& values, double delta,
+    double tolerance) {
+
+  // Create expected value by numerical differentiation
+  JacobianFactor expected = linearizeNumerically(factor, values, delta);
+
+  // Create actual value by linearize
+  boost::shared_ptr<JacobianFactor> actual = //
+      boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
+
+  // Check cast result and then equality
+  return actual && assert_equal(expected, *actual, tolerance);
+}
+}
+
+/// \brief Check the Jacobians produced by a factor against finite differences.
+/// \param factor The factor to test.
+/// \param values Values filled in for testing the Jacobians.
+/// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians
+/// \param tolerance The numerical tolerance to use when comparing Jacobians.
+#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \
+    { EXPECT(gtsam::internal::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance)); }
+
 } // namespace gtsam

gtsam/sam/BearingFactor.h

@@ -0,0 +1,70 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  BearingFactor.h
+ *  @brief Serializable factor induced by a bearing measurement
+ *  @date July 2015
+ *  @author Frank Dellaert
+ **/
+
+#pragma once
+
+#include <gtsam/nonlinear/ExpressionFactor.h>
+
+namespace gtsam {
+
+// forward declaration of Bearing functor, assumed partially specified
+template <typename A1, typename A2>
+struct Bearing;
+
+/**
+ * Binary factor for a bearing measurement
+ * Works for any two types A1,A2 for which the functor Bearing<A1,A2>() is
+ * defined
+ * @addtogroup SAM
+ */
+template <typename A1, typename A2,
+          typename T = typename Bearing<A1, A2>::result_type>
+struct BearingFactor : public ExpressionFactor2<T, A1, A2> {
+  typedef ExpressionFactor2<T, A1, A2> Base;
+
+  /// default constructor
+  BearingFactor() {}
+
+  /// primary constructor
+  BearingFactor(Key key1, Key key2, const T& measured,
+                const SharedNoiseModel& model)
+      : Base(key1, key2, model, measured) {
+    this->initialize(expression(key1, key2));
+  }
+
+  // Return measurement expression
+  virtual Expression<T> expression(Key key1, Key key2) const {
+    Expression<A1> a1_(key1);
+    Expression<A2> a2_(key2);
+    return Expression<T>(Bearing<A1, A2>(), a1_, a2_);
+  }
+
+  /// print
+  void print(const std::string& s = "",
+             const KeyFormatter& kf = DefaultKeyFormatter) const {
+    std::cout << s << "BearingFactor" << std::endl;
+    Base::print(s, kf);
+  }
+};  // BearingFactor
+
+/// traits
+template <typename A1, typename A2, typename T>
+struct traits<BearingFactor<A1, A2, T> >
+    : public Testable<BearingFactor<A1, A2, T> > {};
+
+}  // namespace gtsam

gtsam/sam/BearingRangeFactor.h

@@ -0,0 +1,83 @@
+/* ----------------------------------------------------------------------------
+
+ * 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/ExpressionFactor.h>
+#include <gtsam/geometry/BearingRange.h>
+#include <boost/concept/assert.hpp>
+
+namespace gtsam {
+
+/**
+ * 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 ExpressionFactor2<BearingRange<A1, A2>, A1, A2> {
+ private:
+  typedef BearingRange<A1, A2> T;
+  typedef ExpressionFactor2<T, A1, A2> Base;
+  typedef BearingRangeFactor<A1, A2> This;
+
+ 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)
+      : 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 {
+    return Expression<T>(T::Measure, Expression<A1>(key1),
+                         Expression<A2>(key2));
+  }
+
+  /// print
+  virtual void print(const std::string& s = "",
+                     const KeyFormatter& kf = DefaultKeyFormatter) const {
+    std::cout << s << "BearingRangeFactor" << std::endl;
+    Base::print(s, kf);
+  }
+
+};  // 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/CMakeLists.txt

@@ -0,0 +1,6 @@
+# Install headers
+file(GLOB sam_headers "*.h")
+install(FILES ${sam_headers} DESTINATION include/gtsam/sam)
+
+# Build tests
+add_subdirectory(tests)

gtsam/sam/RangeFactor.h

@@ -0,0 +1,142 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+ *  @date July 2015
+ *  @author Frank Dellaert
+ **/
+
+#pragma once
+
+#include <gtsam/nonlinear/ExpressionFactor.h>
+
+namespace gtsam {
+
+// forward declaration of Range functor, assumed partially specified
+template <typename A1, typename A2>
+struct Range;
+
+/**
+ * Binary factor for a range measurement
+ * Works for any two types A1,A2 for which the functor Range<A1,A2>() is defined
+ * @addtogroup SAM
+ */
+template <typename A1, typename A2 = A1,
+          typename T = typename Range<A1, A2>::result_type>
+class RangeFactor : public ExpressionFactor2<T, A1, A2> {
+ private:
+  typedef RangeFactor<A1, A2> This;
+  typedef ExpressionFactor2<T, A1, A2> Base;
+
+ public:
+  /// default constructor
+  RangeFactor() {}
+
+  RangeFactor(Key key1, Key key2, T 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<T> expression(Key key1, Key key2) const {
+    Expression<A1> a1_(key1);
+    Expression<A2> a2_(key2);
+    return Expression<T>(Range<A1, A2>(), a1_, a2_);
+  }
+
+  /// 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 <typename A1, typename A2, typename T>
+struct traits<RangeFactor<A1, A2, T> >
+    : public Testable<RangeFactor<A1, A2, T> > {};
+
+/**
+ * Binary factor for a range measurement, with a transform applied
+ * @addtogroup SAM
+ */
+template <typename A1, typename A2 = A1,
+          typename T = typename Range<A1, A2>::result_type>
+class RangeFactorWithTransform : public ExpressionFactor2<T, A1, A2> {
+ private:
+  typedef RangeFactorWithTransform<A1, A2> This;
+  typedef ExpressionFactor2<T, A1, A2> Base;
+
+  A1 body_T_sensor_;  ///< The pose of the sensor in the body frame
+
+ public:
+  //// Default constructor
+  RangeFactorWithTransform() {}
+
+  RangeFactorWithTransform(Key key1, Key key2, T 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<T> 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> a2_(key2);
+    return Expression<T>(Range<A1, A2>(), nav_T_sensor_, a2_);
+  }
+
+  /** 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 <typename 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 <typename A1, typename A2, typename T>
+struct traits<RangeFactorWithTransform<A1, A2, T> >
+    : public Testable<RangeFactorWithTransform<A1, A2, T> > {};
+
+}  // \ namespace gtsam

gtsam/sam/tests/CMakeLists.txt

@@ -0,0 +1 @@
+gtsamAddTestsGlob(sam "test*.cpp" "" "gtsam")

gtsam/sam/tests/testBearingFactor.cpp

@@ -0,0 +1,107 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  testBearingFactor.cpp
+ *  @brief Unit tests for BearingFactor Class
+ *  @author Frank Dellaert
+ *  @date July 2015
+ */
+
+#include <gtsam/sam/BearingFactor.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;
+
+Key poseKey(1);
+Key pointKey(2);
+
+typedef BearingFactor<Pose2, Point2> BearingFactor2D;
+double measurement2D(10.0);
+static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(1, 0.5));
+BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model2D);
+
+typedef BearingFactor<Pose3, Point3> BearingFactor3D;
+Unit3 measurement3D = Pose3().bearing(Point3(1, 0, 0));  // has to match values!
+static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5));
+BearingFactor3D factor3D(poseKey, pointKey, measurement3D, 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(BearingFactor, Serialization2D) {
+  EXPECT(serializationTestHelpers::equalsObj(factor2D));
+  EXPECT(serializationTestHelpers::equalsXML(factor2D));
+  EXPECT(serializationTestHelpers::equalsBinary(factor2D));
+}
+
+/* ************************************************************************* */
+TEST(BearingFactor, 2D) {
+  // Serialize the factor
+  std::string serialized = serializeXML(factor2D);
+
+  // And de-serialize it
+  BearingFactor2D 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(BearingFactor, Serialization3D) {
+  EXPECT(serializationTestHelpers::equalsObj(factor3D));
+  EXPECT(serializationTestHelpers::equalsXML(factor3D));
+  EXPECT(serializationTestHelpers::equalsBinary(factor3D));
+}
+
+/* ************************************************************************* */
+TEST(BearingFactor, 3D) {
+  // Serialize the factor
+  std::string serialized = serializeXML(factor3D);
+
+  // And de-serialize it
+  BearingFactor3D 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/sam/tests/testBearingRangeFactor.cpp

@@ -0,0 +1,106 @@
+/* ----------------------------------------------------------------------------
+
+ * 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;
+
+Key poseKey(1);
+Key pointKey(2);
+
+typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
+static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5));
+BearingRangeFactor2D factor2D(poseKey, pointKey, 1, 2, model2D);
+
+typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
+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(serializationTestHelpers::equalsObj(factor2D));
+  EXPECT(serializationTestHelpers::equalsXML(factor2D));
+  EXPECT(serializationTestHelpers::equalsBinary(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(serializationTestHelpers::equalsObj(factor3D));
+  EXPECT(serializationTestHelpers::equalsXML(factor3D));
+  EXPECT(serializationTestHelpers::equalsBinary(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/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/geometry/SimpleCamera.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/serializationTestHelpers.h>
 #include <gtsam/base/TestableAssertions.h>
+
+#include <CppUnitLite/TestHarness.h>
 #include <boost/bind.hpp>
 
 using namespace std;
@@ -37,6 +38,10 @@ typedef RangeFactor<Pose3, Point3> RangeFactor3D;
 typedef RangeFactorWithTransform<Pose2, Point2> RangeFactorWithTransform2D;
 typedef RangeFactorWithTransform<Pose3, Point3> RangeFactorWithTransform3D;
 
+Key poseKey(1);
+Key pointKey(2);
+double measurement(10.0);
+
 /* ************************************************************************* */
 Vector factorError2D(const Pose2& pose, const Point2& point,
     const RangeFactor2D& factor) {
@@ -63,19 +68,33 @@ Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point,
 
 /* ************************************************************************* */
 TEST( RangeFactor, Constructor) {
-  Key poseKey(1);
-  Key pointKey(2);
-  double measurement(10.0);
-
   RangeFactor2D factor2D(poseKey, pointKey, measurement, model);
   RangeFactor3D factor3D(poseKey, pointKey, measurement, model);
 }
 
+/* ************************************************************************* */
+// Export Noisemodels
+// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
+BOOST_CLASS_EXPORT(gtsam::noiseModel::Unit);
+
+/* ************************************************************************* */
+TEST(RangeFactor, Serialization2D) {
+  RangeFactor2D factor2D(poseKey, pointKey, measurement, model);
+  EXPECT(serializationTestHelpers::equalsObj(factor2D));
+  EXPECT(serializationTestHelpers::equalsXML(factor2D));
+  EXPECT(serializationTestHelpers::equalsBinary(factor2D));
+}
+
+/* ************************************************************************* */
+TEST(RangeFactor, Serialization3D) {
+  RangeFactor3D factor3D(poseKey, pointKey, measurement, model);
+  EXPECT(serializationTestHelpers::equalsObj(factor3D));
+  EXPECT(serializationTestHelpers::equalsXML(factor3D));
+  EXPECT(serializationTestHelpers::equalsBinary(factor3D));
+}
+
 /* ************************************************************************* */
 TEST( RangeFactor, ConstructorWithTransform) {
-  Key poseKey(1);
-  Key pointKey(2);
-  double measurement(10.0);
   Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
   Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
       Point3(0.25, -0.10, 1.0));
@@ -89,10 +108,6 @@ TEST( RangeFactor, ConstructorWithTransform) {
 /* ************************************************************************* */
 TEST( RangeFactor, Equals ) {
   // Create two identical factors and make sure they're equal
-  Key poseKey(1);
-  Key pointKey(2);
-  double measurement(10.0);
-
   RangeFactor2D factor2D_1(poseKey, pointKey, measurement, model);
   RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model);
   CHECK(assert_equal(factor2D_1, factor2D_2));
@@ -105,9 +120,6 @@ TEST( RangeFactor, Equals ) {
 /* ************************************************************************* */
 TEST( RangeFactor, EqualsWithTransform ) {
   // Create two identical factors and make sure they're equal
-  Key poseKey(1);
-  Key pointKey(2);
-  double measurement(10.0);
   Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
   Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
       Point3(0.25, -0.10, 1.0));
@@ -128,9 +140,6 @@ TEST( RangeFactor, EqualsWithTransform ) {
 /* ************************************************************************* */
 TEST( RangeFactor, Error2D ) {
   // Create a factor
-  Key poseKey(1);
-  Key pointKey(2);
-  double measurement(10.0);
   RangeFactor2D factor(poseKey, pointKey, measurement, model);
 
   // Set the linearization point
@@ -150,9 +159,6 @@ TEST( RangeFactor, Error2D ) {
 /* ************************************************************************* */
 TEST( RangeFactor, Error2DWithTransform ) {
   // Create a factor
-  Key poseKey(1);
-  Key pointKey(2);
-  double measurement(10.0);
   Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
   RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model,
       body_P_sensor);
@@ -176,9 +182,6 @@ TEST( RangeFactor, Error2DWithTransform ) {
 /* ************************************************************************* */
 TEST( RangeFactor, Error3D ) {
   // Create a factor
-  Key poseKey(1);
-  Key pointKey(2);
-  double measurement(10.0);
   RangeFactor3D factor(poseKey, pointKey, measurement, model);
 
   // Set the linearization point
@@ -198,9 +201,6 @@ TEST( RangeFactor, Error3D ) {
 /* ************************************************************************* */
 TEST( RangeFactor, Error3DWithTransform ) {
   // Create a factor
-  Key poseKey(1);
-  Key pointKey(2);
-  double measurement(10.0);
   Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
       Point3(0.25, -0.10, 1.0));
   RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model,
@@ -225,9 +225,6 @@ TEST( RangeFactor, Error3DWithTransform ) {
 /* ************************************************************************* */
 TEST( RangeFactor, Jacobian2D ) {
   // Create a factor
-  Key poseKey(1);
-  Key pointKey(2);
-  double measurement(10.0);
   RangeFactor2D factor(poseKey, pointKey, measurement, model);
 
   // Set the linearization point
@@ -253,9 +250,6 @@ TEST( RangeFactor, Jacobian2D ) {
 /* ************************************************************************* */
 TEST( RangeFactor, Jacobian2DWithTransform ) {
   // Create a factor
-  Key poseKey(1);
-  Key pointKey(2);
-  double measurement(10.0);
   Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
   RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model,
       body_P_sensor);
@@ -285,9 +279,6 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
 /* ************************************************************************* */
 TEST( RangeFactor, Jacobian3D ) {
   // Create a factor
-  Key poseKey(1);
-  Key pointKey(2);
-  double measurement(10.0);
   RangeFactor3D factor(poseKey, pointKey, measurement, model);
 
   // Set the linearization point
@@ -313,9 +304,6 @@ TEST( RangeFactor, Jacobian3D ) {
 /* ************************************************************************* */
 TEST( RangeFactor, Jacobian3DWithTransform ) {
   // Create a factor
-  Key poseKey(1);
-  Key pointKey(2);
-  double measurement(10.0);
   Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
       Point3(0.25, -0.10, 1.0));
   RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model,
@@ -343,6 +331,64 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
   CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
 }
 
+/* ************************************************************************* */
+// Do a test with Point3
+TEST(RangeFactor, Point3) {
+  // Create a factor
+  RangeFactor<Point3> factor(poseKey, pointKey, measurement, model);
+
+  // Set the linearization point
+  Point3 pose(1.0, 2.0, 00);
+  Point3 point(-4.0, 11.0, 0);
+
+  // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
+  Vector expectedError = (Vector(1) << 0.295630141).finished();
+
+  // Verify we get the expected error
+  CHECK(assert_equal(expectedError, factor.evaluateError(pose, point), 1e-9));
+}
+
+/* ************************************************************************* */
+// Do tests with SimpleCamera
+TEST( RangeFactor, Camera) {
+  RangeFactor<SimpleCamera,Point3> factor1(poseKey, pointKey, measurement, model);
+  RangeFactor<SimpleCamera,Pose3> factor2(poseKey, pointKey, measurement, model);
+  RangeFactor<SimpleCamera,SimpleCamera> factor3(poseKey, pointKey, measurement, model);
+}
+
+/* ************************************************************************* */
+// Do a test with non GTSAM types
+
+namespace gtsam{
+template <>
+struct Range<Vector3, Vector3> {
+  typedef double result_type;
+  double operator()(const Vector3& v1, const Vector3& v2,
+                    OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) {
+    return (v2 - v1).norm();
+    // derivatives not implemented
+  }
+};
+}
+
+TEST(RangeFactor, NonGTSAM) {
+  // Create a factor
+  Key poseKey(1);
+  Key pointKey(2);
+  double measurement(10.0);
+  RangeFactor<Vector3> factor(poseKey, pointKey, measurement, model);
+
+  // Set the linearization point
+  Vector3 pose(1.0, 2.0, 00);
+  Vector3 point(-4.0, 11.0, 0);
+
+  // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
+  Vector expectedError = (Vector(1) << 0.295630141).finished();
+
+  // Verify we get the expected error
+  CHECK(assert_equal(expectedError, factor.evaluateError(pose, point), 1e-9));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/sfm/CMakeLists.txt

@@ -0,0 +1,6 @@
+# Install headers
+file(GLOB sfm_headers "*.h")
+install(FILES ${sfm_headers} DESTINATION include/gtsam/sfm)
+
+# Build tests
+add_subdirectory(tests)

gtsam/sfm/tests/CMakeLists.txt

@@ -0,0 +1 @@
+gtsamAddTestsGlob(sfm "test*.cpp" "" "gtsam")

gtsam/slam/BearingFactor.h

@@ -9,94 +9,13 @@
 
  * -------------------------------------------------------------------------- */
 
-/**
- *  @file  BearingFactor.H
- *  @author Frank Dellaert
- **/
-
 #pragma once
 
-#include <gtsam/nonlinear/NonlinearFactor.h>
+#ifdef _MSC_VER
-#include <gtsam/geometry/concepts.h>
+#pragma message("BearingFactor is now an ExpressionFactor in SAM directory")
-#include <gtsam/base/Testable.h>
+#else
+#warning "BearingFactor is now an ExpressionFactor in SAM directory"
+#endif
 
-namespace gtsam {
 
-  /**
+#include <gtsam/sam/BearingFactor.h>
-   * Binary factor for a bearing measurement
-   * @addtogroup SLAM
-   */
-  template<class POSE, class POINT, class ROTATION = typename POSE::Rotation>
-  class BearingFactor: public NoiseModelFactor2<POSE, POINT> {
-  private:
-
-    typedef POSE Pose;
-    typedef ROTATION Rot;
-    typedef POINT Point;
-
-    typedef BearingFactor<POSE, POINT> This;
-    typedef NoiseModelFactor2<POSE, POINT> Base;
-
-    Rot measured_; /** measurement */
-
-    /** concept check by type */
-    GTSAM_CONCEPT_TESTABLE_TYPE(Rot)
-    GTSAM_CONCEPT_POSE_TYPE(Pose)
-
-  public:
-
-    /** default constructor for serialization/testing only */
-    BearingFactor() {}
-
-    /** primary constructor */
-    BearingFactor(Key poseKey, Key pointKey, const Rot& measured,
-        const SharedNoiseModel& model) :
-          Base(model, poseKey, pointKey), measured_(measured) {
-    }
-
-    virtual ~BearingFactor() {}
-
-    /// @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 -> between(z,h(x)) for Rot2 manifold */
-    Vector evaluateError(const Pose& pose, const Point& point,
-        boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-      Rot hx = pose.bearing(point, H1, H2);
-      return Rot::Logmap(measured_.between(hx));
-    }
-
-    /** return the measured */
-    const Rot& measured() const {
-      return measured_;
-    }
-
-    /** equals */
-    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) && this->measured_.equals(e->measured_, tol);
-    }
-
-    /** print contents */
-    void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-      std::cout << s << "BearingFactor, bearing = ";
-      measured_.print();
-      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_);
-    }
-
-  }; // BearingFactor
-
-} // namespace gtsam

gtsam/slam/BearingRangeFactor.h

@@ -9,116 +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/geometry/concepts.h>
+#ifdef _MSC_VER
-#include <gtsam/base/Testable.h>
+#pragma message( \
-#include <gtsam/nonlinear/NonlinearFactor.h>
+    "BearingRangeFactor is now an ExpressionFactor in SAM directory")
+#else
+#warning "BearingRangeFactor is now an ExpressionFactor in SAM directory"
+#endif
 
-namespace gtsam {
+#include <gtsam/sam/BearingRangeFactor.h>
-
-  /**
-   * Binary factor for a bearing measurement
-   * @addtogroup SLAM
-   */
-  template<class POSE, class POINT, class ROTATION = typename POSE::Rotation>
-  class BearingRangeFactor: public NoiseModelFactor2<POSE, POINT>
-  {
-  public:
-    typedef BearingRangeFactor<POSE, POINT> This;
-    typedef NoiseModelFactor2<POSE, POINT> Base;
-    typedef boost::shared_ptr<This> shared_ptr;
-
-  private:
-    typedef POSE Pose;
-    typedef ROTATION Rot;
-    typedef POINT Point;
-
-    // the measurement
-    Rot measuredBearing_;
-    double measuredRange_;
-
-    /** concept check by type */
-    GTSAM_CONCEPT_TESTABLE_TYPE(Rot)
-    GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(Pose, Point)
-    GTSAM_CONCEPT_POSE_TYPE(Pose)
-
-  public:
-
-    BearingRangeFactor() {} /* Default constructor */
-    BearingRangeFactor(Key poseKey, Key pointKey, const Rot& measuredBearing, const double 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, double 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 Rot manifold */
-    Vector evaluateError(const Pose& pose, const Point& point,
-        boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-      Matrix H11, H21, H12, H22;
-      boost::optional<Matrix&> H11_ = H1 ? boost::optional<Matrix&>(H11) : boost::optional<Matrix&>();
-      boost::optional<Matrix&> H21_ = H1 ? boost::optional<Matrix&>(H21) : boost::optional<Matrix&>();
-      boost::optional<Matrix&> H12_ = H2 ? boost::optional<Matrix&>(H12) : boost::optional<Matrix&>();
-      boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>();
-
-      Rot y1 = pose.bearing(point, H11_, H12_);
-      Vector e1 = Rot::Logmap(measuredBearing_.between(y1));
-
-      double y2 = pose.range(point, H21_, H22_);
-      Vector e2 = (Vector(1) << y2 - measuredRange_).finished();
-
-      if (H1) *H1 = gtsam::stack(2, &H11, &H21);
-      if (H2) *H2 = gtsam::stack(2, &H12, &H22);
-      return concatVectors(2, &e1, &e2);
-    }
-
-    /** return the measured */
-    const std::pair<Rot, double> measured() const {
-      return std::make_pair(measuredBearing_, measuredRange_);
-    }
-
-  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(measuredBearing_);
-      ar & BOOST_SERIALIZATION_NVP(measuredRange_);
-    }
-  }; // BearingRangeFactor
-
-} // namespace gtsam

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/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/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/smart/CMakeLists.txt

@@ -0,0 +1,6 @@
+# Install headers
+file(GLOB smart_headers "*.h")
+install(FILES ${smart_headers} DESTINATION include/gtsam/smart)
+
+# Build tests
+add_subdirectory(tests)

gtsam/smart/tests/CMakeLists.txt

@@ -0,0 +1 @@
+gtsamAddTestsGlob(smart "test*.cpp" "" "gtsam")

gtsam_unstable/dynamics/PoseRTV.h

@@ -170,4 +170,17 @@ private:
 template<>
 struct traits<PoseRTV> : public internal::LieGroup<PoseRTV> {};
 
+// Define Range functor specializations that are used in RangeFactor
+template <typename A1, typename A2> struct Range;
+
+template <>
+struct Range<PoseRTV, PoseRTV> {
+  typedef double result_type;
+  double operator()(const PoseRTV& pose1, const PoseRTV& pose2,
+                    OptionalJacobian<1, 9> H1 = boost::none,
+                    OptionalJacobian<1, 9> H2 = boost::none) {
+    return pose1.range(pose2, H1, H2);
+  }
+};
+
 } // \namespace gtsam

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);
@@ -452,7 +452,7 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor {
   DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model);
 };
 
-#include <gtsam/base/LieScalar_Deprecated.h>
+#include <gtsam/base/deprecated/LieScalar.h>
 
 #include <gtsam_unstable/dynamics/VelocityConstraint3.h>
 virtual class VelocityConstraint3 : gtsam::NonlinearFactor {

gtsam_unstable/slam/serialization.cpp

@@ -5,20 +5,19 @@
  * @author Alex Cunningham
  */
 
-#include <gtsam/base/LieMatrix_Deprecated.h>
+#include <gtsam/base/deprecated/LieMatrix_Deprecated.h>
-#include <gtsam/base/LieVector_Deprecated.h>
+#include <gtsam/base/deprecated/LieVector_Deprecated.h>
 #include <gtsam/slam/serialization.h>
 #include <gtsam/base/serialization.h>
 
 //#include <gtsam/slam/AntiFactor.h>
-#include <gtsam/slam/BearingFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 //#include <gtsam/slam/BoundingConstraint.h>
 #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>
@@ -80,9 +79,6 @@ typedef RangeFactor<SimpleCamera, Point3>               RangeFactorSimpleCameraP
 typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
 typedef RangeFactor<SimpleCamera, SimpleCamera>         RangeFactorSimpleCamera;
 
-typedef BearingFactor<Pose2, Point2, Rot2> BearingFactor2D;
-typedef BearingFactor<Pose3, Point3, Rot3> BearingFactor3D;
-
 typedef BearingRangeFactor<Pose2, Point2>  BearingRangeFactor2D;
 typedef BearingRangeFactor<Pose3, Point3>  BearingRangeFactor3D;
 
@@ -185,8 +181,6 @@ BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleC
 BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera");
 BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera");
 
-BOOST_CLASS_EXPORT_GUID(BearingFactor2D, "gtsam::BearingFactor2D");
-
 BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D");
 
 BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2");

gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp

@@ -21,7 +21,7 @@
 #include <gtsam/inference/Key.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/LieVector_Deprecated.h>
+#include <gtsam/base/deprecated/LieVector.h>
 
 using namespace std;
 using namespace gtsam;

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/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/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>
@@ -25,7 +25,7 @@
 #include <gtsam/base/treeTraversal-inst.h>
 #include <boost/foreach.hpp>
 #include <boost/assign/list_of.hpp>
-#include <gtsam/base/LieScalar_Deprecated.h>
+#include <gtsam/base/deprecated/LieScalar.h>
 using namespace boost::assign;
 #include <boost/range/adaptor/map.hpp>
 namespace br { using namespace boost::adaptors; using namespace boost::range; }

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,15 +22,14 @@
 
 #include <tests/smallExample.h>
 //#include <gtsam/slam/AntiFactor.h>
-#include <gtsam/slam/BearingFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 //#include <gtsam/slam/BoundingConstraint.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
 //#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>
@@ -106,9 +105,6 @@ typedef RangeFactor<SimpleCamera, Point3>               RangeFactorSimpleCameraP
 typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
 typedef RangeFactor<SimpleCamera, SimpleCamera>         RangeFactorSimpleCamera;
 
-typedef BearingFactor<Pose2, Point2, Rot2> BearingFactor2D;
-typedef BearingFactor<Pose3, Point3, Rot3> BearingFactor3D;
-
 typedef BearingRangeFactor<Pose2, Point2>  BearingRangeFactor2D;
 typedef BearingRangeFactor<Pose3, Point3>  BearingRangeFactor3D;
 
@@ -217,8 +213,6 @@ BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleC
 BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera");
 BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera");
 
-BOOST_CLASS_EXPORT_GUID(BearingFactor2D, "gtsam::BearingFactor2D");
-
 BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D");
 
 BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2");
@@ -393,8 +387,6 @@ TEST (testSerializationSLAM, factors) {
   RangeFactorCalibratedCamera rangeFactorCalibratedCamera(a12, b12, 2.0, model1);
   RangeFactorSimpleCamera rangeFactorSimpleCamera(a13, b13, 2.0, model1);
 
-  BearingFactor2D bearingFactor2D(a08, a03, rot2, model1);
-
   BearingRangeFactor2D bearingRangeFactor2D(a08, a03, rot2, 2.0, model2);
 
   GenericProjectionFactorCal3_S2 genericProjectionFactorCal3_S2(point2, model2, a09, a05, boost::make_shared<Cal3_S2>(cal3_s2));
@@ -456,8 +448,6 @@ TEST (testSerializationSLAM, factors) {
   graph += rangeFactorCalibratedCamera;
   graph += rangeFactorSimpleCamera;
 
-  graph += bearingFactor2D;
-
   graph += bearingRangeFactor2D;
 
   graph += genericProjectionFactorCal3_S2;
@@ -524,8 +514,6 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsObj<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
   EXPECT(equalsObj<RangeFactorSimpleCamera>(rangeFactorSimpleCamera));
 
-  EXPECT(equalsObj<BearingFactor2D>(bearingFactor2D));
-
   EXPECT(equalsObj<BearingRangeFactor2D>(bearingRangeFactor2D));
 
   EXPECT(equalsObj<GenericProjectionFactorCal3_S2>(genericProjectionFactorCal3_S2));
@@ -592,8 +580,6 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsXML<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
   EXPECT(equalsXML<RangeFactorSimpleCamera>(rangeFactorSimpleCamera));
 
-  EXPECT(equalsXML<BearingFactor2D>(bearingFactor2D));
-
   EXPECT(equalsXML<BearingRangeFactor2D>(bearingRangeFactor2D));
 
   EXPECT(equalsXML<GenericProjectionFactorCal3_S2>(genericProjectionFactorCal3_S2));
@@ -660,8 +646,6 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsBinary<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
   EXPECT(equalsBinary<RangeFactorSimpleCamera>(rangeFactorSimpleCamera));
 
-  EXPECT(equalsBinary<BearingFactor2D>(bearingFactor2D));
-
   EXPECT(equalsBinary<BearingRangeFactor2D>(bearingRangeFactor2D));
 
   EXPECT(equalsBinary<GenericProjectionFactorCal3_S2>(genericProjectionFactorCal3_S2));

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>

timing/timeRot2.cpp

@@ -92,11 +92,8 @@ int main()
   int n = 50000000;
   cout << "NOTE:  Times are reported for " << n << " calls" << endl;
 
-  // create a random direction:
+  Vector1 v; v << 0.1;
-  double norm=sqrt(16.0+4.0);
+  Rot2 R = Rot2(0.4), R2(0.5), R3(0.6);
-  double x=4.0/norm, y=2.0/norm;
-  Vector v = (Vector(2) << x, y).finished();
-  Rot2 R = Rot2(0.4), R2 = R.retract(v), R3(0.6);
 
   TEST(Rot2_Expmap, Rot2::Expmap(v));
   TEST(Rot2_Retract, R.retract(v));