keys and Jacobians

gtsam_unstable/slam/SmartRangeFactor.h

@@ -60,6 +60,7 @@ public:
   /// Add a range measurement to a pose with given key.
   void addRange(Key key, double measuredRange) {
     measurements_.push_back(KeyedRange(key, measuredRange));
+    keys_.push_back(key);
   }
 
   /// Number of measurements added
@@ -102,8 +103,7 @@ public:
     }
     std::list<Point2> solutions = Point2::CircleCircleIntersection(
         circle1.center, best_circle->center, best_fh);
-    solutions.front().print("front");
+    // TODO, pick winner based on other measurement
-    solutions.back().print("back");
     return solutions.front();
   }
 
@@ -118,13 +118,19 @@ public:
       std::list<Circle2> circles;
       BOOST_FOREACH(const KeyedRange& it, measurements_) {
         const Pose2& pose = x.at<Pose2>(it.key);
-        circles.push_back(
+        circles.push_back(Circle2(pose.translation(), it.range));
-            Circle2(pose.translation(), it.range));
       }
       Point2 optimizedPoint = triangulate(circles);
       size_t i = 0;
-      BOOST_FOREACH(const Circle2& it, circles) {
+      if (H) *H = std::vector<Matrix>();
-        errors[i++] = it.radius - it.center.distance(optimizedPoint);
+      BOOST_FOREACH(const KeyedRange& it, measurements_) {
+        const Pose2& pose = x.at<Pose2>(it.key);
+        if (H) {
+          Matrix Hi;
+          errors[i] = pose.range(optimizedPoint, Hi) - it.range;
+          H->push_back(Hi);
+        } else
+          i += 1;
       }
     }
     return errors;

gtsam_unstable/slam/tests/testSmartRangeFactor.cpp

@@ -70,8 +70,14 @@ TEST( SmartRangeFactor, unwhitenedError ) {
   EXPECT(assert_equal(Vector2(0,0), actual2));
 
   f.addRange(3, r3);
-  Vector actual3 = f.unwhitenedError(values);
+  vector<Matrix> H;
+  Vector actual3 = f.unwhitenedError(values,H);
   EXPECT(assert_equal(Vector3(0,0,0), actual3));
+
+  // Check keys and Jacobian
+  EXPECT_LONGS_EQUAL(3,f.keys().size());
+  EXPECT(assert_equal(Matrix_(1,3,0.0,-1.0,0.0), H.front()));
+  EXPECT(assert_equal(Matrix_(1,3,sqrt(2)/2,-sqrt(2)/2,0.0), H.back()));
 }
 
 /* ************************************************************************* */