From 59c258623aa621333dcb39eba1c14ded1ae839e3 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 12 Jul 2015 22:30:37 -0700
Subject: [PATCH 1/3] Fixed friend declaration

---
 gtsam/sam/RangeFactor.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h
index 6e1a33481..a7e535646 100644
--- a/gtsam/sam/RangeFactor.h
+++ b/gtsam/sam/RangeFactor.h
@@ -126,7 +126,7 @@ class RangeFactorWithTransform
 
  private:
   /** Serialization function */
-  friend typename boost::serialization::access;
+  friend class boost::serialization::access;
   template <typename ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
     ar& boost::serialization::make_nvp(

From 6bb5b03c7a74ce5e0b07c68846dd49240a4090cc Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 12 Jul 2015 22:48:54 -0700
Subject: [PATCH 2/3] Added two specializations for matlab wrapper

---
 gtsam/geometry/SimpleCamera.h | 24 ++++++++++++++++++++++--
 1 file changed, 22 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h
index a119096d4..09b128d91 100644
--- a/gtsam/geometry/SimpleCamera.h
+++ b/gtsam/geometry/SimpleCamera.h
@@ -133,6 +133,26 @@ template<>
 struct traits<const SimpleCamera> : public internal::Manifold<SimpleCamera> {
 };
 
-  /// Recover camera from 3*4 camera matrix
-  GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);
+template <>
+struct Range<SimpleCamera, Point3> {
+  typedef double result_type;
+  double operator()(const SimpleCamera& camera, const Point3& point,
+                    OptionalJacobian<1, 11> H1 = boost::none,
+                    OptionalJacobian<1, 3> H2 = boost::none) {
+    return camera.range(point, H1, H2);
+  }
+};
+
+template <>
+struct Range<SimpleCamera, SimpleCamera> {
+  typedef double result_type;
+  double operator()(const SimpleCamera& camera, const SimpleCamera& sc,
+                    OptionalJacobian<1, 11> H1 = boost::none,
+                    OptionalJacobian<1, 11> H2 = boost::none) {
+    return camera.range(sc, H1, H2);
+  }
+};
+
+/// Recover camera from 3*4 camera matrix
+GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);
 }

From b711f5f96439500895d2a79969bb84e887602191 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 12 Jul 2015 22:49:04 -0700
Subject: [PATCH 3/3] Fixed wrapper

---
 gtsam.h | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index d874467fc..1ddae3f48 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -1,4 +1,5 @@
 /**
+
  * GTSAM Wrap Module Definition
  *
  * These are the current classes available through the matlab toolbox interface,
@@ -2289,19 +2290,18 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {
 
 typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
 
-
 #include <gtsam/sam/BearingRangeFactor.h>
-template<POSE, POINT, BEARING>
+template<POSE, POINT, BEARING, RANGE>
 virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
-  BearingRangeFactor(size_t poseKey, size_t pointKey, const BEARING& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);
-
-  pair<MEASURED, double> measured() const;
+  BearingRangeFactor(size_t poseKey, size_t pointKey,
+      const BEARING& measuredBearing, const RANGE& measuredRange,
+      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>