added cameraConstraint and corresponding unit test based on same 2 camera/4 landmark example, but now there are constraints on the two cameras and none of the landmarks.

cpp/VSLAMGraph.cpp

@@ -102,6 +102,23 @@ void VSLAMGraph::addLandmarkConstraint(int j, const gtsam::Point3& p) {
   push_back(factor);
 }
 
+/* ************************************************************************* */
+bool compareCamera(const std::string& key,
+					const VSLAMConfig& feasible,
+					const VSLAMConfig& input) {
+	int j = atoi(key.substr(1, key.size() - 1).c_str());
+	return feasible.cameraPose(j).equals(input.cameraPose(j));
+}
+
+/* ************************************************************************* */
+void VSLAMGraph::addCameraConstraint(int j, const gtsam::Pose3& p) {
+  typedef NonlinearEquality<VSLAMConfig> NLE;
+  VSLAMConfig feasible;
+  feasible.addCameraPose(j,p);
+  boost::shared_ptr<NLE> factor(new NLE(symbol('x',j), feasible, 6, compareCamera));
+  push_back(factor);
+}
+
 /* ************************************************************************* */
 
 } // namespace gtsam

cpp/VSLAMGraph.h

@@ -64,6 +64,13 @@ public:
    */
   void addLandmarkConstraint(int j, const Point3& p = Point3());
 
+  /**
+   *  Add a constraint on a camera (for now, *must* be satisfied in any Config)
+   *  @param j index of camera
+   *  @param p to which pose to constrain it to
+   */
+  void addCameraConstraint(int j, const Pose3& p = Pose3());
+
 private:
 	/** Serialization function */
 	friend class boost::serialization::access;

cpp/testVSLAMGraph.cpp

@@ -59,12 +59,6 @@ VSLAMGraph testGraph() {
   g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z22, sigma, 2, 2, sK)));
   g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z23, sigma, 2, 3, sK)));
   g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z24, sigma, 2, 4, sK)));
-
-	// add 3 landmark constraints
-  g.addLandmarkConstraint(1, landmark1);
-  g.addLandmarkConstraint(2, landmark2);
-  g.addLandmarkConstraint(3, landmark3);
-
   return g;
 }
 
@@ -73,6 +67,10 @@ TEST( VSLAMGraph, optimizeLM)
 {
   // build a graph
   VSLAMGraph graph = testGraph();
+	// add 3 landmark constraints
+  graph.addLandmarkConstraint(1, landmark1);
+  graph.addLandmarkConstraint(2, landmark2);
+  graph.addLandmarkConstraint(3, landmark3);
 
   // Create an initial configuration corresponding to the ground truth
   boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig);
@@ -107,6 +105,50 @@ TEST( VSLAMGraph, optimizeLM)
   CHECK(assert_equal(*initialEstimate,*(afterOneIteration.config())));
 }
 
+
+/* ************************************************************************* */
+TEST( VSLAMGraph, optimizeLM2)
+{
+  // build a graph
+  VSLAMGraph graph = testGraph();
+	// add 2 camera constraints
+  graph.addCameraConstraint(1, camera1);
+  graph.addCameraConstraint(2, camera2);
+
+  // Create an initial configuration corresponding to the ground truth
+  boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig);
+  initialEstimate->addCameraPose(1, camera1);
+  initialEstimate->addCameraPose(2, camera2);
+  initialEstimate->addLandmarkPoint(1, landmark1);
+  initialEstimate->addLandmarkPoint(2, landmark2);
+  initialEstimate->addLandmarkPoint(3, landmark3);
+  initialEstimate->addLandmarkPoint(4, landmark4);
+
+  // Create an ordering of the variables
+  list<string> keys;
+
+  keys.push_back("l1");
+  keys.push_back("l2");
+  keys.push_back("l3");
+  keys.push_back("l4");
+  keys.push_back("x1");
+  keys.push_back("x2");
+  Ordering ordering(keys);
+
+  // Create an optimizer and check its error
+  // We expect the initial to be zero because config is the ground truth
+  Optimizer optimizer(graph, ordering, initialEstimate, 1e-5);
+  DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
+
+  // Iterate once, and the config should not have changed because we started
+  // with the ground truth
+  Optimizer afterOneIteration = optimizer.iterate();
+  DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
+
+  // check if correct
+  CHECK(assert_equal(*initialEstimate,*(afterOneIteration.config())));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; TestRegistry::runAllTests(tr); return 0;}
 /* ************************************************************************* */