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

adapt spcg to new optimization interface

release/4.3a0
Yong-Dian Jian 2010-10-23 05:47:29 +00:00
parent eaabbdf7cd
commit 3bb1f26916
13 changed files with 197 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/Makefile.am
View File

@ -16,8 +16,8 @@ noinst_PROGRAMS += PlanarSLAMExample_easy # Solves SLAM example from tutorial
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
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

17
examples/Pose2SLAMwSPCG_advanced.cpp
View File

@ -25,9 +25,16 @@ 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;
typedef boost::shared_ptr<Graph> sharedGraph ;
typedef boost::shared_ptr<Values> sharedValue ;
//typedef NonlinearOptimizer<Graph, Values, SubgraphPreconditioner, SubgraphSolver<Graph,Values> > SPCGOptimizer;
typedef SubgraphSolver<Graph, GaussianFactorGraph, Values> Solver;
typedef boost::shared_ptr<Solver> sharedSolver ;
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, Solver> SPCGOptimizer;
sharedGraph graph;
sharedValue initial;
@ -44,8 +51,8 @@ int main(void) {
graph->print("full graph") ;
initial->print("initial estimate") ;
SPCGOptimizer::shared_solver solver(new SPCGOptimizer::solver(*graph, *initial)) ;
SPCGOptimizer optimizer(graph, initial, solver) ;
sharedSolver solver(new Solver(*graph, *initial)) ;
SPCGOptimizer optimizer(graph, initial, solver->ordering(), solver) ;
cout << "before optimization, sum of error is " << optimizer.error() << endl;
NonlinearOptimizationParameters parameter;

30
linear/GaussianFactorGraph.cpp
View File

@ -163,4 +163,34 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const vector<s
return result;
}
bool GaussianFactorGraph::split(const std::map<Index, Index> &M, GaussianFactorGraph &Ab1, GaussianFactorGraph &Ab2) const {
typedef sharedFactor F ;
Ab1 = GaussianFactorGraph();
Ab2 = GaussianFactorGraph();
BOOST_FOREACH(const F& factor, *this) {
if (factor->keys().size() > 2)
throw(invalid_argument("split: only support factors with at most two keys"));
if (factor->keys().size() == 1) {
Ab1.push_back(factor);
Ab2.push_back(factor);
continue;
}
Index key1 = factor->keys_[0];
Index key2 = factor->keys_[1];
if ((M.find(key1) != M.end() && M.find(key1)->second == key2) ||
(M.find(key2) != M.end() && M.find(key2)->second == key1))
Ab1.push_back(factor);
else
Ab2.push_back(factor);
}
return true ;
}
} // namespace gtsam

7
linear/GaussianFactorGraph.h
View File

@ -152,6 +152,13 @@ namespace gtsam {
*/
GaussianFactorGraph add_priors(double sigma, const std::vector<size_t>& dimensions) const;
/**
* Split a Gaussian factor graph into two, according to M
* M keeps the vertex indices of edges of A1. The others belong to A2.
*/
bool split(const std::map<Index, Index> &M, GaussianFactorGraph &A1, GaussianFactorGraph &A2) const ;
};
} // namespace gtsam

2
linear/GaussianMultifrontalSolver.cpp
View File

@ -59,7 +59,7 @@ GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginal(Index j) const {
std::pair<Vector, Matrix> GaussianMultifrontalSolver::marginalStandard(Index j) const {
GaussianConditional::shared_ptr conditional = Base::marginal(j)->eliminateFirst();
Matrix R = conditional->get_R();
return make_pair(conditional->get_d(), inverse(trans(R)*R));
return make_pair(conditional->get_d(), inverse(prod(trans(R),R)));
}
}

2
linear/GaussianSequentialSolver.cpp
View File

@ -75,7 +75,7 @@ GaussianFactor::shared_ptr GaussianSequentialSolver::marginal(Index j) const {
std::pair<Vector, Matrix> GaussianSequentialSolver::marginalStandard(Index j) const {
GaussianConditional::shared_ptr conditional = Base::marginal(j)->eliminateFirst();
Matrix R = conditional->get_R();
return make_pair(conditional->get_d(), inverse(trans(R)*R));
return make_pair(conditional->get_d(), inverse(prod(trans(R),R)));
}

6
linear/SubgraphPreconditioner.cpp
View File

@ -23,8 +23,8 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2,
sharedBayesNet& Rc1, sharedValues& xbar) :
SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2,
const sharedBayesNet& Rc1, const sharedValues& xbar) :
Ab1_(Ab1), Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(Ab2_->errors_(*xbar)) {
}
@ -50,8 +50,6 @@ namespace gtsam {
/* ************************************************************************* */
double SubgraphPreconditioner::error(const VectorValues& y) const {
Errors e(y);
VectorValues x = this->x(y);
Errors e2 = Ab2_->errors(x);

3
linear/SubgraphPreconditioner.h
View File

@ -46,6 +46,7 @@ namespace gtsam {
public:
SubgraphPreconditioner();
/**
* Constructor
* @param Ab1: the Graph A1*x=b1
@ -53,7 +54,7 @@ namespace gtsam {
* @param Rc1: the Bayes Net R1*x=c1
* @param xbar: the solution to R1*x=c1
*/
SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2, sharedBayesNet& Rc1, sharedValues& xbar);
SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar);
/**

69
linear/SubgraphSolver-inl.h
View File

@ -19,75 +19,8 @@
#pragma once
#include <boost/tuple/tuple.hpp>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/inference/graph-inl.h>
#include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/inference/EliminationTree-inl.h>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
template<class GRAPH, class VALUES>
SubgraphSolver<GRAPH, VALUES>::SubgraphSolver(const GRAPH& G, const VALUES& theta0) {
initialize(G,theta0);
}
/* ************************************************************************* */
template<class GRAPH, class VALUES>
void SubgraphSolver<GRAPH, VALUES>::initialize(const GRAPH& G, const VALUES& theta0) {
// generate spanning tree
PredecessorMap<Key> tree = gtsam::findMinimumSpanningTree<GRAPH, Key, Constraint>(G);
// split the graph
if (verbose_) cout << "generating spanning tree and split the graph ...";
gtsam::split<GRAPH,Key,Constraint>(G, tree, T_, C_) ;
if (verbose_) cout << ",with " << T_.size() << " and " << C_.size() << " factors" << endl;
// make the ordering
list<Key> keys = predecessorMap2Keys(tree);
ordering_ = boost::shared_ptr<Ordering>(new Ordering(list<Symbol>(keys.begin(), keys.end())));
// Add a HardConstraint to the root, otherwise the root will be singular
Key root = keys.back();
T_.addHardConstraint(root, theta0[root]);
// compose the approximate solution
theta_bar_ = composePoses<GRAPH, Constraint, Pose, VALUES> (T_, tree, theta0[root]);
}
/* ************************************************************************* */
template<class GRAPH, class VALUES>
boost::shared_ptr<SubgraphPreconditioner> SubgraphSolver<GRAPH, VALUES>::linearize(const GRAPH& G, const VALUES& theta_bar) const {
SubgraphPreconditioner::sharedFG Ab1 = T_.linearize(theta_bar, *ordering_);
SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar, *ordering_);
#ifdef TIMING
SubgraphPreconditioner::sharedBayesNet Rc1;
SubgraphPreconditioner::sharedValues xbar;
#else
GaussianFactorGraph sacrificialAb1 = *Ab1; // duplicate !!!!!
SubgraphPreconditioner::sharedBayesNet Rc1 = EliminationTree<GaussianFactor>::Create(sacrificialAb1)->eliminate();
SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
#endif
// TODO: there does not seem to be a good reason to have Ab1_
// It seems only be used to provide an ordering for creating sparse matrices
return boost::shared_ptr<SubgraphPreconditioner>(new SubgraphPreconditioner(Ab1, Ab2, Rc1, xbar));
}
/* ************************************************************************* */
template<class GRAPH, class VALUES>
VectorValues SubgraphSolver<GRAPH, VALUES>::optimize(SubgraphPreconditioner& system) const {
VectorValues zeros = system.zero();
// Solve the subgraph PCG
VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues,
Errors> (system, zeros, verbose_, epsilon_, epsilon_abs_, maxIterations_);
VectorValues xbar = system.x(ybar);
return xbar;
}
}
namespace gtsam {}

131
linear/SubgraphSolver.h
View File

@ -9,17 +9,19 @@
* -------------------------------------------------------------------------- */
/*
* SubgraphSolver.h
* Created on: Dec 31, 2009
* @author: Frank Dellaert
*/
#pragma once
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {
@ -30,13 +32,21 @@ namespace gtsam {
* linearize: G * T -> L
* solve : L -> VectorValues
*/
template<class GRAPH, class VALUES>
template<class GRAPH, class LINEAR, class VALUES>
class SubgraphSolver {
private:
typedef typename VALUES::Key Key;
typedef typename GRAPH::Constraint Constraint;
typedef typename GRAPH::Pose Pose;
typedef typename GRAPH::Constraint Constraint;
typedef boost::shared_ptr<const SubgraphSolver> shared_ptr ;
typedef boost::shared_ptr<Ordering> shared_ordering ;
typedef boost::shared_ptr<GRAPH> shared_graph ;
typedef boost::shared_ptr<LINEAR> shared_linear ;
typedef boost::shared_ptr<VALUES> shared_values ;
typedef boost::shared_ptr<SubgraphPreconditioner> shared_preconditioner ;
typedef std::map<Index,Index> mapPairIndex ;
// TODO not hardcode
static const size_t maxIterations_=100;
@ -44,50 +54,95 @@ namespace gtsam {
static const bool verbose_=true;
/* the ordering derived from the spanning tree */
boost::shared_ptr<Ordering> ordering_;
shared_ordering ordering_;
/* the solution computed from the first subgraph */
boost::shared_ptr<VALUES> theta_bar_;
/* the indice of two vertices in the gaussian factor graph */
mapPairIndex pairs_;
GRAPH T_, C_;
/* preconditioner */
shared_preconditioner pc_;
public:
SubgraphSolver() {}
SubgraphSolver(){}
SubgraphSolver(const GRAPH& G, const VALUES& theta0);
SubgraphSolver(const LINEAR &GFG) {
throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported");
}
void initialize(const GRAPH& G, const VALUES& theta0);
SubgraphSolver(const SubgraphSolver& solver) :
ordering_(solver.ordering_), pairs_(solver.pairs_), pc_(solver.pc_){}
boost::shared_ptr<Ordering> ordering() const { return ordering_; }
boost::shared_ptr<VALUES> theta_bar() const { return theta_bar_; }
/**
* linearize the non-linear graph around the current config and build the subgraph preconditioner systme
*/
boost::shared_ptr<SubgraphPreconditioner> linearize(const GRAPH& G, const VALUES& theta_bar) const;
SubgraphSolver(shared_ordering ordering,
mapPairIndex pairs,
shared_preconditioner pc) :
ordering_(ordering), pairs_(pairs), pc_(pc) {}
/**
* solve for the optimal displacement in the tangent space, and then solve
* the resulted linear system
*/
VectorValues optimize(SubgraphPreconditioner& system) const;
SubgraphSolver(const GRAPH& G, const VALUES& theta0) { initialize(G,theta0); }
boost::shared_ptr<SubgraphSolver> prepareLinear(const SubgraphPreconditioner& fg) const {
return boost::shared_ptr<SubgraphSolver>(new SubgraphSolver(*this));
shared_ptr update(const LINEAR &graph) const {
shared_linear Ab1 = boost::make_shared<LINEAR>(),
Ab2 = boost::make_shared<LINEAR>();
if (verbose_) cout << "split the graph ...";
graph.split(pairs_, *Ab1, *Ab2) ;
if (verbose_) cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl;
// // Add a HardConstra int to the root, otherwise the root will be singular
// Key root = keys.back();
// T_.addHardConstraint(root, theta0[root]);
//
// // compose the approximate solution
// theta_bar_ = composePoses<GRAPH, Constraint, Pose, Values> (T_, tree, theta0[root]);
LINEAR sacrificialAb1 = *Ab1; // duplicate !!!!!
SubgraphPreconditioner::sharedBayesNet Rc1 = EliminationTree<GaussianFactor>::Create(sacrificialAb1)->eliminate();
SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
shared_preconditioner pc = boost::make_shared<SubgraphPreconditioner>(Ab1,Ab2,Rc1,xbar);
return boost::make_shared<SubgraphSolver>(ordering_, pairs_, pc) ;
}
VectorValues::shared_ptr optimize() const {
// preconditioned conjugate gradient
VectorValues zeros = pc_->zero();
VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues,
Errors> (*pc_, zeros, verbose_, epsilon_, epsilon_abs_, maxIterations_);
boost::shared_ptr<VectorValues> xbar = boost::make_shared<VectorValues>() ;
*xbar = pc_->x(ybar);
return xbar;
}
shared_ordering ordering() const { return ordering_; }
protected:
void initialize(const GRAPH& G, const VALUES& theta0) {
// generate spanning tree
PredecessorMap<Key> tree_ = gtsam::findMinimumSpanningTree<GRAPH, Key, Constraint>(G);
// make the ordering
list<Key> keys = predecessorMap2Keys(tree_);
ordering_ = boost::make_shared<Ordering>(list<Symbol>(keys.begin(), keys.end()));
// build factor pairs
pairs_.clear();
typedef pair<Key,Key> EG ;
BOOST_FOREACH( const EG &eg, tree_ ) {
Symbol key1 = Symbol(eg.first),
key2 = Symbol(eg.second) ;
pairs_.insert(pair<Index, Index>((*ordering_)[key1], (*ordering_)[key2])) ;
}
}
/** expmap the Values given the stored Ordering */
VALUES expmap(const VALUES& config, const VectorValues& delta) const {
return config.expmap(delta, *ordering_);
}
};
template<class GRAPH, class VALUES> const size_t SubgraphSolver<GRAPH,VALUES>::maxIterations_;
template<class GRAPH, class VALUES> const bool SubgraphSolver<GRAPH,VALUES>::verbose_;
template<class GRAPH, class VALUES> const double SubgraphSolver<GRAPH,VALUES>::epsilon_;
template<class GRAPH, class VALUES> const double SubgraphSolver<GRAPH,VALUES>::epsilon_abs_;
template<class GRAPH, class LINEAR, class VALUES> const size_t SubgraphSolver<GRAPH, LINEAR, VALUES>::maxIterations_;
template<class GRAPH, class LINEAR, class VALUES> const bool SubgraphSolver<GRAPH, LINEAR, VALUES>::verbose_;
template<class GRAPH, class LINEAR, class VALUES> const double SubgraphSolver<GRAPH, LINEAR, VALUES>::epsilon_;
template<class GRAPH, class LINEAR, class VALUES> const double SubgraphSolver<GRAPH, LINEAR, VALUES>::epsilon_abs_;
} // nsamespace gtsam

39
nonlinear/NonlinearOptimization-inl.h
View File

@ -67,24 +67,27 @@ namespace gtsam {
return *result.values();
}
// /**
// * The sparse preconditioned conjucate gradient solver
// */
// template<class G, class T>
// T optimizeSPCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) {
//
// // initial optimization state is the same in both cases tested
// typedef NonlinearOptimizer<G, T, SubgraphPreconditioner, SubgraphSolver<G,T> > SPCGOptimizer;
// typename SPCGOptimizer::shared_solver solver(new SubgraphSolver<G,T>(graph, initialEstimate));
// SPCGOptimizer optimizer(
// boost::make_shared<const G>(graph),
// boost::make_shared<const T>(initialEstimate),
// solver);
//
// // Levenberg-Marquardt
// SPCGOptimizer result = optimizer.levenbergMarquardt(parameters);
// return *result.values();
// }
/**
* The sparse preconditioned conjucate gradient solver
*/
template<class G, class T>
T optimizeSPCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) {
// initial optimization state is the same in both cases tested
typedef SubgraphSolver<G,GaussianFactorGraph,T> Solver;
typedef boost::shared_ptr<Solver> shared_Solver;
typedef NonlinearOptimizer<G, T, GaussianFactorGraph, Solver> SPCGOptimizer;
shared_Solver solver = boost::make_shared<Solver>(graph, initialEstimate);
SPCGOptimizer optimizer(
boost::make_shared<const G>(graph),
boost::make_shared<const T>(initialEstimate),
solver->ordering(),
solver);
// Levenberg-Marquardt
SPCGOptimizer result = optimizer.levenbergMarquardt(parameters);
return *result.values();
}
/**
* optimization that returns the values

18
nonlinear/NonlinearOptimizer-inl.h
View File

@ -85,6 +85,24 @@ namespace gtsam {
"NonlinearOptimizer constructor: ordering = NULL");
}
template<class G, class C, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(
shared_graph graph, shared_values values, shared_ordering ordering, shared_solver solver, const double lambda):
graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering), solver_(solver),
lambda_(lambda), dimensions_(new vector<size_t>(values->dims(*ordering))) {
if (!graph) throw std::invalid_argument(
"NonlinearOptimizer constructor: graph = NULL");
if (!values) throw std::invalid_argument(
"NonlinearOptimizer constructor: values = NULL");
if (!ordering) throw std::invalid_argument(
"NonlinearOptimizer constructor: ordering = NULL");
if (!solver) throw std::invalid_argument(
"NonlinearOptimizer constructor: solver = NULL");
}
/* ************************************************************************* */
// linearize and optimize
/* ************************************************************************* */

7
nonlinear/NonlinearOptimizer.h
View File

@ -130,6 +130,13 @@ namespace gtsam {
NonlinearOptimizer(shared_graph graph, shared_values values, shared_ordering ordering,
const double lambda = 1e-5);
NonlinearOptimizer(shared_graph graph,
shared_values values,
shared_ordering ordering,
shared_solver solver,
const double lambda = 1e-5);
/**
* Copy constructor
*/