From adc8921f3bc430703755498871fb3ef97f79c960 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Thu, 21 Oct 2010 21:38:38 +0000 Subject: [PATCH] separate the examples into easy/advanced style, remove shared pointer in easy examples, unify the convention --- examples/Makefile.am | 12 +- ...Example.cpp => PlanarSLAMExample_easy.cpp} | 6 +- ...p => PlanarSLAMSelfContained_advanced.cpp} | 18 +-- ...mple.cpp => Pose2SLAMExample_advanced.cpp} | 37 ++---- examples/Pose2SLAMExample_easy.cpp | 59 ++++++++++ examples/Pose2SLAMwSPCG.cpp | 111 ------------------ examples/Pose2SLAMwSPCG_advanced.cpp | 98 ++++++++++++++++ examples/Pose2SLAMwSPCG_easy.cpp | 79 +++++++++++++ examples/SimpleRotation.cpp | 16 ++- 9 files changed, 275 insertions(+), 161 deletions(-) rename examples/{PlanarSLAMExample.cpp => PlanarSLAMExample_easy.cpp} (96%) rename examples/{PlanarSLAMSelfContained.cpp => PlanarSLAMSelfContained_advanced.cpp} (92%) rename examples/{Pose2SLAMExample.cpp => Pose2SLAMExample_advanced.cpp} (68%) create mode 100644 examples/Pose2SLAMExample_easy.cpp delete mode 100644 examples/Pose2SLAMwSPCG.cpp create mode 100644 examples/Pose2SLAMwSPCG_advanced.cpp create mode 100644 examples/Pose2SLAMwSPCG_easy.cpp diff --git a/examples/Makefile.am b/examples/Makefile.am index 41ad7322b..f623abbb5 100644 --- a/examples/Makefile.am +++ b/examples/Makefile.am @@ -11,11 +11,13 @@ sources = check_PROGRAMS = # Examples -noinst_PROGRAMS = SimpleRotation # Optimizes a single nonlinear rotation variable -noinst_PROGRAMS += PlanarSLAMExample # Solves SLAM example from tutorial by using planarSLAM -noinst_PROGRAMS += Pose2SLAMExample # Solves SLAM example from tutorial by using planarSLAM -noinst_PROGRAMS += PlanarSLAMSelfContained # Solves SLAM example from tutorial with all typedefs in the script -noinst_PROGRAMS += Pose2SLAMwSPCG # Solves a simple SLAM example with SPCG solver +noinst_PROGRAMS = SimpleRotation # Optimizes a single nonlinear rotation variable +noinst_PROGRAMS += PlanarSLAMExample_easy # Solves SLAM example from tutorial by using planarSLAM +noinst_PROGRAMS += PlanarSLAMSelfContained_advanced # Solves SLAM example from tutorial with all typedefs in the script +noinst_PROGRAMS += Pose2SLAMExample_easy # Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface +noinst_PROGRAMS += Pose2SLAMExample_advanced # Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface +noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver +noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver SUBDIRS = vSLAMexample # does not compile.... #---------------------------------------------------------------------------------------------------- # rules to build local programs diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample_easy.cpp similarity index 96% rename from examples/PlanarSLAMExample.cpp rename to examples/PlanarSLAMExample_easy.cpp index 09e83e831..0b1d7db36 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample_easy.cpp @@ -74,7 +74,7 @@ int main(int argc, char** argv) { graph.addBearingRange(x2, l1, bearing21, range21, meas_model); graph.addBearingRange(x3, l2, bearing32, range32, meas_model); - graph.print("Full Graph"); + graph.print("full graph"); // initialize to noisy points Values initialEstimate; @@ -84,11 +84,11 @@ int main(int argc, char** argv) { initialEstimate.insert(l1, Point2(1.8, 2.1)); initialEstimate.insert(l2, Point2(4.1, 1.8)); - initialEstimate.print("Initial Estimate"); + initialEstimate.print("initial estimate"); // optimize using Levenberg-Marquardt optimization with an ordering from colamd Values result = optimize(graph, initialEstimate); - result.print("Final Result"); + result.print("final result"); return 0; } diff --git a/examples/PlanarSLAMSelfContained.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp similarity index 92% rename from examples/PlanarSLAMSelfContained.cpp rename to examples/PlanarSLAMSelfContained_advanced.cpp index d74661281..a37467f1f 100644 --- a/examples/PlanarSLAMSelfContained.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -110,19 +110,19 @@ int main(int argc, char** argv) { graph.print("Full Graph"); // initialize to noisy points - Values initialEstimate; - initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); - initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); - initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); - initialEstimate.insert(l1, Point2(1.8, 2.1)); - initialEstimate.insert(l2, Point2(4.1, 1.8)); + Values initial; + initial.insert(x1, Pose2(0.5, 0.0, 0.2)); + initial.insert(x2, Pose2(2.3, 0.1,-0.2)); + initial.insert(x3, Pose2(4.1, 0.1, 0.1)); + initial.insert(l1, Point2(1.8, 2.1)); + initial.insert(l2, Point2(4.1, 1.8)); - initialEstimate.print("Initial Estimate"); + initial.print("initial estimate"); // optimize using Levenburg-Marquadt optimization with an ordering from colamd - Optimizer::shared_values result = Optimizer::optimizeLM(graph, initialEstimate); + Optimizer::shared_values result = Optimizer::optimizeLM(graph, initial); - result->print("Final Result"); + result->print("final result"); return 0; } diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample_advanced.cpp similarity index 68% rename from examples/Pose2SLAMExample.cpp rename to examples/Pose2SLAMExample_advanced.cpp index c44e6077b..50799883d 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -34,7 +34,6 @@ int main(int argc, char** argv) { Key x1(1), x2(2), x3(3); /* 1. create graph container and add factors to it */ - //Graph graph; shared_ptr graph(new Graph); /* 2.a add prior */ @@ -51,39 +50,29 @@ int main(int argc, char** argv) { Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) graph->addConstraint(x1, x2, odom_measurement, odom_model); graph->addConstraint(x2, x3, odom_measurement, odom_model); - - graph->print("Full Graph"); + graph->print("full graph"); /* 3. Create the data structure to hold the initial estinmate to the solution * initialize to noisy points */ - shared_ptr initialEstimate(new Values); - initialEstimate->insert(x1, Pose2(0.5, 0.0, 0.2)); - initialEstimate->insert(x2, Pose2(2.3, 0.1,-0.2)); - initialEstimate->insert(x3, Pose2(4.1, 0.1, 0.1)); + shared_ptr initial(new Values); + initial->insert(x1, Pose2(0.5, 0.0, 0.2)); + initial->insert(x2, Pose2(2.3, 0.1,-0.2)); + initial->insert(x3, Pose2(4.1, 0.1, 0.1)); + initial->print("initial estimate"); - initialEstimate->print("Initial Estimate"); - - /* There are several ways to solve the graph. */ - - /* 4.1 Single Step: - * optimize using Levenberg-Marquardt optimization with an ordering from colamd */ - Values result = optimize(*graph, *initialEstimate); - result.print("Final Result"); /* 4.2.1 Alternatively, you can go through the process step by step * Choose an ordering */ - Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initialEstimate); + Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initial); - /* 4.2.2 set up solver and optimize */ + /* 4.2.2 set up solver and optimize */ Optimizer::shared_solver solver(new Optimizer::solver(ordering)); + Optimizer optimizer(graph, initial, solver); + Optimizer::Parameters::verbosityLevel verbosity = pose2SLAM::Optimizer::Parameters::SILENT; + Optimizer optimizer_result = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity); - Optimizer optimizer(graph, initialEstimate, solver); - Optimizer::Parameters::verbosityLevel verbosity = pose2SLAM::Optimizer::Parameters::SILENT; - Optimizer optimizer0 = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity); - - Values result2 = *optimizer0.config(); - result2.print("Final Result 2"); - + Values result = *optimizer_result.config(); + result.print("final result"); return 0; } diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp new file mode 100644 index 000000000..35a2368df --- /dev/null +++ b/examples/Pose2SLAMExample_easy.cpp @@ -0,0 +1,59 @@ +/* + * Pose2SLAMExample_easy.cpp + * + * Created on: Oct 21, 2010 + * Author: ydjian + */ + +#include +#include +#include + +// pull in the Pose2 SLAM domain with all typedefs and helper functions defined +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::pose2SLAM; + +int main(int argc, char** argv) { + // create keys for robot positions + Key x1(1), x2(2), x3(3); + + /* 1. create graph container and add factors to it */ + //Graph graph; + Graph graph ; + + /* 2.a add prior */ + // gaussian for prior + SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin + graph.addPrior(x1, prior_measurement, prior_model); // add directly to graph + + /* 2.b add odometry */ + // general noisemodel for odometry + SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + + /* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/ + Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) + graph.addConstraint(x1, x2, odom_measurement, odom_model); + graph.addConstraint(x2, x3, odom_measurement, odom_model); + graph.print("full graph"); + + /* 3. Create the data structure to hold the initial estinmate to the solution + * initialize to noisy points */ + Values initial; + initial.insert(x1, Pose2(0.5, 0.0, 0.2)); + initial.insert(x2, Pose2(2.3, 0.1,-0.2)); + initial.insert(x3, Pose2(4.1, 0.1, 0.1)); + initial.print("initial estimate"); + + /* 4 Single Step Optimization + * optimize using Levenberg-Marquardt optimization with an ordering from colamd */ + Values result = optimize(graph, initial); + result.print("final result"); + + + return 0; +} diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp deleted file mode 100644 index 7ad7d38a2..000000000 --- a/examples/Pose2SLAMwSPCG.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * Pose2SLAMwSPCG.cpp - * - * Created on: Oct 18, 2010 - * Author: Yong-Dian Jian - * - * Demonstrate how to use SPCG solver to solve Pose2 SLAM problem - */ - -#include - -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace pose2SLAM; - -typedef boost::shared_ptr sharedGraph; -typedef boost::shared_ptr sharedValue; - - -typedef NonlinearOptimizer > SPCGOptimizer; - -sharedGraph G; -Values initial; -Values result; - -void generateData() ; - -/* ************************************************************************* */ -int main(void) { - - const bool bNewInterface = true ; - const SharedGaussian sigma(noiseModel::Unit::Create(0.1)); - - // generate measurement and initial configuration - generateData() ; - - if ( bNewInterface == true ) { - result = optimizeSPCG(*G, initial, NonlinearOptimizationParameters()); - } - else { - cout << "Initialize .... " << endl; - SPCGOptimizer::shared_solver solver(new SPCGOptimizer::solver(*G, initial)) ; - sharedValue SV(new Values(initial)) ; - SPCGOptimizer optimizer(G, SV, solver) ; - cout << "before optimization, sum of error is " << optimizer.error() << endl; - cout << "Optimize .... " << endl; - NonlinearOptimizationParameters parameter; - SPCGOptimizer optimizer2 = optimizer.levenbergMarquardt(parameter); - cout << "after optimization, sum of error is " << optimizer2.error() << endl; - result = *optimizer2.config() ; - } - - result.print("result") ; - - return 0 ; -} - -void generateData() { - - // noise model - const SharedGaussian sigma(noiseModel::Unit::Create(0.1)); - - Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9); - - // create a 3 by 3 grid - // x3 x6 x9 - // x2 x5 x8 - // x1 x4 x7 - G = sharedGraph(new Graph) ; - G->addConstraint(x1,x2,Pose2(0,2,0),sigma) ; - G->addConstraint(x2,x3,Pose2(0,2,0),sigma) ; - G->addConstraint(x4,x5,Pose2(0,2,0),sigma) ; - G->addConstraint(x5,x6,Pose2(0,2,0),sigma) ; - G->addConstraint(x7,x8,Pose2(0,2,0),sigma) ; - G->addConstraint(x8,x9,Pose2(0,2,0),sigma) ; - G->addConstraint(x1,x4,Pose2(2,0,0),sigma) ; - G->addConstraint(x4,x7,Pose2(2,0,0),sigma) ; - G->addConstraint(x2,x5,Pose2(2,0,0),sigma) ; - G->addConstraint(x5,x8,Pose2(2,0,0),sigma) ; - G->addConstraint(x3,x6,Pose2(2,0,0),sigma) ; - G->addConstraint(x6,x9,Pose2(2,0,0),sigma) ; - G->addPrior(x1, Pose2(0,0,0), sigma) ; - - initial.insert(x1, Pose2( 0, 0, 0)); - initial.insert(x2, Pose2( 0, 2.1, 0.01)); - initial.insert(x3, Pose2( 0, 3.9,-0.01)); - initial.insert(x4, Pose2(2.1,-0.1, 0)); - initial.insert(x5, Pose2(1.9, 2.1, 0.02)); - initial.insert(x6, Pose2(2.0, 3.9,-0.02)); - initial.insert(x7, Pose2(4.0, 0.1, 0.03 )); - initial.insert(x8, Pose2(3.9, 2.1, 0.01)); - initial.insert(x9, Pose2(4.1, 3.9,-0.01)); -} diff --git a/examples/Pose2SLAMwSPCG_advanced.cpp b/examples/Pose2SLAMwSPCG_advanced.cpp new file mode 100644 index 000000000..f35bd6097 --- /dev/null +++ b/examples/Pose2SLAMwSPCG_advanced.cpp @@ -0,0 +1,98 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * Solve a simple 3 by 3 grid of Pose2 SLAM problem by using advanced SPCG interface + */ + +#include + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace pose2SLAM; + +typedef boost::shared_ptr sharedGraph; +typedef boost::shared_ptr sharedValue; +typedef NonlinearOptimizer > SPCGOptimizer; + +sharedGraph graph; +sharedValue initial; +Values result; + +void generateData() ; + +/* ************************************************************************* */ +int main(void) { + + const SharedGaussian sigma(noiseModel::Unit::Create(0.1)); + generateData() ; + + graph->print("full graph") ; + initial->print("initial estimate") ; + + SPCGOptimizer::shared_solver solver(new SPCGOptimizer::solver(*graph, *initial)) ; + SPCGOptimizer optimizer(graph, initial, solver) ; + + cout << "before optimization, sum of error is " << optimizer.error() << endl; + NonlinearOptimizationParameters parameter; + SPCGOptimizer optimizer2 = optimizer.levenbergMarquardt(parameter); + cout << "after optimization, sum of error is " << optimizer2.error() << endl; + + result = *optimizer2.config() ; + result.print("final result") ; + + return 0 ; +} + +void generateData() { + + // noise model + const SharedGaussian sigma(noiseModel::Unit::Create(0.1)); + + Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9); + + graph = boost::make_shared() ; + initial = boost::make_shared() ; + + // create a 3 by 3 grid + // x3 x6 x9 + // x2 x5 x8 + // x1 x4 x7 + graph->addConstraint(x1,x2,Pose2(0,2,0),sigma) ; + graph->addConstraint(x2,x3,Pose2(0,2,0),sigma) ; + graph->addConstraint(x4,x5,Pose2(0,2,0),sigma) ; + graph->addConstraint(x5,x6,Pose2(0,2,0),sigma) ; + graph->addConstraint(x7,x8,Pose2(0,2,0),sigma) ; + graph->addConstraint(x8,x9,Pose2(0,2,0),sigma) ; + graph->addConstraint(x1,x4,Pose2(2,0,0),sigma) ; + graph->addConstraint(x4,x7,Pose2(2,0,0),sigma) ; + graph->addConstraint(x2,x5,Pose2(2,0,0),sigma) ; + graph->addConstraint(x5,x8,Pose2(2,0,0),sigma) ; + graph->addConstraint(x3,x6,Pose2(2,0,0),sigma) ; + graph->addConstraint(x6,x9,Pose2(2,0,0),sigma) ; + graph->addPrior(x1, Pose2(0,0,0), sigma) ; + + initial->insert(x1, Pose2( 0, 0, 0)); + initial->insert(x2, Pose2( 0, 2.1, 0.01)); + initial->insert(x3, Pose2( 0, 3.9,-0.01)); + initial->insert(x4, Pose2(2.1,-0.1, 0)); + initial->insert(x5, Pose2(1.9, 2.1, 0.02)); + initial->insert(x6, Pose2(2.0, 3.9,-0.02)); + initial->insert(x7, Pose2(4.0, 0.1, 0.03 )); + initial->insert(x8, Pose2(3.9, 2.1, 0.01)); + initial->insert(x9, Pose2(4.1, 3.9,-0.01)); +} diff --git a/examples/Pose2SLAMwSPCG_easy.cpp b/examples/Pose2SLAMwSPCG_easy.cpp new file mode 100644 index 000000000..8e0d96a58 --- /dev/null +++ b/examples/Pose2SLAMwSPCG_easy.cpp @@ -0,0 +1,79 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * Solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface + */ + + +#include + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace pose2SLAM; + +Graph graph; +Values initial; +Values result; + +void generateData() ; + +/* ************************************************************************* */ +int main(void) { + + generateData() ; + graph.print("full graph") ; + initial.print("initial estimate"); + result = optimizeSPCG(graph, initial); + result.print("final result") ; + return 0 ; +} + +void generateData() { + + // noise model + const SharedGaussian sigma(noiseModel::Unit::Create(0.1)); + + Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9); + + // create a 3 by 3 grid + // x3 x6 x9 + // x2 x5 x8 + // x1 x4 x7 + graph.addConstraint(x1,x2,Pose2(0,2,0),sigma) ; + graph.addConstraint(x2,x3,Pose2(0,2,0),sigma) ; + graph.addConstraint(x4,x5,Pose2(0,2,0),sigma) ; + graph.addConstraint(x5,x6,Pose2(0,2,0),sigma) ; + graph.addConstraint(x7,x8,Pose2(0,2,0),sigma) ; + graph.addConstraint(x8,x9,Pose2(0,2,0),sigma) ; + graph.addConstraint(x1,x4,Pose2(2,0,0),sigma) ; + graph.addConstraint(x4,x7,Pose2(2,0,0),sigma) ; + graph.addConstraint(x2,x5,Pose2(2,0,0),sigma) ; + graph.addConstraint(x5,x8,Pose2(2,0,0),sigma) ; + graph.addConstraint(x3,x6,Pose2(2,0,0),sigma) ; + graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ; + graph.addPrior(x1, Pose2(0,0,0), sigma) ; + + initial.insert(x1, Pose2( 0, 0, 0)); + initial.insert(x2, Pose2( 0, 2.1, 0.01)); + initial.insert(x3, Pose2( 0, 3.9,-0.01)); + initial.insert(x4, Pose2(2.1,-0.1, 0)); + initial.insert(x5, Pose2(1.9, 2.1, 0.02)); + initial.insert(x6, Pose2(2.0, 3.9,-0.02)); + initial.insert(x7, Pose2(4.0, 0.1, 0.03 )); + initial.insert(x8, Pose2(3.9, 2.1, 0.01)); + initial.insert(x9, Pose2(4.1, 3.9,-0.01)); +} diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index ca319cb50..be541795e 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -54,7 +54,7 @@ int main() { // Create a factor Rot2 prior1 = Rot2::fromAngle(30 * degree); - prior1.print("Goal Angle"); + prior1.print("goal angle"); SharedDiagonal model1 = noiseModel::Isotropic::Sigma(1, 1 * degree); Key key1(1); PriorFactor factor1(key1, prior1, model1); @@ -62,17 +62,15 @@ int main() { // Create a factor graph Graph graph; graph.add(factor1); + graph.print("full graph") ; // and an initial estimate - Values initialEstimate; - initialEstimate.insert(key1, Rot2::fromAngle(20 * degree)); - initialEstimate.print("Initialization"); + Values initial; + initial.insert(key1, Rot2::fromAngle(20 * degree)); + initial.print("initial estimate"); - // create an ordering - Optimizer::Parameters parameters; - parameters.verbosity_ = Optimizer::Parameters::LAMBDA; - Values result = optimize(graph, initialEstimate, parameters); - result.print("Final config"); + Values result = optimize(graph, initial); + result.print("final result"); return 0; }