addMeasurements adds a bunch of measurements at the same time

gtsam.h

@@ -1288,10 +1288,14 @@ class Graph {
   void addRangeFactor(size_t poseKey, size_t pointKey, double range, const gtsam::noiseModel::Base* model);
 
   // Measurements
-  void addMeasurement(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model,
+    void addMeasurement(const gtsam::Point2& measured,
-      size_t poseKey, size_t pointKey, const gtsam::Cal3_S2* K);
+        const gtsam::noiseModel::Base* model, size_t poseKey, size_t pointKey,
-  void addStereoMeasurement(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* model,
+        const gtsam::Cal3_S2* K);
-      size_t poseKey, size_t pointKey, const gtsam::Cal3_S2Stereo* K);
+    void addMeasurements(size_t i, const gtsam::KeyVector& J, Matrix Z,
+        const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
+    void addStereoMeasurement(const gtsam::StereoPoint2& measured,
+        const gtsam::noiseModel::Base* model, size_t poseKey, size_t pointKey,
+        const gtsam::Cal3_S2Stereo* K);
 };
 
 class ISAM {

gtsam/slam/visualSLAM.cpp

@@ -47,7 +47,20 @@ namespace visualSLAM {
   /* ************************************************************************* */
   void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model,
        Key poseKey, Key pointKey, const shared_ptrK K) {
-    push_back(make_shared<GenericProjectionFactor<Pose3, Point3> >(measured, model, poseKey, pointKey, K));
+    push_back(
+        make_shared<GenericProjectionFactor<Pose3, Point3> >
+          (measured, model, poseKey, pointKey, K));
+  }
+
+  /* ************************************************************************* */
+  void Graph::addMeasurements(Key i, const KeyVector& J, const Matrix& Z,
+      const SharedNoiseModel& model, const shared_ptrK K) {
+    if ( Z.rows()!=2) throw std::invalid_argument("addMeasurements: Z must be 2*K");
+    if (Z.cols() != J.size())
+      throw std::invalid_argument(
+          "addMeasurements: J and Z must have same number of entries");
+    for(size_t k=0;k<Z.cols();k++)
+      addMeasurement(Point2(Z(0,k),Z(1,k)),model,i, J[k], K);
   }
 
   /* ************************************************************************* */

gtsam/slam/visualSLAM.h

@@ -94,14 +94,14 @@ namespace visualSLAM {
 
     /**
      *  Add a constraint on a point (for now, *must* be satisfied in any Values)
-     *  @param key variable key of the landmark
+     *  @param key variable key of the point
      *  @param p point around which soft prior is defined
      */
     void addPointConstraint(Key pointKey, const Point3& p = Point3());
 
     /**
-     *  Add a prior on a landmark
+     *  Add a prior on a point
-     *  @param key variable key of the landmark
+     *  @param key variable key of the point
      *  @param p to which point to constrain it to
      *  @param model uncertainty model of this prior
      */
@@ -109,10 +109,10 @@ namespace visualSLAM {
 				const SharedNoiseModel& model = noiseModel::Unit::Create(3));
 
     /**
-     *  Add a range prior to a landmark
+     *  Add a range prior to a point
      *  @param poseKey variable key of the camera pose
-     *  @param pointKey variable key of the landmark
+     *  @param pointKey variable key of the point
-     *  @param range approximate range to landmark
+     *  @param range approximate range to point
      *  @param model uncertainty model of this prior
      */
     void addRangeFactor(Key poseKey, Key pointKey, double range,
@@ -123,18 +123,29 @@ namespace visualSLAM {
      *  @param measured the measurement
      *  @param model the noise model for the measurement
      *  @param poseKey variable key for the camera pose
-     *  @param pointKey variable key for the landmark
+     *  @param pointKey variable key for the point
      *  @param K shared pointer to calibration object
      */
     void addMeasurement(const Point2& measured, const SharedNoiseModel& model,
         Key poseKey, Key pointKey, const shared_ptrK K);
 
+    /**
+     *  Add a number of measurements at the same time
+     *  @param i variable key for the camera pose
+     *  @param J variable keys for the point, KeyVector of size K
+     *  @param Z the 2*K measurements as a matrix
+     *  @param model the noise model for the measurement
+     *  @param K shared pointer to calibration object
+     */
+    void addMeasurements(Key i, const KeyVector& J, const Matrix& Z,
+        const SharedNoiseModel& model, const shared_ptrK K);
+
     /**
      *  Add a stereo factor measurement
      *  @param measured the measurement
      *  @param model the noise model for the measurement
      *  @param poseKey variable key for the camera pose
-     *  @param pointKey variable key for the landmark
+     *  @param pointKey variable key for the point
      *  @param K shared pointer to stereo calibration object
      */
     void addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model,