diff --git a/examples/Makefile.am b/examples/Makefile.am index c84443b2f..359cc0822 100644 --- a/examples/Makefile.am +++ b/examples/Makefile.am @@ -15,6 +15,7 @@ noinst_PROGRAMS = SimpleRotation # Optimizes a single nonlinear rotation v 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 #SUBDIRS = vSLAMexample # does not compile.... #---------------------------------------------------------------------------------------------------- # rules to build local programs diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp new file mode 100644 index 000000000..4bd026b0d --- /dev/null +++ b/examples/Pose2SLAMwSPCG.cpp @@ -0,0 +1,107 @@ +/* ---------------------------------------------------------------------------- + + * 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 + +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 SharedGaussian sigma(noiseModel::Unit::Create(0.1)); + + // generate measurement and initial configuration + generateData() ; + + 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/linear/SubgraphSolver-inl.h b/linear/SubgraphSolver-inl.h index 023aae9b9..86ebc2ad2 100644 --- a/linear/SubgraphSolver-inl.h +++ b/linear/SubgraphSolver-inl.h @@ -44,9 +44,8 @@ namespace gtsam { // split the graph if (verbose_) cout << "generating spanning tree and split the graph ..."; - // G.template split(tree, T_, C_); gtsam::split(G, tree, T_, C_) ; - if (verbose_) cout << T_.size() << " and " << C_.size() << " factors" << endl; + if (verbose_) cout << ",with " << T_.size() << " and " << C_.size() << " factors" << endl; // make the ordering list keys = predecessorMap2Keys(tree); diff --git a/linear/SubgraphSolver.h b/linear/SubgraphSolver.h index 3512e1615..118af4b99 100644 --- a/linear/SubgraphSolver.h +++ b/linear/SubgraphSolver.h @@ -40,8 +40,8 @@ namespace gtsam { // TODO not hardcode static const size_t maxIterations_=100; - static const bool verbose_=false; static const double epsilon_=1e-4, epsilon_abs_=1e-5; + static const bool verbose_=true; /* the ordering derived from the spanning tree */ boost::shared_ptr ordering_;