More efficient operations from within MATLAB

gtsam.h

@@ -1272,6 +1272,8 @@ class Values {
   void insertPoint(size_t key, const gtsam::Point3& pose);
   void updatePoint(size_t key, const gtsam::Point3& pose);
   gtsam::Point3 point(size_t j);
+  void insertBackprojections(const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth);
+  void perturbPoints(double sigma, size_t seed);
   Matrix points() const;
 };
 
@@ -1312,7 +1314,7 @@ class Graph {
     void addMeasurement(const gtsam::Point2& measured,
         const gtsam::noiseModel::Base* model, size_t poseKey, size_t pointKey,
         const gtsam::Cal3_S2* K);
-    void addMeasurements(size_t i, const gtsam::KeyVector& J, Matrix Z,
+    void addMeasurements(size_t i, Vector 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,

gtsam/slam/tests/testVisualSLAM.cpp

@@ -42,14 +42,14 @@ static Point3 landmark2(-1.0, 1.0, 0.0);
 static Point3 landmark3( 1.0, 1.0, 0.0);
 static Point3 landmark4( 1.0,-1.0, 0.0);
 
-static Pose3 camera1(Matrix_(3,3,
+static Pose3 pose1(Matrix_(3,3,
 		       1., 0., 0.,
 		       0.,-1., 0.,
 		       0., 0.,-1.
 		       ),
 	      Point3(0,0,6.25));
 
-static Pose3 camera2(Matrix_(3,3,
+static Pose3 pose2(Matrix_(3,3,
 		       1., 0., 0.,
 		       0.,-1., 0.,
 		       0., 0.,-1.
@@ -92,8 +92,8 @@ TEST( VisualSLAM, optimizeLM)
 
   // Create an initial values structure corresponding to the ground truth
   Values initialEstimate;
-  initialEstimate.insert(X(1), camera1);
+  initialEstimate.insert(X(1), pose1);
-  initialEstimate.insert(X(2), camera2);
+  initialEstimate.insert(X(2), pose2);
   initialEstimate.insert(L(1), landmark1);
   initialEstimate.insert(L(2), landmark2);
   initialEstimate.insert(L(3), landmark3);
@@ -124,13 +124,13 @@ TEST( VisualSLAM, optimizeLM2)
   // build a graph
   visualSLAM::Graph graph(testGraph());
 	// add 2 camera constraints
-  graph.addPoseConstraint(X(1), camera1);
+  graph.addPoseConstraint(X(1), pose1);
-  graph.addPoseConstraint(X(2), camera2);
+  graph.addPoseConstraint(X(2), pose2);
 
   // Create an initial values structure corresponding to the ground truth
   Values initialEstimate;
-  initialEstimate.insert(X(1), camera1);
+  initialEstimate.insert(X(1), pose1);
-  initialEstimate.insert(X(2), camera2);
+  initialEstimate.insert(X(2), pose2);
   initialEstimate.insert(L(1), landmark1);
   initialEstimate.insert(L(2), landmark2);
   initialEstimate.insert(L(3), landmark3);
@@ -160,13 +160,13 @@ TEST( VisualSLAM, LMoptimizer)
   // build a graph
   visualSLAM::Graph graph(testGraph());
   // add 2 camera constraints
-  graph.addPoseConstraint(X(1), camera1);
+  graph.addPoseConstraint(X(1), pose1);
-  graph.addPoseConstraint(X(2), camera2);
+  graph.addPoseConstraint(X(2), pose2);
 
   // Create an initial values structure corresponding to the ground truth
   Values initialEstimate;
-  initialEstimate.insert(X(1), camera1);
+  initialEstimate.insert(X(1), pose1);
-  initialEstimate.insert(X(2), camera2);
+  initialEstimate.insert(X(2), pose2);
   initialEstimate.insert(L(1), landmark1);
   initialEstimate.insert(L(2), landmark2);
   initialEstimate.insert(L(3), landmark3);
@@ -193,13 +193,13 @@ TEST( VisualSLAM, CHECK_ORDERING)
   // build a graph
   visualSLAM::Graph graph = testGraph();
   // add 2 camera constraints
-  graph.addPoseConstraint(X(1), camera1);
+  graph.addPoseConstraint(X(1), pose1);
-  graph.addPoseConstraint(X(2), camera2);
+  graph.addPoseConstraint(X(2), pose2);
 
   // Create an initial values structure corresponding to the ground truth
   Values initialEstimate;
-  initialEstimate.insert(X(1), camera1);
+  initialEstimate.insert(X(1), pose1);
-  initialEstimate.insert(X(2), camera2);
+  initialEstimate.insert(X(2), pose2);
   initialEstimate.insert(L(1), landmark1);
   initialEstimate.insert(L(2), landmark2);
   initialEstimate.insert(L(3), landmark3);
@@ -274,8 +274,8 @@ TEST( VisualSLAM, keys_and_view )
 {
 	// create config
 	visualSLAM::Values c;
-	c.insert(X(1), camera1);
+	c.insert(X(1), pose1);
-	c.insert(X(2), camera2);
+	c.insert(X(2), pose2);
 	c.insert(L(2), landmark2);
 	LONGS_EQUAL(2,c.nrPoses());
 	LONGS_EQUAL(1,c.nrPoints());
@@ -299,6 +299,42 @@ TEST( VisualSLAM, keys_and_view )
 	}
 }
 
+/* ************************************************************************* */
+TEST( VisualSLAM, addMeasurements )
+{
+  // create config
+  visualSLAM::Graph g;
+  Vector J = Vector_(3,1.0,2.0,3.0);
+  Matrix Z = Matrix_(2,3, -1.0,0.0,1.0, -1.0,0.0,1.0);
+  shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0));
+  g.addMeasurements(0,J,Z,sigma,sK);
+  EXPECT_LONGS_EQUAL(3,g.size());
+}
+
+/* ************************************************************************* */
+TEST( VisualSLAM, insertBackProjections )
+{
+  // create config
+  visualSLAM::Values c;
+  SimpleCamera camera(pose1);
+  Vector J = Vector_(3,1.0,2.0,3.0);
+  Matrix Z = Matrix_(2,3, -1.0,0.0,1.0, -1.0,0.0,1.0);
+  c.insertBackprojections(camera,J,Z,1.0);
+  EXPECT_LONGS_EQUAL(3,c.nrPoints());
+}
+
+/* ************************************************************************* */
+TEST( VisualSLAM, perturbPoints )
+{
+  visualSLAM::Values c1,c2;
+  c1.insert(L(2), landmark2);
+  c1.perturbPoints(0.01,42u);
+  CHECK(assert_equal(Point3(-0.986984, 0.999534, -0.0147962),c1.point(L(2)),1e-6));
+  c2.insert(L(2), landmark2);
+  c2.perturbPoints(0.01,42u);
+  CHECK(assert_equal(Point3(-0.986984, 0.999534, -0.0147962),c2.point(L(2)),1e-6));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/slam/visualSLAM.cpp

@@ -18,12 +18,36 @@
 #include <gtsam/slam/visualSLAM.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/linear/Sampler.h>
 #include <boost/make_shared.hpp>
 
-using boost::make_shared;
-
 namespace visualSLAM {
 
+  using boost::make_shared;
+
+  /* ************************************************************************* */
+  void Values::insertBackprojections(const SimpleCamera& camera,
+      const Vector& J, const Matrix& Z, double depth) {
+    if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K");
+    if (Z.cols() != J.size()) throw std::invalid_argument(
+          "insertBackProjections: J and Z must have same number of entries");
+    for(size_t k=0;k<Z.cols();k++) {
+      Point2 p(Z(0,k),Z(1,k));
+      Point3 P = camera.backproject(p, depth);
+      insertPoint(J(k), P);
+    }
+  }
+
+  /* ************************************************************************* */
+  void Values::perturbPoints(double sigma, int32_t seed) {
+    ConstFiltered<Point3> points = allPoints();
+    noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma);
+    Sampler sampler(model, seed);
+    BOOST_FOREACH(const ConstFiltered<Point3>::KeyValuePair& keyValue, points) {
+      update(keyValue.key, keyValue.value.retract(sampler.sample()));
+    }
+  }
+
   /* ************************************************************************* */
   Matrix Values::points() const {
     size_t j=0;
@@ -53,14 +77,13 @@ namespace visualSLAM {
   }
 
   /* ************************************************************************* */
-  void Graph::addMeasurements(Key i, const KeyVector& J, const Matrix& Z,
+  void Graph::addMeasurements(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.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K");
-    if (Z.cols() != J.size())
+    if (Z.cols() != J.size()) throw std::invalid_argument(
-      throw std::invalid_argument(
           "addMeasurements: J and Z must have same number of entries");
-    for(size_t k=0;k<Z.cols();k++)
+    for (size_t k = 0; k < Z.cols(); k++)
-      addMeasurement(Point2(Z(0,k),Z(1,k)),model,i, J[k], K);
+      addMeasurement(Point2(Z(0, k), Z(1, k)), model, i, J(k), K);
   }
 
   /* ************************************************************************* */

gtsam/slam/visualSLAM.h

@@ -68,7 +68,15 @@ namespace visualSLAM {
     /// get a point
     Point3 point(Key j) const { return at<Point3>(j); }
 
-    Matrix points() const; ///< get all point coordinates in a matrix
+    /// insert a number of initial point values by backprojecting
+    void insertBackprojections(const SimpleCamera& c, const Vector& J,
+        const Matrix& Z, double depth);
+
+    /// perturb all points using normally distributed noise
+    void perturbPoints(double sigma, int32_t seed = 42u);
+
+    /// get all point coordinates in a matrix
+    Matrix points() const;
 
   };
 
@@ -137,7 +145,7 @@ namespace visualSLAM {
      *  @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,
+    void addMeasurements(Key i, const Vector& J, const Matrix& Z,
         const SharedNoiseModel& model, const shared_ptrK K);
 
     /**