Moved optimization tests to testPose2Graph

cpp/Pose2Config.cpp

@@ -1,6 +1,6 @@
 /**
- * @file    VectorConfig.cpp
+ * @file    Pose2Config.cpp
- * @brief   Pose2Graph Configuration
+ * @brief   Configuration of 2D poses
  * @author  Frank Dellaert
  */
 

cpp/Pose2Config.h

@@ -1,5 +1,5 @@
 /**
- * @file    PoseConfig.cpp
+ * @file    Pose2Config.cpp
  * @brief   Configuration of 2D poses
  * @author  Frank Dellaert
  */

cpp/testPose2Factor.cpp

@@ -4,20 +4,9 @@
  *  @authors Frank Dellaert, Viorela Ila
  **/
 
-#include <iostream>
-#include <boost/shared_ptr.hpp>
-#include <boost/assign/std/list.hpp>
-using namespace boost::assign;
-
 #include <CppUnitLite/TestHarness.h>
-
-#include "Vector.h"
 #include "numericalDerivative.h"
-#include "NonlinearOptimizer-inl.h"
-#include "NonlinearEquality.h"
 #include "Pose2Factor.h"
-#include "Ordering.h"
-#include "Pose2Graph.h"
 
 using namespace std;
 using namespace gtsam;
@@ -142,95 +131,6 @@ TEST( Pose2Factor, linearize )
 	CHECK(assert_equal(expectedH2,numericalH2));
 }
 
-/* ************************************************************************* */
-bool poseCompare(const std::string& key,
-    const gtsam::Pose2Config& feasible,
-    const gtsam::Pose2Config& input) {
-  return feasible.get(key).equals(input.get(key));
-}
-
-/* ************************************************************************* */
-TEST(Pose2Factor, optimize) {
-
-	// create a Pose graph with one equality constraint and one measurement
-  Pose2Graph fg;
-  Pose2Config feasible;
-  feasible.insert("p0", Pose2(0,0,0));
-  fg.push_back(Pose2Graph::sharedFactor(
-      new NonlinearEquality<Pose2Config>("p0", feasible, dim(Pose2()), poseCompare)));
-  fg.add("p0", "p1", Pose2(1,2,M_PI_2), covariance);
-
-  // Create initial config
-  boost::shared_ptr<Pose2Config> initial(new Pose2Config());
-  initial->insert("p0", Pose2(0,0,0));
-  initial->insert("p1", Pose2(0,0,0));
-
-  // Choose an ordering and optimize
-  Ordering ordering;
-  ordering += "p0","p1";
-  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);
-
-  // Check with expected config
-  Pose2Config expected;
-  expected.insert("p0", Pose2(0,0,0));
-  expected.insert("p1", Pose2(1,2,M_PI_2));
-  CHECK(assert_equal(expected, *optimizer.config()));
-
-}
-
-/* ************************************************************************* */
-// 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;

cpp/testPose2Graph.cpp

@@ -4,11 +4,17 @@
  **/
 
 #include <iostream>
+#include <boost/shared_ptr.hpp>
+#include <boost/assign/std/list.hpp>
+using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
+
+#include "NonlinearOptimizer-inl.h"
+#include "NonlinearEquality.h"
+#include "Ordering.h"
+#include "Pose2Config.h"
 #include "Pose2Graph.h"
-#include "NonlinearFactorGraph-inl.h"
-#include "GaussianFactorGraph.h"
 
 using namespace std;
 using namespace gtsam;
@@ -28,7 +34,7 @@ TEST( Pose2Graph, constructor )
 	Pose2 measured(2,2,M_PI_2);
 	Pose2Factor constraint("x1","x2",measured, covariance);
 	Pose2Graph graph;
-	graph.push_back(Pose2Factor::shared_ptr(new Pose2Factor("x1","x2",measured, covariance)));
+	graph.add("x1","x2",measured, covariance);
 	// get the size of the graph
 	size_t actual = graph.size();
 	// verify
@@ -44,7 +50,7 @@ TEST( Pose2Graph, linerization )
 	Pose2 measured(2,2,M_PI_2);
 	Pose2Factor constraint("x1","x2",measured, covariance);
 	Pose2Graph graph;
-	graph.push_back(Pose2Factor::shared_ptr(new Pose2Factor("x1","x2",measured, covariance)));
+	graph.add("x1","x2",measured, covariance);
 
 	// Choose a linearization point
 	Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
@@ -75,6 +81,95 @@ TEST( Pose2Graph, linerization )
 	CHECK(assert_equal(lfg_expected, lfg_linearized));
 }
 
+/* ************************************************************************* */
+bool poseCompare(const std::string& key,
+    const gtsam::Pose2Config& feasible,
+    const gtsam::Pose2Config& input) {
+  return feasible.get(key).equals(input.get(key));
+}
+
+/* ************************************************************************* */
+TEST(Pose2Graph, optimize) {
+
+	// create a Pose graph with one equality constraint and one measurement
+  Pose2Graph fg;
+  Pose2Config feasible;
+  feasible.insert("p0", Pose2(0,0,0));
+  fg.push_back(Pose2Graph::sharedFactor(
+      new NonlinearEquality<Pose2Config>("p0", feasible, dim(Pose2()), poseCompare)));
+  fg.add("p0", "p1", Pose2(1,2,M_PI_2), covariance);
+
+  // Create initial config
+  boost::shared_ptr<Pose2Config> initial(new Pose2Config());
+  initial->insert("p0", Pose2(0,0,0));
+  initial->insert("p1", Pose2(0,0,0));
+
+  // Choose an ordering and optimize
+  Ordering ordering;
+  ordering += "p0","p1";
+  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);
+
+  // Check with expected config
+  Pose2Config expected;
+  expected.insert("p0", Pose2(0,0,0));
+  expected.insert("p1", Pose2(1,2,M_PI_2));
+  CHECK(assert_equal(expected, *optimizer.config()));
+
+}
+
+/* ************************************************************************* */
+// test optimization with 6 poses arranged in a hexagon and a loop closure
+TEST(Pose2Graph, 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;