add a self-contained example to demonstrate spcg solver
							parent
							
								
									2a72112cde
								
							
						
					
					
						commit
						1e01e31363
					
				|  | @ -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
 | ||||
|  |  | |||
|  | @ -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)); | ||||
| } | ||||
|  | @ -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); | ||||
|  |  | |||
|  | @ -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_; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue