add a self-contained example to demonstrate spcg solver

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

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 <boost/shared_ptr.hpp>
+
+#include <gtsam/inference/graph-inl.h>
+#include <gtsam/inference/FactorGraph-inl.h>
+#include <gtsam/linear/SubgraphSolver-inl.h>
+#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
+#include <gtsam/slam/pose2SLAM.h>
+
+using namespace std;
+using namespace gtsam;
+using namespace pose2SLAM;
+
+typedef boost::shared_ptr<Graph> sharedGraph;
+typedef boost::shared_ptr<Values> sharedValue;
+
+
+typedef NonlinearOptimizer<Graph, Values, SubgraphPreconditioner, SubgraphSolver<Graph,Values> > 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));
+}

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<Key, Constraint>(tree, T_, C_);
 		gtsam::split<Graph,Key,Constraint>(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<Key> keys = predecessorMap2Keys(tree);

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> ordering_;