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

Separated split() from the SubgraphSolver because it won't compile in other projects, rearranged headers to remove redundancy and fix template problems

release/4.3a0
Alex Cunningham 2011-10-18 21:01:16 +00:00
parent 2abd5ee4aa
commit 4962c8ed38
6 changed files with 133 additions and 122 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/linear/Makefile.am
View File

@ -37,9 +37,9 @@ check_PROGRAMS += tests/testKalmanFilter
# Iterative Methods
headers += iterative-inl.h
sources += iterative.cpp SubgraphPreconditioner.cpp
sources += iterative.cpp SubgraphPreconditioner.cpp SubgraphSolver.cpp
headers += IterativeSolver.h IterativeOptimizationParameters.h
headers += SubgraphSolver.h SubgraphSolver-inl.h
headers += SubgraphSolver-inl.h
# Timing tests
noinst_PROGRAMS = tests/timeGaussianFactor tests/timeFactorOverhead tests/timeSLAMlike

81
gtsam/linear/SubgraphSolver-inl.h
View File

@ -11,17 +11,9 @@
#pragma once
#include <map>
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/inference/EliminationTree-inl.h>
@ -29,39 +21,6 @@ using namespace std;
namespace gtsam {
/* split the gaussian factor graph Ab into Ab1 and Ab2 according to the map */
bool split(const std::map<Index, Index> &M,
const GaussianFactorGraph &Ab,
GaussianFactorGraph &Ab1,
GaussianFactorGraph &Ab2) {
Ab1 = GaussianFactorGraph();
Ab2 = GaussianFactorGraph();
for ( size_t i = 0 ; i < Ab.size() ; ++i ) {
boost::shared_ptr<GaussianFactor> factor = Ab[i] ;
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 ;
}
template<class GRAPH, class LINEAR, class VALUES>
void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
@ -85,7 +44,7 @@ void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::
SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
pc_ = boost::make_shared<SubgraphPreconditioner>(
Ab1->dynamicCastFactors<FactorGraph<JacobianFactor> >(), Ab2->dynamicCastFactors<FactorGraph<JacobianFactor> >(),Rc1,xbar);
Ab1->dynamicCastFactors<FactorGraph<JacobianFactor> >(), Ab2->dynamicCastFactors<FactorGraph<JacobianFactor> >(),Rc1,xbar);
}
template<class GRAPH, class LINEAR, class VALUES>
@ -94,7 +53,7 @@ VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() const {
// preconditioned conjugate gradient
VectorValues zeros = pc_->zero();
VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues, Errors>
(*pc_, zeros, *parameters_);
(*pc_, zeros, *parameters_);
boost::shared_ptr<VectorValues> xbar = boost::make_shared<VectorValues>() ;
*xbar = pc_->x(ybar);
@ -103,29 +62,21 @@ VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() const {
template<class GRAPH, class LINEAR, class VALUES>
void SubgraphSolver<GRAPH,LINEAR,VALUES>::initialize(const GRAPH& G, const VALUES& theta0) {
// generate spanning tree
PredecessorMap<Key> tree_ = gtsam::findMinimumSpanningTree<GRAPH, Key, Constraint>(G);
// 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()));
// 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])) ;
}
// 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])) ;
}
}
} // \namespace gtsam

50
gtsam/linear/SubgraphSolver.cpp Normal file
View File

@ -0,0 +1,50 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#include <gtsam/linear/SubgraphSolver.h>
using namespace std;
namespace gtsam {
/* split the gaussian factor graph Ab into Ab1 and Ab2 according to the map */
bool split(const std::map<Index, Index> &M,
const GaussianFactorGraph &Ab,
GaussianFactorGraph &Ab1,
GaussianFactorGraph &Ab2) {
Ab1 = GaussianFactorGraph();
Ab2 = GaussianFactorGraph();
for ( size_t i = 0 ; i < Ab.size() ; ++i ) {
boost::shared_ptr<GaussianFactor> factor = Ab[i] ;
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

116
gtsam/linear/SubgraphSolver.h
View File

@ -11,80 +11,88 @@
#pragma once
#include <boost/make_shared.hpp>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
namespace gtsam {
/**
* A nonlinear system solver using subgraph preconditioning conjugate gradient
* Concept NonLinearSolver<G,T,L> implements
* linearize: G * T -> L
* solve : L -> VectorValues
*/
template<class GRAPH, class LINEAR, class VALUES>
class SubgraphSolver : public IterativeSolver {
/* split the gaussian factor graph Ab into Ab1 and Ab2 according to the map */
bool split(const std::map<Index, Index> &M,
const GaussianFactorGraph &Ab,
GaussianFactorGraph &Ab1,
GaussianFactorGraph &Ab2);
private:
typedef typename VALUES::Key Key;
typedef typename GRAPH::Pose Pose;
typedef typename GRAPH::Constraint Constraint;
/**
* A nonlinear system solver using subgraph preconditioning conjugate gradient
* Concept NonLinearSolver<G,T,L> implements
* linearize: G * T -> L
* solve : L -> VectorValues
*/
template<class GRAPH, class LINEAR, class VALUES>
class SubgraphSolver : public IterativeSolver {
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 ;
private:
typedef typename VALUES::Key Key;
typedef typename GRAPH::Pose Pose;
typedef typename GRAPH::Constraint Constraint;
/* the ordering derived from the spanning tree */
shared_ordering ordering_;
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 ;
/* the indice of two vertices in the gaussian factor graph */
mapPairIndex pairs_;
/* the ordering derived from the spanning tree */
shared_ordering ordering_;
/* preconditioner */
shared_preconditioner pc_;
/* the indice of two vertices in the gaussian factor graph */
mapPairIndex pairs_;
/* flag for direct solver - either QR or LDL */
bool useQR_;
/* preconditioner */
shared_preconditioner pc_;
public:
/* flag for direct solver - either QR or LDL */
bool useQR_;
SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters &parameters = Parameters(), bool useQR = false):
IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); }
public:
SubgraphSolver(const LINEAR& GFG) {
std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl;
throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported");
}
SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters &parameters = Parameters(), bool useQR = false):
IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); }
SubgraphSolver(const shared_linear& GFG, const boost::shared_ptr<VariableIndex>& structure, bool useQR = false) {
std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl;
throw std::runtime_error("SubgraphSolver: gaussian factor graph and variable index initialization not supported");
}
SubgraphSolver(const LINEAR& GFG) {
std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl;
throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported");
}
SubgraphSolver(const SubgraphSolver& solver) :
IterativeSolver(solver), ordering_(solver.ordering_), pairs_(solver.pairs_), pc_(solver.pc_), useQR_(solver.useQR_) {}
SubgraphSolver(const shared_linear& GFG, const boost::shared_ptr<VariableIndex>& structure, bool useQR = false) {
std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl;
throw std::runtime_error("SubgraphSolver: gaussian factor graph and variable index initialization not supported");
}
SubgraphSolver(shared_ordering ordering,
mapPairIndex pairs,
shared_preconditioner pc,
sharedParameters parameters = boost::make_shared<Parameters>(),
bool useQR = true) :
IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc), useQR_(useQR) {}
SubgraphSolver(const SubgraphSolver& solver) :
IterativeSolver(solver), ordering_(solver.ordering_), pairs_(solver.pairs_), pc_(solver.pc_), useQR_(solver.useQR_) {}
void replaceFactors(const typename LINEAR::shared_ptr &graph);
VectorValues::shared_ptr optimize() const ;
shared_ordering ordering() const { return ordering_; }
SubgraphSolver(shared_ordering ordering,
mapPairIndex pairs,
shared_preconditioner pc,
sharedParameters parameters = boost::make_shared<Parameters>(),
bool useQR = true) :
IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc), useQR_(useQR) {}
protected:
void initialize(const GRAPH& G, const VALUES& theta0);
void replaceFactors(const typename LINEAR::shared_ptr &graph);
VectorValues::shared_ptr optimize() const ;
shared_ordering ordering() const { return ordering_; }
private:
SubgraphSolver():IterativeSolver(){}
};
protected:
void initialize(const GRAPH& G, const VALUES& theta0);
private:
SubgraphSolver():IterativeSolver(){}
};
} // nsamespace gtsam

3
gtsam/nonlinear/NonlinearOptimization-inl.h
View File

@ -20,9 +20,10 @@
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
#include <gtsam/linear/SubgraphSolver-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
using namespace std;

1
gtsam/nonlinear/NonlinearOptimizationParameters.h
View File

@ -20,6 +20,7 @@
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/serialization/nvp.hpp>
namespace gtsam {