Added a C++ function for matlab to insert multiple projection factors from a single frame.

gtsam.h

@@ -1550,6 +1550,7 @@ namespace utilities {
   void perturbPoint2(gtsam::Values& values, double sigma, int seed);
   void perturbPoint3(gtsam::Values& values, double sigma, int seed);
   void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth);
+  void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
   Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);
 
 } //\namespace utilities

matlab.h

@@ -108,6 +108,19 @@ namespace gtsam {
       }
     }
 
+    /// insert multiple projection factors for a single pose key
+    void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, const Vector& 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 (int k = 0; k < Z.cols(); k++) {
+        graph.push_back(
+            make_shared<GenericProjectionFactor<Pose3, Point3> >
+            (Point2(Z(0, k), Z(1, k)), model, i, J(k), K));
+      }
+    }
+
     /// calculate the errors of all projection factors in a graph
     Matrix reprojectionErrors(const NonlinearFactorGraph& graph, const Values& values) {
       // first count