Define and partially specify Range and Bearing functors

gtsam.h

@@ -2272,20 +2272,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/sam/BearingFactor.h>
-template<POSE, POINT, MEASURED>
+template<POSE, POINT, BEARING>
 virtual class BearingFactor : gtsam::NoiseModelFactor {
-  BearingFactor(size_t key1, size_t key2, const MEASURED& 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;
@@ -2295,9 +2291,9 @@ typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFa
 
 
 #include <gtsam/slam/BearingRangeFactor.h>
-template<POSE, POINT, MEASURED>
+template<POSE, POINT, BEARING>
 virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
-  BearingRangeFactor(size_t poseKey, size_t pointKey, const MEASURED& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);
+  BearingRangeFactor(size_t poseKey, size_t pointKey, const BEARING& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);
 
   pair<MEASURED, double> measured() const;
 

gtsam/geometry/CalibratedCamera.h

@@ -318,7 +318,7 @@ public:
   }
 
   /// @}
-  /// @name Transformations and mesaurement functions
+  /// @name Transformations and measurement functions
   /// @{
 
   /**
@@ -393,5 +393,38 @@ struct traits<const CalibratedCamera> : public internal::Manifold<
     CalibratedCamera> {
 };
 
+// Define Range functor specializations that are used in RangeFactor
+template <typename A1, typename A2> struct Range;
+
+template <>
+struct Range<CalibratedCamera, Point3> {
+  typedef double result_type;
+  double operator()(const CalibratedCamera& camera, const Point3& point,
+                    OptionalJacobian<1, 6> H1 = boost::none,
+                    OptionalJacobian<1, 3> H2 = boost::none) {
+    return camera.range(point, H1, H2);
+  }
+};
+
+template <>
+struct Range<CalibratedCamera, Pose3> {
+  typedef double result_type;
+  double operator()(const CalibratedCamera& camera, const Pose3& pose,
+                    OptionalJacobian<1, 6> H1 = boost::none,
+                    OptionalJacobian<1, 6> H2 = boost::none) {
+    return camera.range(pose, H1, H2);
+  }
+};
+
+template <>
+struct Range<CalibratedCamera, CalibratedCamera> {
+  typedef double result_type;
+  double operator()(const CalibratedCamera& camera1,
+                    const CalibratedCamera& camera2,
+                    OptionalJacobian<1, 6> H1 = boost::none,
+                    OptionalJacobian<1, 6> H2 = boost::none) {
+    return camera1.range(camera2, H1, H2);
+  }
+};
 }
 

gtsam/geometry/PinholeCamera.h

@@ -263,24 +263,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)
    */
@@ -314,4 +312,52 @@ struct traits<const PinholeCamera<Calibration> > : public internal::Manifold<
     PinholeCamera<Calibration> > {
 };
 
+// Define Range functor specializations that are used in RangeFactor
+template <typename A1, typename A2> struct Range;
+
+template <typename Calibration>
+struct Range<PinholeCamera<Calibration>, Point3> {
+  typedef double result_type;
+  typedef PinholeCamera<Calibration> Camera;
+  double operator()(const Camera& camera, const Point3& point,
+                    OptionalJacobian<1, Camera::dimension> H1 = boost::none,
+                    OptionalJacobian<1, 3> H2 = boost::none) {
+    return camera.range(point, H1, H2);
+  }
+};
+
+template <typename Calibration>
+struct Range<PinholeCamera<Calibration>, Pose3> {
+  typedef double result_type;
+  typedef PinholeCamera<Calibration> Camera;
+  double operator()(const Camera& camera, const Pose3& pose,
+                    OptionalJacobian<1, Camera::dimension> H1 = boost::none,
+                    OptionalJacobian<1, 6> H2 = boost::none) {
+    return camera.range(pose, H1, H2);
+  }
+};
+
+template <typename CalibrationA, typename CalibrationB>
+struct Range<PinholeCamera<CalibrationA>, PinholeCamera<CalibrationB> > {
+  typedef double result_type;
+  typedef PinholeCamera<CalibrationA> CameraA;
+  typedef PinholeCamera<CalibrationB> CameraB;
+  double operator()(const CameraA& camera1, const CameraB& camera2,
+                    OptionalJacobian<1, CameraA::dimension> H1 = boost::none,
+                    OptionalJacobian<1, CameraB::dimension> H2 = boost::none) {
+    return camera1.range(camera2, H1, H2);
+  }
+};
+
+template <typename Calibration>
+struct Range<PinholeCamera<Calibration>, CalibratedCamera> {
+  typedef double result_type;
+  typedef PinholeCamera<Calibration> Camera;
+  double operator()(const Camera& camera, const CalibratedCamera& cc,
+                    OptionalJacobian<1, Camera::dimension> H1 = boost::none,
+                    OptionalJacobian<1, 6> H2 = boost::none) {
+    return camera.range(cc, H1, H2);
+  }
+};
+
 }  // \ 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

@@ -23,6 +23,7 @@
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/base/Lie.h>
+#include <gtsam/dllexport.h>
 
 namespace gtsam {
 
@@ -295,5 +296,51 @@ struct traits<Pose2> : public internal::LieGroup<Pose2> {};
 template<>
 struct traits<const Pose2> : public internal::LieGroup<Pose2> {};
 
+// Define Bearing functor specializations that are used in BearingFactor
+template <typename A1, typename A2> struct Bearing;
+
+template <>
+struct Bearing<Pose2, Point2> {
+  typedef Rot2 result_type;
+  Rot2 operator()(const Pose2& pose, const Point2& point,
+                  OptionalJacobian<1, 3> H1 = boost::none,
+                  OptionalJacobian<1, 2> H2 = boost::none) {
+    return pose.bearing(point, H1, H2);
+  }
+};
+
+template <>
+struct Bearing<Pose2, Pose2> {
+  typedef Rot2 result_type;
+  Rot2 operator()(const Pose2& pose1, const Pose2& pose2,
+                  OptionalJacobian<1, 3> H1 = boost::none,
+                  OptionalJacobian<1, 3> H2 = boost::none) {
+    return pose1.bearing(pose2, H1, H2);
+  }
+};
+
+// Define Range functor specializations that are used in RangeFactor
+template <typename A1, typename A2> struct Range;
+
+template <>
+struct Range<Pose2, Point2> {
+  typedef double result_type;
+  double operator()(const Pose2& pose, const Point2& point,
+                    OptionalJacobian<1, 3> H1 = boost::none,
+                    OptionalJacobian<1, 2> H2 = boost::none) {
+    return pose.range(point, H1, H2);
+  }
+};
+
+template <>
+struct Range<Pose2, Pose2> {
+  typedef double result_type;
+  double operator()(const Pose2& pose1, const Pose2& pose2,
+                    OptionalJacobian<1, 3> H1 = boost::none,
+                    OptionalJacobian<1, 3> H2 = boost::none) {
+    return pose1.range(pose2, H1, H2);
+  }
+};
+
 } // namespace gtsam
 

gtsam/geometry/Pose3.h

@@ -337,5 +337,40 @@ struct traits<Pose3> : public internal::LieGroup<Pose3> {};
 template <>
 struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
 
+// Define Bearing functor specializations that are used in BearingFactor
+template <typename A1, typename A2> struct Bearing;
+
+template <>
+struct Bearing<Pose3, Point3> {
+  typedef Unit3 result_type;
+  Unit3 operator()(const Pose3& pose, const Point3& point,
+                   OptionalJacobian<2, 6> H1 = boost::none,
+                   OptionalJacobian<2, 3> H2 = boost::none) {
+    return pose.bearing(point, H1, H2);
+  }
+};
+
+// Define Range functor specializations that are used in RangeFactor
+template <typename A1, typename A2> struct Range;
+
+template <>
+struct Range<Pose3, Point3> {
+  typedef double result_type;
+  double operator()(const Pose3& pose, const Point3& point,
+                    OptionalJacobian<1, 6> H1 = boost::none,
+                    OptionalJacobian<1, 3> H2 = boost::none) {
+    return pose.range(point, H1, H2);
+  }
+};
+
+template <>
+struct Range<Pose3, Pose3> {
+  typedef double result_type;
+  double operator()(const Pose3& pose1, const Pose3& pose2,
+                    OptionalJacobian<1, 6> H1 = boost::none,
+                    OptionalJacobian<1, 6> H2 = boost::none) {
+    return pose1.range(pose2, H1, H2);
+  }
+};
 
 }  // 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/sam/BearingFactor.h

@@ -11,8 +11,7 @@
 
 /**
  *  @file  BearingFactor.h
- *  @brief Serializable factor induced by a bearing measurement of a point from
+ *  @brief Serializable factor induced by a bearing measurement
- *a given pose
  *  @date July 2015
  *  @author Frank Dellaert
  **/
@@ -20,17 +19,23 @@
 #pragma once
 
 #include <gtsam/nonlinear/SerializableExpressionFactor.h>
-#include <gtsam/geometry/concepts.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 POSE, typename POINT, typename T>
+template <typename A1, typename A2,
-struct BearingFactor : public SerializableExpressionFactor2<T, POSE, POINT> {
+          typename T = typename Bearing<A1, A2>::result_type>
-  typedef SerializableExpressionFactor2<T, POSE, POINT> Base;
+struct BearingFactor : public SerializableExpressionFactor2<T, A1, A2> {
+  typedef SerializableExpressionFactor2<T, A1, A2> Base;
 
   /// default constructor
   BearingFactor() {}
@@ -44,8 +49,9 @@ struct BearingFactor : public SerializableExpressionFactor2<T, POSE, POINT> {
 
   // Return measurement expression
   virtual Expression<T> expression(Key key1, Key key2) const {
-    return Expression<T>(Expression<POSE>(key1), &POSE::bearing,
+    Expression<A1> a1_(key1);
-                         Expression<POINT>(key2));
+    Expression<A2> a2_(key2);
+    return Expression<T>(Bearing<A1, A2>(), a1_, a2_);
   }
 
   /// print
@@ -57,8 +63,8 @@ struct BearingFactor : public SerializableExpressionFactor2<T, POSE, POINT> {
 };  // BearingFactor
 
 /// traits
-template <class POSE, class POINT, class T>
+template <typename A1, typename A2, typename T>
-struct traits<BearingFactor<POSE, POINT, T> >
+struct traits<BearingFactor<A1, A2, T> >
-    : public Testable<BearingFactor<POSE, POINT, T> > {};
+    : public Testable<BearingFactor<A1, A2, T> > {};
 
 }  // namespace gtsam

gtsam/sam/RangeFactor.h

@@ -11,8 +11,7 @@
 
 /**
  *  @file  RangeFactor.h
- *  @brief Serializable factor induced by a range measurement between two points
+ *  @brief Serializable factor induced by a range measurement
- *and/or poses
  *  @date July 2015
  *  @author Frank Dellaert
  **/
@@ -20,30 +19,30 @@
 #pragma once
 
 #include <gtsam/nonlinear/SerializableExpressionFactor.h>
-#include <gtsam/geometry/concepts.h>
-#include <boost/lexical_cast.hpp>
 
 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 <class A1, class A2 = A1>
+template <typename A1, typename A2 = A1,
-struct RangeFactor : public SerializableExpressionFactor2<double, A1, A2> {
+          typename T = typename Range<A1, A2>::result_type>
+class RangeFactor : public SerializableExpressionFactor2<T, A1, A2> {
  private:
   typedef RangeFactor<A1, A2> This;
-  typedef SerializableExpressionFactor2<double, A1, A2> Base;
+  typedef SerializableExpressionFactor2<T, A1, A2> Base;
-
-  // Concept requirements for this factor
-  GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(A1, A2)
 
  public:
   /// default constructor
   RangeFactor() {}
 
-  RangeFactor(Key key1, Key key2, double measured,
+  RangeFactor(Key key1, Key key2, T measured, const SharedNoiseModel& model)
-              const SharedNoiseModel& model)
       : Base(key1, key2, model, measured) {
     this->initialize(expression(key1, key2));
   }
@@ -55,10 +54,10 @@ struct RangeFactor : public SerializableExpressionFactor2<double, A1, A2> {
   }
 
   // Return measurement expression
-  virtual Expression<double> expression(Key key1, Key key2) const {
+  virtual Expression<T> expression(Key key1, Key key2) const {
-    Expression<A1> t1_(key1);
+    Expression<A1> a1_(key1);
-    Expression<A2> t2_(key2);
+    Expression<A2> a2_(key2);
-    return Expression<double>(t1_, &A1::range, t2_);
+    return Expression<T>(Range<A1, A2>(), a1_, a2_);
   }
 
   /// print
@@ -70,30 +69,29 @@ struct RangeFactor : public SerializableExpressionFactor2<double, A1, A2> {
 };  // \ RangeFactor
 
 /// traits
-template <class A1, class A2>
+template <typename A1, typename A2, typename T>
-struct traits<RangeFactor<A1, A2> > : public Testable<RangeFactor<A1, A2> > {};
+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 <class A1, class A2 = A1>
+template <typename A1, typename A2 = A1,
+          typename T = typename Range<A1, A2>::result_type>
 class RangeFactorWithTransform
-    : public SerializableExpressionFactor2<double, A1, A2> {
+    : public SerializableExpressionFactor2<T, A1, A2> {
  private:
   typedef RangeFactorWithTransform<A1, A2> This;
-  typedef SerializableExpressionFactor2<double, A1, A2> Base;
+  typedef SerializableExpressionFactor2<T, A1, A2> Base;
 
   A1 body_T_sensor_;  ///< The pose of the sensor in the body frame
 
-  // Concept requirements for this factor
-  GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(A1, A2)
-
  public:
   //// Default constructor
   RangeFactorWithTransform() {}
 
-  RangeFactorWithTransform(Key key1, Key key2, double measured,
+  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) {
@@ -109,13 +107,13 @@ class RangeFactorWithTransform
   }
 
   // Return measurement expression
-  virtual Expression<double> expression(Key key1, Key key2) const {
+  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> t2_(key2);
+    Expression<A2> a2_(key2);
-    return Expression<double>(nav_T_sensor_, &A1::range, t2_);
+    return Expression<T>(Range<A1, A2>(), nav_T_sensor_, a2_);
   }
 
   /** print contents */
@@ -128,8 +126,8 @@ class RangeFactorWithTransform
 
  private:
   /** Serialization function */
-  friend class boost::serialization::access;
+  friend typename boost::serialization::access;
-  template <class ARCHIVE>
+  template <typename ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
     ar& boost::serialization::make_nvp(
         "Base", boost::serialization::base_object<Base>(*this));
@@ -138,8 +136,8 @@ class RangeFactorWithTransform
 };  // \ RangeFactorWithTransform
 
 /// traits
-template <class A1, class A2>
+template <typename A1, typename A2, typename T>
-struct traits<RangeFactorWithTransform<A1, A2> >
+struct traits<RangeFactorWithTransform<A1, A2, T> >
-    : public Testable<RangeFactorWithTransform<A1, A2> > {};
+    : public Testable<RangeFactorWithTransform<A1, A2, T> > {};
 
 }  // \ namespace gtsam

gtsam/sam/tests/testBearingFactor.cpp

@@ -32,12 +32,12 @@ using namespace gtsam;
 Key poseKey(1);
 Key pointKey(2);
 
-typedef BearingFactor<Pose2, Point2, Rot2> BearingFactor2D;
+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, Unit3> BearingFactor3D;
+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);

gtsam/sam/tests/testRangeFactor.cpp

@@ -344,6 +344,58 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
   CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
 }
 
+/* ************************************************************************* */
+// Do a test with Point3
+TEST(RangeFactor, Point3) {
+  // Create a factor
+  Key poseKey(1);
+  Key pointKey(2);
+  double measurement(10.0);
+  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 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();
+  }
+};
+}
+
+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/slam/BearingRangeFactor.h

@@ -47,7 +47,6 @@ namespace gtsam {
 
     /** concept check by type */
     GTSAM_CONCEPT_TESTABLE_TYPE(Rot)
-    GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(Pose, Point)
     GTSAM_CONCEPT_POSE_TYPE(Pose)
 
   public:

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