diff --git a/cpp/Pose2Config.cpp b/cpp/Pose2Config.cpp index 38f7ce839..597f048da 100644 --- a/cpp/Pose2Config.cpp +++ b/cpp/Pose2Config.cpp @@ -1,6 +1,6 @@ /** - * @file VectorConfig.cpp - * @brief Pose2Graph Configuration + * @file Pose2Config.cpp + * @brief Configuration of 2D poses * @author Frank Dellaert */ diff --git a/cpp/Pose2Config.h b/cpp/Pose2Config.h index 2c73f1b57..94c6f6811 100644 --- a/cpp/Pose2Config.h +++ b/cpp/Pose2Config.h @@ -1,5 +1,5 @@ /** - * @file PoseConfig.cpp + * @file Pose2Config.cpp * @brief Configuration of 2D poses * @author Frank Dellaert */ diff --git a/cpp/testPose2Factor.cpp b/cpp/testPose2Factor.cpp index a5ac19f2f..6ad7b18db 100644 --- a/cpp/testPose2Factor.cpp +++ b/cpp/testPose2Factor.cpp @@ -4,20 +4,9 @@ * @authors Frank Dellaert, Viorela Ila **/ -#include -#include -#include -using namespace boost::assign; - #include - -#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("p0", feasible, dim(Pose2()), poseCompare))); - fg.add("p0", "p1", Pose2(1,2,M_PI_2), covariance); - - // Create initial config - boost::shared_ptr 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 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("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 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 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; diff --git a/cpp/testPose2Graph.cpp b/cpp/testPose2Graph.cpp index ccb1ff2a9..99a870ff0 100644 --- a/cpp/testPose2Graph.cpp +++ b/cpp/testPose2Graph.cpp @@ -4,11 +4,17 @@ **/ #include +#include +#include +using namespace boost::assign; #include + +#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("p0", feasible, dim(Pose2()), poseCompare))); + fg.add("p0", "p1", Pose2(1,2,M_PI_2), covariance); + + // Create initial config + boost::shared_ptr 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 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("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 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 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;