12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

separate the examples into easy/advanced style, remove shared pointer in easy examples, unify the convention

release/4.3a0
Yong-Dian Jian 2010-10-21 21:38:38 +00:00
parent c7248a2b13
commit adc8921f3b
9 changed files with 275 additions and 161 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
examples/Makefile.am
View File

@ -11,11 +11,13 @@ sources =
check_PROGRAMS = check_PROGRAMS =
# Examples # Examples
noinst_PROGRAMS = SimpleRotation # Optimizes a single nonlinear rotation variable noinst_PROGRAMS = SimpleRotation # Optimizes a single nonlinear rotation variable
noinst_PROGRAMS += PlanarSLAMExample # Solves SLAM example from tutorial by using planarSLAM noinst_PROGRAMS += PlanarSLAMExample_easy # Solves SLAM example from tutorial by using planarSLAM
noinst_PROGRAMS += Pose2SLAMExample # 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 += PlanarSLAMSelfContained # 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 += Pose2SLAMwSPCG # Solves a simple SLAM example with SPCG solver 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.... SUBDIRS = vSLAMexample # does not compile....
#---------------------------------------------------------------------------------------------------- #----------------------------------------------------------------------------------------------------
# rules to build local programs # rules to build local programs

6
examples/PlanarSLAMExample.cpp → examples/PlanarSLAMExample_easy.cpp
View File

@ -74,7 +74,7 @@ int main(int argc, char** argv) {
graph.addBearingRange(x2, l1, bearing21, range21, meas_model); graph.addBearingRange(x2, l1, bearing21, range21, meas_model);
graph.addBearingRange(x3, l2, bearing32, range32, meas_model); graph.addBearingRange(x3, l2, bearing32, range32, meas_model);
graph.print("Full Graph"); graph.print("full graph");
// initialize to noisy points // initialize to noisy points
Values initialEstimate; Values initialEstimate;
@ -84,11 +84,11 @@ int main(int argc, char** argv) {
initialEstimate.insert(l1, Point2(1.8, 2.1)); initialEstimate.insert(l1, Point2(1.8, 2.1));
initialEstimate.insert(l2, Point2(4.1, 1.8)); 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 // optimize using Levenberg-Marquardt optimization with an ordering from colamd
Values result = optimize<Graph, Values>(graph, initialEstimate); Values result = optimize<Graph, Values>(graph, initialEstimate);
result.print("Final Result"); result.print("final result");
return 0; return 0;
} }

18
examples/PlanarSLAMSelfContained.cpp → examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -110,19 +110,19 @@ int main(int argc, char** argv) {
graph.print("Full Graph"); graph.print("Full Graph");
// initialize to noisy points // initialize to noisy points
Values initialEstimate; Values initial;
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); initial.insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); initial.insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); initial.insert(x3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insert(l1, Point2(1.8, 2.1)); initial.insert(l1, Point2(1.8, 2.1));
initialEstimate.insert(l2, Point2(4.1, 1.8)); 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 // 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; return 0;
} }

37
examples/Pose2SLAMExample.cpp → examples/Pose2SLAMExample_advanced.cpp
View File

@ -34,7 +34,6 @@ int main(int argc, char** argv) {
Key x1(1), x2(2), x3(3); Key x1(1), x2(2), x3(3);
/* 1. create graph container and add factors to it */ /* 1. create graph container and add factors to it */
//Graph graph;
shared_ptr<Graph> graph(new Graph); shared_ptr<Graph> graph(new Graph);
/* 2.a add prior */ /* 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) 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(x1, x2, odom_measurement, odom_model);
graph->addConstraint(x2, x3, 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 /* 3. Create the data structure to hold the initial estinmate to the solution
* initialize to noisy points */ * initialize to noisy points */
shared_ptr<Values> initialEstimate(new Values); shared_ptr<Values> initial(new Values);
initialEstimate->insert(x1, Pose2(0.5, 0.0, 0.2)); initial->insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate->insert(x2, Pose2(2.3, 0.1,-0.2)); initial->insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate->insert(x3, Pose2(4.1, 0.1, 0.1)); 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, Values>(*graph, *initialEstimate);
result.print("Final Result");
/* 4.2.1 Alternatively, you can go through the process step by step /* 4.2.1 Alternatively, you can go through the process step by step
* Choose an ordering */ * 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::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); Values result = *optimizer_result.config();
Optimizer::Parameters::verbosityLevel verbosity = pose2SLAM::Optimizer::Parameters::SILENT; result.print("final result");
Optimizer optimizer0 = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity);
Values result2 = *optimizer0.config();
result2.print("Final Result 2");
return 0; return 0;
} }

59
examples/Pose2SLAMExample_easy.cpp Normal file
View File

@ -0,0 +1,59 @@
/*
* Pose2SLAMExample_easy.cpp
*
* Created on: Oct 21, 2010
* Author: ydjian
*/
#include <cmath>
#include <iostream>
#include <boost/shared_ptr.hpp>
// pull in the Pose2 SLAM domain with all typedefs and helper functions defined
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
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, Values>(graph, initial);
result.print("final result");
return 0;
}

111
examples/Pose2SLAMwSPCG.cpp
View File

@ -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 <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/nonlinear/NonlinearOptimization-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 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));
}

98
examples/Pose2SLAMwSPCG_advanced.cpp Normal file
View File

@ -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 <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 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<Graph>() ;
initial = boost::make_shared<Values>() ;
// 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));
}

79
examples/Pose2SLAMwSPCG_easy.cpp Normal file
View File

@ -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 <boost/shared_ptr.hpp>
#include <gtsam/inference/graph-inl.h>
#include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
#include <gtsam/slam/pose2SLAM.h>
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));
}

16
examples/SimpleRotation.cpp
View File

@ -54,7 +54,7 @@ int main() {
// Create a factor // Create a factor
Rot2 prior1 = Rot2::fromAngle(30 * degree); Rot2 prior1 = Rot2::fromAngle(30 * degree);
prior1.print("Goal Angle"); prior1.print("goal angle");
SharedDiagonal model1 = noiseModel::Isotropic::Sigma(1, 1 * degree); SharedDiagonal model1 = noiseModel::Isotropic::Sigma(1, 1 * degree);
Key key1(1); Key key1(1);
PriorFactor<Values, Key> factor1(key1, prior1, model1); PriorFactor<Values, Key> factor1(key1, prior1, model1);
@ -62,17 +62,15 @@ int main() {
// Create a factor graph // Create a factor graph
Graph graph; Graph graph;
graph.add(factor1); graph.add(factor1);
graph.print("full graph") ;
// and an initial estimate // and an initial estimate
Values initialEstimate; Values initial;
initialEstimate.insert(key1, Rot2::fromAngle(20 * degree)); initial.insert(key1, Rot2::fromAngle(20 * degree));
initialEstimate.print("Initialization"); initial.print("initial estimate");
// create an ordering Values result = optimize<Graph, Values>(graph, initial);
Optimizer::Parameters parameters; result.print("final result");
parameters.verbosity_ = Optimizer::Parameters::LAMBDA;
Values result = optimize<Graph, Values>(graph, initialEstimate, parameters);
result.print("Final config");
return 0; return 0;
} }