Loop closure tested

cpp/testPose2Factor.cpp

@@ -16,6 +16,7 @@ using namespace boost::assign;
 #include "NonlinearOptimizer-inl.h"
 #include "NonlinearEquality.h"
 #include "Pose2Factor.h"
+#include "Ordering.h"
 #include "Pose2Graph.h"
 
 using namespace std;
@@ -157,12 +158,10 @@ TEST(Pose2Factor, optimize) {
   feasible.insert("p0", Pose2(0,0,0));
   fg.push_back(Pose2Graph::sharedFactor(
       new NonlinearEquality<Pose2Config>("p0", feasible, dim(Pose2()), poseCompare)));
-  fg.push_back(Pose2Graph::sharedFactor(
+  fg.add("p0", "p1", Pose2(1,2,M_PI_2), covariance);
-      new Pose2Factor("p0", "p1", Pose2(1,2,M_PI_2), covariance)));
 
   // Create initial config
-  boost::shared_ptr<Pose2Config> initial =
+  boost::shared_ptr<Pose2Config> initial(new Pose2Config());
-  		boost::shared_ptr<Pose2Config>(new Pose2Config());
   initial->insert("p0", Pose2(0,0,0));
   initial->insert("p1", Pose2(0,0,0));
 
@@ -183,6 +182,55 @@ TEST(Pose2Factor, optimize) {
 
 }
 
+/* ************************************************************************* */
+// test optimization with 6 poses arranged in a hexagon and a loop closure
+TEST(Pose2Factor, optimizeCircle) {
+
+	// Create a hexagon of poses
+	Pose2Config hexagon = pose2Circle(6,1.0,'p');
+  Pose2 p0 = hexagon["p0"], p1 = hexagon["p1"];
+
+	// create a Pose graph with one equality constraint and one measurement
+  Pose2Graph fg;
+  Pose2Config feasible;
+  feasible.insert("p0", p0);
+  fg.push_back(Pose2Graph::sharedFactor(
+      new NonlinearEquality<Pose2Config>("p0", feasible, dim(Pose2()), poseCompare)));
+  Pose2 delta = between(p0,p1);
+  fg.add("p0", "p1", delta, covariance);
+  fg.add("p1", "p2", delta, covariance);
+  fg.add("p2", "p3", delta, covariance);
+  fg.add("p3", "p4", delta, covariance);
+  fg.add("p4", "p5", delta, covariance);
+  fg.add("p5", "p0", delta, covariance);
+
+  // Create initial config
+  boost::shared_ptr<Pose2Config> initial(new Pose2Config());
+  initial->insert("p0", p0);
+  initial->insert("p1", expmap(hexagon["p1"],Vector_(3,-0.1, 0.1,-0.1)));
+  initial->insert("p2", expmap(hexagon["p2"],Vector_(3, 0.1,-0.1, 0.1)));
+  initial->insert("p3", expmap(hexagon["p3"],Vector_(3,-0.1, 0.1,-0.1)));
+  initial->insert("p4", expmap(hexagon["p4"],Vector_(3, 0.1,-0.1, 0.1)));
+  initial->insert("p5", expmap(hexagon["p5"],Vector_(3,-0.1, 0.1,-0.1)));
+
+  // Choose an ordering and optimize
+  Ordering ordering;
+  ordering += "p0","p1","p2","p3","p4","p5";
+  typedef NonlinearOptimizer<Pose2Graph, Pose2Config> Optimizer;
+  Optimizer optimizer(fg, ordering, initial);
+//  Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
+  Optimizer::verbosityLevel verbosity = Optimizer::ERROR;
+  optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity);
+
+  Pose2Config actual = *optimizer.config();
+
+  // Check with ground truth
+  CHECK(assert_equal(hexagon, actual));
+
+  // Check loop closure
+  CHECK(assert_equal(delta,between(actual["p5"],actual["p0"])));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;