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

Template arguments capitalized

release/4.3a0
Rahul Sawhney 2010-10-20 16:37:47 +00:00
parent e5aacb8ba1
commit 82a2f29e48
17 changed files with 186 additions and 186 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
linear/Factorization.h
View File

@ -31,7 +31,7 @@ namespace gtsam {
/** /**
* A linear system solver using factorization * A linear system solver using factorization
*/ */
template <class NonlinearGraph, class Values> template <class NONLINEARGRAPH, class VALUES>
class Factorization { class Factorization {
private: private:
boost::shared_ptr<Ordering> ordering_; boost::shared_ptr<Ordering> ordering_;
@ -53,7 +53,7 @@ namespace gtsam {
/** /**
* linearize the non-linear graph around the current config * linearize the non-linear graph around the current config
*/ */
boost::shared_ptr<GaussianFactorGraph> linearize(const NonlinearGraph& g, const Values& config) const { boost::shared_ptr<GaussianFactorGraph> linearize(const NONLINEARGRAPH& g, const VALUES& config) const {
return g.linearize(config, *ordering_); return g.linearize(config, *ordering_);
} }
@ -65,7 +65,7 @@ namespace gtsam {
} }
/** expmap the Values given the stored Ordering */ /** expmap the Values given the stored Ordering */
Values expmap(const Values& config, const VectorValues& delta) const { VALUES expmap(const VALUES& config, const VectorValues& delta) const {
return config.expmap(delta, *ordering_); return config.expmap(delta, *ordering_);
} }
}; };

10
linear/GaussianConditional.h
View File

@ -100,8 +100,8 @@ public:
* Constructor when matrices are already stored in a combined matrix, allows * Constructor when matrices are already stored in a combined matrix, allows
* for multiple frontal variables. * for multiple frontal variables.
*/ */
template<typename Iterator, class Matrix> template<typename ITERATOR, class MATRIX>
GaussianConditional(Iterator firstKey, Iterator lastKey, size_t nrFrontals, const VerticalBlockView<Matrix>& matrices, const Vector& sigmas); GaussianConditional(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals, const VerticalBlockView<MATRIX>& matrices, const Vector& sigmas);
/** print */ /** print */
void print(const std::string& = "GaussianConditional") const; void print(const std::string& = "GaussianConditional") const;
@ -162,9 +162,9 @@ private:
}; };
/* ************************************************************************* */ /* ************************************************************************* */
template<typename Iterator, class Matrix> template<typename ITERATOR, class MATRIX>
GaussianConditional::GaussianConditional(Iterator firstKey, Iterator lastKey, GaussianConditional::GaussianConditional(ITERATOR firstKey, ITERATOR lastKey,
size_t nrFrontals, const VerticalBlockView<Matrix>& matrices, size_t nrFrontals, const VerticalBlockView<MATRIX>& matrices,
const Vector& sigmas) : const Vector& sigmas) :
rsd_(matrix_), sigmas_(sigmas) { rsd_(matrix_), sigmas_(sigmas) {
nrFrontals_ = nrFrontals; nrFrontals_ = nrFrontals;

4
linear/GaussianFactor.cpp
View File

@ -593,9 +593,9 @@ struct _RowSource {
template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>&, const GaussianVariableIndex<VariableIndexStorage_vector>&, const vector<size_t>&, const vector<Index>&, const std::vector<std::vector<size_t> >&); template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>&, const GaussianVariableIndex<VariableIndexStorage_vector>&, const vector<size_t>&, const vector<Index>&, const std::vector<std::vector<size_t> >&);
template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>&, const GaussianVariableIndex<VariableIndexStorage_deque>&, const vector<size_t>&, const vector<Index>&, const std::vector<std::vector<size_t> >&); template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>&, const GaussianVariableIndex<VariableIndexStorage_deque>&, const vector<size_t>&, const vector<Index>&, const std::vector<std::vector<size_t> >&);
template<class Storage> template<class STORAGE>
GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>& factorGraph, GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>& factorGraph,
const GaussianVariableIndex<Storage>& variableIndex, const vector<size_t>& factors, const GaussianVariableIndex<STORAGE>& variableIndex, const vector<size_t>& factors,
const vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions) { const vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions) {
shared_ptr ret(boost::make_shared<GaussianFactor>()); shared_ptr ret(boost::make_shared<GaussianFactor>());

6
linear/GaussianFactor.h
View File

@ -49,7 +49,7 @@
namespace gtsam { namespace gtsam {
class GaussianFactorGraph; class GaussianFactorGraph;
template<class VariableIndexStorage=VariableIndexStorage_vector> class GaussianVariableIndex; template<class VARIABLEINDEXSTORAGE=VariableIndexStorage_vector> class GaussianVariableIndex;
/** A map from key to dimension, useful in various contexts */ /** A map from key to dimension, useful in various contexts */
typedef std::map<Index, size_t> Dimensions; typedef std::map<Index, size_t> Dimensions;
@ -149,9 +149,9 @@ public:
/** Named constructor for combining a set of factors with pre-computed set of /** Named constructor for combining a set of factors with pre-computed set of
* variables. */ * variables. */
template<class Storage> template<class STORAGE>
static shared_ptr Combine(const FactorGraph<GaussianFactor>& factorGraph, static shared_ptr Combine(const FactorGraph<GaussianFactor>& factorGraph,
const GaussianVariableIndex<Storage>& variableIndex, const std::vector<size_t>& factors, const GaussianVariableIndex<STORAGE>& variableIndex, const std::vector<size_t>& factors,
const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions); const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions);
/** /**

46
linear/GaussianFactorGraph.h
View File

@ -157,11 +157,11 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class VariableIndexStorage> template<class VARIABLEINDEXSTORAGE>
class GaussianVariableIndex : public VariableIndex<VariableIndexStorage> { class GaussianVariableIndex : public VariableIndex<VARIABLEINDEXSTORAGE> {
public: public:
typedef VariableIndex<VariableIndexStorage> Base; typedef VariableIndex<VARIABLEINDEXSTORAGE> Base;
typedef typename VariableIndexStorage::template type_factory<size_t>::type storage_type; typedef typename VARIABLEINDEXSTORAGE::template type_factory<size_t>::type storage_type;
storage_type dims_; storage_type dims_;
@ -181,13 +181,13 @@ namespace gtsam {
* Constructor to "upgrade" from the base class without recomputing the * Constructor to "upgrade" from the base class without recomputing the
* column index, i.e. just fills the dims_ array. * column index, i.e. just fills the dims_ array.
*/ */
GaussianVariableIndex(const VariableIndex<VariableIndexStorage>& variableIndex, const GaussianFactorGraph& factorGraph); GaussianVariableIndex(const VariableIndex<VARIABLEINDEXSTORAGE>& variableIndex, const GaussianFactorGraph& factorGraph);
/** /**
* Another constructor to upgrade from the base class using an existing * Another constructor to upgrade from the base class using an existing
* array of variable dimensions. * array of variable dimensions.
*/ */
GaussianVariableIndex(const VariableIndex<VariableIndexStorage>& variableIndex, const storage_type& dimensions); GaussianVariableIndex(const VariableIndex<VARIABLEINDEXSTORAGE>& variableIndex, const storage_type& dimensions);
const storage_type& dims() const { return dims_; } const storage_type& dims() const { return dims_; }
size_t dim(Index variable) const { Base::checkVar(variable); return dims_[variable]; } size_t dim(Index variable) const { Base::checkVar(variable); return dims_[variable]; }
@ -205,28 +205,28 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class Storage> template<class STORAGE>
GaussianVariableIndex<Storage>::GaussianVariableIndex(const GaussianFactorGraph& factorGraph) : GaussianVariableIndex<STORAGE>::GaussianVariableIndex(const GaussianFactorGraph& factorGraph) :
Base(factorGraph), dims_(Base::index_.size()) { Base(factorGraph), dims_(Base::index_.size()) {
fillDims(factorGraph); } fillDims(factorGraph); }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Storage> template<class STORAGE>
GaussianVariableIndex<Storage>::GaussianVariableIndex( GaussianVariableIndex<STORAGE>::GaussianVariableIndex(
const VariableIndex<Storage>& variableIndex, const GaussianFactorGraph& factorGraph) : const VariableIndex<STORAGE>& variableIndex, const GaussianFactorGraph& factorGraph) :
Base(variableIndex), dims_(Base::index_.size()) { Base(variableIndex), dims_(Base::index_.size()) {
fillDims(factorGraph); } fillDims(factorGraph); }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Storage> template<class STORAGE>
GaussianVariableIndex<Storage>::GaussianVariableIndex( GaussianVariableIndex<STORAGE>::GaussianVariableIndex(
const VariableIndex<Storage>& variableIndex, const storage_type& dimensions) : const VariableIndex<STORAGE>& variableIndex, const storage_type& dimensions) :
Base(variableIndex), dims_(dimensions) { Base(variableIndex), dims_(dimensions) {
assert(Base::index_.size() == dims_.size()); } assert(Base::index_.size() == dims_.size()); }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Storage> template<class STORAGE>
void GaussianVariableIndex<Storage>::fillDims(const GaussianFactorGraph& factorGraph) { void GaussianVariableIndex<STORAGE>::fillDims(const GaussianFactorGraph& factorGraph) {
// Store dimensions of each variable // Store dimensions of each variable
assert(dims_.size() == Base::index_.size()); assert(dims_.size() == Base::index_.size());
for(Index var=0; var<Base::index_.size(); ++var) for(Index var=0; var<Base::index_.size(); ++var)
@ -240,9 +240,9 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Storage> template<class STORAGE>
void GaussianVariableIndex<Storage>::permute(const Permutation& permutation) { void GaussianVariableIndex<STORAGE>::permute(const Permutation& permutation) {
VariableIndex<Storage>::permute(permutation); VariableIndex<STORAGE>::permute(permutation);
storage_type original(this->dims_.size()); storage_type original(this->dims_.size());
this->dims_.swap(original); this->dims_.swap(original);
for(Index j=0; j<permutation.size(); ++j) for(Index j=0; j<permutation.size(); ++j)
@ -250,8 +250,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Storage> template<class STORAGE>
void GaussianVariableIndex<Storage>::augment(const GaussianFactorGraph& factorGraph) { void GaussianVariableIndex<STORAGE>::augment(const GaussianFactorGraph& factorGraph) {
Base::augment(factorGraph); Base::augment(factorGraph);
dims_.resize(Base::index_.size(), 0); dims_.resize(Base::index_.size(), 0);
BOOST_FOREACH(boost::shared_ptr<const GaussianFactor> factor, factorGraph) { BOOST_FOREACH(boost::shared_ptr<const GaussianFactor> factor, factorGraph) {
@ -283,9 +283,9 @@ namespace gtsam {
// * @param ordering of variables needed for matrix column order // * @param ordering of variables needed for matrix column order
// * @return the augmented matrix and a noise model // * @return the augmented matrix and a noise model
// */ // */
// template <class Factors> // template <class FACTORS>
// std::pair<Matrix, SharedDiagonal> combineFactorsAndCreateMatrix( // std::pair<Matrix, SharedDiagonal> combineFactorsAndCreateMatrix(
// const Factors& factors, // const FACTORS& factors,
// const Ordering& order, const Dimensions& dimensions); // const Ordering& order, const Dimensions& dimensions);
} // namespace gtsam } // namespace gtsam

12
linear/GaussianISAM.h
View File

@ -38,14 +38,14 @@ public:
GaussianISAM(const GaussianBayesNet& bayesNet) : ISAM<GaussianConditional>(bayesNet) {} GaussianISAM(const GaussianBayesNet& bayesNet) : ISAM<GaussianConditional>(bayesNet) {}
/** Override update_internal to also keep track of variable dimensions. */ /** Override update_internal to also keep track of variable dimensions. */
template<class FactorGraph> template<class FACTORGRAPH>
void update_internal(const FactorGraph& newFactors, Cliques& orphans) { void update_internal(const FACTORGRAPH& newFactors, Cliques& orphans) {
ISAM<GaussianConditional>::update_internal(newFactors, orphans); ISAM<GaussianConditional>::update_internal(newFactors, orphans);
// update dimensions // update dimensions
BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, newFactors) { BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, newFactors) {
for(typename FactorGraph::Factor::const_iterator key = factor->begin(); key != factor->end(); ++key) { for(typename FACTORGRAPH::Factor::const_iterator key = factor->begin(); key != factor->end(); ++key) {
if(*key >= dims_.size()) if(*key >= dims_.size())
dims_.resize(*key + 1); dims_.resize(*key + 1);
if(dims_[*key] == 0) if(dims_[*key] == 0)
@ -56,8 +56,8 @@ public:
} }
} }
template<class FactorGraph> template<class FACTORGRAPH>
void update(const FactorGraph& newFactors) { void update(const FACTORGRAPH& newFactors) {
Cliques orphans; Cliques orphans;
this->update_internal(newFactors, orphans); this->update_internal(newFactors, orphans);
} }

22
linear/SubgraphSolver-inl.h
View File

@ -30,21 +30,21 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class Graph, class Values> template<class GRAPH, class VALUES>
SubgraphSolver<Graph, Values>::SubgraphSolver(const Graph& G, const Values& theta0) { SubgraphSolver<GRAPH, VALUES>::SubgraphSolver(const GRAPH& G, const VALUES& theta0) {
initialize(G,theta0); initialize(G,theta0);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Graph, class Values> template<class GRAPH, class VALUES>
void SubgraphSolver<Graph, Values>::initialize(const Graph& G, const Values& theta0) { void SubgraphSolver<GRAPH, VALUES>::initialize(const GRAPH& G, const VALUES& theta0) {
// generate spanning tree // generate spanning tree
PredecessorMap<Key> tree = gtsam::findMinimumSpanningTree<Graph, Key, Constraint>(G); PredecessorMap<Key> tree = gtsam::findMinimumSpanningTree<GRAPH, Key, Constraint>(G);
// split the graph // split the graph
if (verbose_) cout << "generating spanning tree and split the graph ..."; if (verbose_) cout << "generating spanning tree and split the graph ...";
gtsam::split<Graph,Key,Constraint>(G, tree, T_, C_) ; gtsam::split<GRAPH,Key,Constraint>(G, tree, T_, C_) ;
if (verbose_) cout << ",with " << T_.size() << " and " << C_.size() << " factors" << endl; if (verbose_) cout << ",with " << T_.size() << " and " << C_.size() << " factors" << endl;
// make the ordering // make the ordering
@ -56,12 +56,12 @@ namespace gtsam {
T_.addHardConstraint(root, theta0[root]); T_.addHardConstraint(root, theta0[root]);
// compose the approximate solution // compose the approximate solution
theta_bar_ = composePoses<Graph, Constraint, Pose, Values> (T_, tree, theta0[root]); theta_bar_ = composePoses<GRAPH, Constraint, Pose, VALUES> (T_, tree, theta0[root]);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Graph, class Values> template<class GRAPH, class VALUES>
boost::shared_ptr<SubgraphPreconditioner> SubgraphSolver<Graph, Values>::linearize(const Graph& G, const Values& theta_bar) const { 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 Ab1 = T_.linearize(theta_bar, *ordering_);
SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar, *ordering_); SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar, *ordering_);
#ifdef TIMING #ifdef TIMING
@ -78,8 +78,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Graph, class Values> template<class GRAPH, class VALUES>
VectorValues SubgraphSolver<Graph, Values>::optimize(SubgraphPreconditioner& system) const { VectorValues SubgraphSolver<GRAPH, VALUES>::optimize(SubgraphPreconditioner& system) const {
VectorValues zeros = system.zero(); VectorValues zeros = system.zero();
// Solve the subgraph PCG // Solve the subgraph PCG

30
linear/SubgraphSolver.h
View File

@ -30,13 +30,13 @@ namespace gtsam {
* linearize: G * T -> L * linearize: G * T -> L
* solve : L -> VectorValues * solve : L -> VectorValues
*/ */
template<class Graph, class Values> template<class GRAPH, class VALUES>
class SubgraphSolver { class SubgraphSolver {
private: private:
typedef typename Values::Key Key; typedef typename VALUES::Key Key;
typedef typename Graph::Constraint Constraint; typedef typename GRAPH::Constraint Constraint;
typedef typename Graph::Pose Pose; typedef typename GRAPH::Pose Pose;
// TODO not hardcode // TODO not hardcode
static const size_t maxIterations_=100; static const size_t maxIterations_=100;
@ -47,25 +47,25 @@ namespace gtsam {
boost::shared_ptr<Ordering> ordering_; boost::shared_ptr<Ordering> ordering_;
/* the solution computed from the first subgraph */ /* the solution computed from the first subgraph */
boost::shared_ptr<Values> theta_bar_; boost::shared_ptr<VALUES> theta_bar_;
Graph T_, C_; GRAPH T_, C_;
public: public:
SubgraphSolver() {} SubgraphSolver() {}
SubgraphSolver(const Graph& G, const Values& theta0); SubgraphSolver(const GRAPH& G, const VALUES& theta0);
void initialize(const Graph& G, const Values& theta0); void initialize(const GRAPH& G, const VALUES& theta0);
boost::shared_ptr<Ordering> ordering() const { return ordering_; } boost::shared_ptr<Ordering> ordering() const { return ordering_; }
boost::shared_ptr<Values> theta_bar() const { return theta_bar_; } 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 * 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; boost::shared_ptr<SubgraphPreconditioner> linearize(const GRAPH& G, const VALUES& theta_bar) const;
/** /**
@ -80,14 +80,14 @@ namespace gtsam {
/** expmap the Values given the stored Ordering */ /** expmap the Values given the stored Ordering */
Values expmap(const Values& config, const VectorValues& delta) const { VALUES expmap(const VALUES& config, const VectorValues& delta) const {
return config.expmap(delta, *ordering_); return config.expmap(delta, *ordering_);
} }
}; };
template<class Graph, class Values> const size_t SubgraphSolver<Graph,Values>::maxIterations_; 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 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_;
template<class Graph, class Values> const double SubgraphSolver<Graph,Values>::epsilon_abs_; template<class GRAPH, class VALUES> const double SubgraphSolver<GRAPH,VALUES>::epsilon_abs_;
} // nsamespace gtsam } // nsamespace gtsam

4
linear/VectorBTree.h
View File

@ -234,8 +234,8 @@ public:
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class ARCHIVE>
void serialize(Archive & ar, const unsigned int version) void serialize(ARCHIVE & ar, const unsigned int version)
{ {
ar & BOOST_SERIALIZATION_NVP(values); ar & BOOST_SERIALIZATION_NVP(values);
} }

4
linear/VectorMap.h
View File

@ -145,8 +145,8 @@ namespace gtsam {
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class ARCHIVE>
void serialize(Archive & ar, const unsigned int version) void serialize(ARCHIVE & ar, const unsigned int version)
{ {
ar & BOOST_SERIALIZATION_NVP(values); ar & BOOST_SERIALIZATION_NVP(values);
} }

8
linear/VectorValues.h
View File

@ -60,8 +60,8 @@ public:
VectorValues(const VectorValues &V) : values_(V.values_), varStarts_(V.varStarts_) {} VectorValues(const VectorValues &V) : values_(V.values_), varStarts_(V.varStarts_) {}
/** Construct from a container of variable dimensions (in variable order). */ /** Construct from a container of variable dimensions (in variable order). */
template<class Container> template<class CONTAINER>
VectorValues(const Container& dimensions); VectorValues(const CONTAINER& dimensions);
/** Construct to hold nVars vectors of varDim dimension each. */ /** Construct to hold nVars vectors of varDim dimension each. */
VectorValues(Index nVars, size_t varDim); VectorValues(Index nVars, size_t varDim);
@ -213,8 +213,8 @@ public:
// values_.resize(varStarts_.back(), false); // values_.resize(varStarts_.back(), false);
//} //}
template<class Container> template<class CONTAINER>
inline VectorValues::VectorValues(const Container& dimensions) : varStarts_(dimensions.size()+1) { inline VectorValues::VectorValues(const CONTAINER& dimensions) : varStarts_(dimensions.size()+1) {
varStarts_[0] = 0; varStarts_[0] = 0;
size_t varStart = 0; size_t varStart = 0;
Index var = 0; Index var = 0;

18
nonlinear/Key.h
View File

@ -79,8 +79,8 @@ namespace gtsam {
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class ARCHIVE>
void serialize(Archive & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(j_); ar & BOOST_SERIALIZATION_NVP(j_);
} }
}; };
@ -171,8 +171,8 @@ namespace gtsam {
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class ARCHIVE>
void serialize(Archive & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(c_);
ar & BOOST_SERIALIZATION_NVP(j_); ar & BOOST_SERIALIZATION_NVP(j_);
} }
@ -180,13 +180,13 @@ namespace gtsam {
// Conversion utilities // Conversion utilities
template<class Key> Symbol key2symbol(Key key) { template<class KEY> Symbol key2symbol(KEY key) {
return Symbol(key); return Symbol(key);
} }
template<class Key> std::list<Symbol> keys2symbols(std::list<Key> keys) { template<class KEY> std::list<Symbol> keys2symbols(std::list<KEY> keys) {
std::list<Symbol> symbols; std::list<Symbol> symbols;
std::transform(keys.begin(), keys.end(), std::back_inserter(symbols), key2symbol<Key> ); std::transform(keys.begin(), keys.end(), std::back_inserter(symbols), key2symbol<KEY> );
return symbols; return symbols;
} }
@ -281,8 +281,8 @@ namespace gtsam {
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class ARCHIVE>
void serialize(Archive & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
typedef TypedSymbol<T,C> Base; typedef TypedSymbol<T,C> Base;
ar & boost::serialization::make_nvp("TypedLabeledSymbol", ar & boost::serialization::make_nvp("TypedLabeledSymbol",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));

8
nonlinear/LieValues.h
View File

@ -72,8 +72,8 @@ namespace gtsam {
LieValues() {} LieValues() {}
LieValues(const LieValues& config) : LieValues(const LieValues& config) :
values_(config.values_) {} values_(config.values_) {}
template<class J_alt> template<class J_ALT>
LieValues(const LieValues<J_alt>& other) {} // do nothing when initializing with wrong type LieValues(const LieValues<J_ALT>& other) {} // do nothing when initializing with wrong type
virtual ~LieValues() {} virtual ~LieValues() {}
/** print */ /** print */
@ -197,8 +197,8 @@ namespace gtsam {
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class ARCHIVE>
void serialize(Archive & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(values_); ar & BOOST_SERIALIZATION_NVP(values_);
} }

18
nonlinear/NonlinearEquality.h
View File

@ -41,11 +41,11 @@ namespace gtsam {
* - ALLLOW_ERROR : if we allow that there can be nonzero error, does not throw, and uses gain * - ALLLOW_ERROR : if we allow that there can be nonzero error, does not throw, and uses gain
* - ONLY_EXACT : throws error at linearization if not at exact feasible point, and infinite error * - ONLY_EXACT : throws error at linearization if not at exact feasible point, and infinite error
*/ */
template<class Values, class Key> template<class VALUES, class KEY>
class NonlinearEquality: public NonlinearFactor1<Values, Key> { class NonlinearEquality: public NonlinearFactor1<VALUES, KEY> {
public: public:
typedef typename Key::Value T; typedef typename KEY::Value T;
private: private:
@ -65,12 +65,12 @@ namespace gtsam {
*/ */
bool (*compare_)(const T& a, const T& b); bool (*compare_)(const T& a, const T& b);
typedef NonlinearFactor1<Values, Key> Base; typedef NonlinearFactor1<VALUES, KEY> Base;
/** /**
* Constructor - forces exact evaluation * Constructor - forces exact evaluation
*/ */
NonlinearEquality(const Key& j, const T& feasible, bool (*compare)(const T&, const T&) = compare<T>) : NonlinearEquality(const KEY& j, const T& feasible, bool (*compare)(const T&, const T&) = compare<T>) :
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
allow_error_(false), error_gain_(std::numeric_limits<double>::infinity()), allow_error_(false), error_gain_(std::numeric_limits<double>::infinity()),
compare_(compare) { compare_(compare) {
@ -79,7 +79,7 @@ namespace gtsam {
/** /**
* Constructor - allows inexact evaluation * Constructor - allows inexact evaluation
*/ */
NonlinearEquality(const Key& j, const T& feasible, double error_gain, bool (*compare)(const T&, const T&) = compare<T>) : NonlinearEquality(const KEY& j, const T& feasible, double error_gain, bool (*compare)(const T&, const T&) = compare<T>) :
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
allow_error_(true), error_gain_(error_gain), allow_error_(true), error_gain_(error_gain),
compare_(compare) { compare_(compare) {
@ -92,13 +92,13 @@ namespace gtsam {
} }
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const NonlinearEquality<Values,Key>& f, double tol = 1e-9) const { bool equals(const NonlinearEquality<VALUES,KEY>& f, double tol = 1e-9) const {
if (!Base::equals(f)) return false; if (!Base::equals(f)) return false;
return compare_(feasible_, f.feasible_); return compare_(feasible_, f.feasible_);
} }
/** actual error function calculation */ /** actual error function calculation */
virtual double error(const Values& c) const { virtual double error(const VALUES& c) const {
const T& xj = c[this->key_]; const T& xj = c[this->key_];
Vector e = this->unwhitenedError(c); Vector e = this->unwhitenedError(c);
if (allow_error_ || !compare_(xj, feasible_)) { if (allow_error_ || !compare_(xj, feasible_)) {
@ -125,7 +125,7 @@ namespace gtsam {
} }
// Linearize is over-written, because base linearization tries to whiten // Linearize is over-written, because base linearization tries to whiten
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x, const Ordering& ordering) const { virtual boost::shared_ptr<GaussianFactor> linearize(const VALUES& x, const Ordering& ordering) const {
const T& xj = x[this->key_]; const T& xj = x[this->key_];
Matrix A; Matrix A;
Vector b = evaluateError(xj, A); Vector b = evaluateError(xj, A);

122
nonlinear/NonlinearFactor.h
View File

@ -51,19 +51,19 @@ namespace gtsam {
* more general than just vectors, e.g., Rot3 or Pose3, * more general than just vectors, e.g., Rot3 or Pose3,
* which are objects in non-linear manifolds (Lie groups). * which are objects in non-linear manifolds (Lie groups).
*/ */
template<class Values> template<class VALUES>
class NonlinearFactor: public Testable<NonlinearFactor<Values> > { class NonlinearFactor: public Testable<NonlinearFactor<VALUES> > {
protected: protected:
typedef NonlinearFactor<Values> This; typedef NonlinearFactor<VALUES> This;
SharedGaussian noiseModel_; /** Noise model */ SharedGaussian noiseModel_; /** Noise model */
std::list<Symbol> keys_; /** cached keys */ std::list<Symbol> keys_; /** cached keys */
public: public:
typedef boost::shared_ptr<NonlinearFactor<Values> > shared_ptr; typedef boost::shared_ptr<NonlinearFactor<VALUES> > shared_ptr;
/** Default constructor for I/O only */ /** Default constructor for I/O only */
NonlinearFactor() { NonlinearFactor() {
@ -84,7 +84,7 @@ namespace gtsam {
} }
/** Check if two NonlinearFactor objects are equal */ /** Check if two NonlinearFactor objects are equal */
bool equals(const NonlinearFactor<Values>& f, double tol = 1e-9) const { bool equals(const NonlinearFactor<VALUES>& f, double tol = 1e-9) const {
return noiseModel_->equals(*f.noiseModel_, tol); return noiseModel_->equals(*f.noiseModel_, tol);
} }
@ -92,7 +92,7 @@ namespace gtsam {
* calculate the error of the factor * calculate the error of the factor
* Override for systems with unusual noise models * Override for systems with unusual noise models
*/ */
virtual double error(const Values& c) const { virtual double error(const VALUES& c) const {
return 0.5 * noiseModel_->Mahalanobis(unwhitenedError(c)); return 0.5 * noiseModel_->Mahalanobis(unwhitenedError(c));
} }
@ -123,16 +123,16 @@ namespace gtsam {
} }
/** Vector of errors, unwhitened ! */ /** Vector of errors, unwhitened ! */
virtual Vector unwhitenedError(const Values& c) const = 0; virtual Vector unwhitenedError(const VALUES& c) const = 0;
/** Vector of errors, whitened ! */ /** Vector of errors, whitened ! */
Vector whitenedError(const Values& c) const { Vector whitenedError(const VALUES& c) const {
return noiseModel_->whiten(unwhitenedError(c)); return noiseModel_->whiten(unwhitenedError(c));
} }
/** linearize to a GaussianFactor */ /** linearize to a GaussianFactor */
virtual boost::shared_ptr<GaussianFactor> virtual boost::shared_ptr<GaussianFactor>
linearize(const Values& c, const Ordering& ordering) const = 0; linearize(const VALUES& c, const Ordering& ordering) const = 0;
/** /**
* Create a symbolic factor using the given ordering to determine the * Create a symbolic factor using the given ordering to determine the
@ -144,8 +144,8 @@ namespace gtsam {
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class ARCHIVE>
void serialize(Archive & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
// TODO NoiseModel // TODO NoiseModel
} }
@ -160,21 +160,21 @@ namespace gtsam {
* the derived class implements error_vector(c) = h(x)-z \approx Ax-b * the derived class implements error_vector(c) = h(x)-z \approx Ax-b
* This allows a graph to have factors with measurements of mixed type. * This allows a graph to have factors with measurements of mixed type.
*/ */
template<class Values, class Key> template<class VALUES, class KEY>
class NonlinearFactor1: public NonlinearFactor<Values> { class NonlinearFactor1: public NonlinearFactor<VALUES> {
public: public:
// typedefs for value types pulled from keys // typedefs for value types pulled from keys
typedef typename Key::Value X; typedef typename KEY::Value X;
protected: protected:
// The value of the key. Not const to allow serialization // The value of the key. Not const to allow serialization
Key key_; KEY key_;
typedef NonlinearFactor<Values> Base; typedef NonlinearFactor<VALUES> Base;
typedef NonlinearFactor1<Values, Key> This; typedef NonlinearFactor1<VALUES, KEY> This;
public: public:
@ -182,7 +182,7 @@ namespace gtsam {
NonlinearFactor1() { NonlinearFactor1() {
} }
inline const Key& key() const { inline const KEY& key() const {
return key_; return key_;
} }
@ -192,7 +192,7 @@ namespace gtsam {
* @param key by which to look up X value in Values * @param key by which to look up X value in Values
*/ */
NonlinearFactor1(const SharedGaussian& noiseModel, NonlinearFactor1(const SharedGaussian& noiseModel,
const Key& key1) : const KEY& key1) :
Base(noiseModel), key_(key1) { Base(noiseModel), key_(key1) {
this->keys_.push_back(key_); this->keys_.push_back(key_);
} }
@ -205,13 +205,13 @@ namespace gtsam {
} }
/** Check if two factors are equal. Note type is IndexFactor and needs cast. */ /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
bool equals(const NonlinearFactor1<Values,Key>& f, double tol = 1e-9) const { bool equals(const NonlinearFactor1<VALUES,KEY>& f, double tol = 1e-9) const {
return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key_ == f.key_); return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key_ == f.key_);
} }
/** error function h(x)-z, unwhitened !!! */ /** error function h(x)-z, unwhitened !!! */
inline Vector unwhitenedError(const Values& x) const { inline Vector unwhitenedError(const VALUES& x) const {
const Key& j = key_; const KEY& j = key_;
const X& xj = x[j]; const X& xj = x[j];
return evaluateError(xj); return evaluateError(xj);
} }
@ -221,7 +221,7 @@ namespace gtsam {
* Ax-b \approx h(x0+dx)-z = h(x0) + A*dx - z * Ax-b \approx h(x0+dx)-z = h(x0) + A*dx - z
* Hence b = z - h(x0) = - error_vector(x) * Hence b = z - h(x0) = - error_vector(x)
*/ */
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x, const Ordering& ordering) const { virtual boost::shared_ptr<GaussianFactor> linearize(const VALUES& x, const Ordering& ordering) const {
const X& xj = x[key_]; const X& xj = x[key_];
Matrix A; Matrix A;
Vector b = - evaluateError(xj, A); Vector b = - evaluateError(xj, A);
@ -258,8 +258,8 @@ namespace gtsam {
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class ARCHIVE>
void serialize(Archive & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor", ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<NonlinearFactor>(*this)); boost::serialization::base_object<NonlinearFactor>(*this));
ar & BOOST_SERIALIZATION_NVP(key_); ar & BOOST_SERIALIZATION_NVP(key_);
@ -270,23 +270,23 @@ namespace gtsam {
/** /**
* A Gaussian nonlinear factor that takes 2 parameters * A Gaussian nonlinear factor that takes 2 parameters
*/ */
template<class Values, class Key1, class Key2> template<class VALUES, class KEY1, class KEY2>
class NonlinearFactor2: public NonlinearFactor<Values> { class NonlinearFactor2: public NonlinearFactor<VALUES> {
public: public:
// typedefs for value types pulled from keys // typedefs for value types pulled from keys
typedef typename Key1::Value X1; typedef typename KEY1::Value X1;
typedef typename Key2::Value X2; typedef typename KEY2::Value X2;
protected: protected:
// The values of the keys. Not const to allow serialization // The values of the keys. Not const to allow serialization
Key1 key1_; KEY1 key1_;
Key2 key2_; KEY2 key2_;
typedef NonlinearFactor<Values> Base; typedef NonlinearFactor<VALUES> Base;
typedef NonlinearFactor2<Values, Key1, Key2> This; typedef NonlinearFactor2<VALUES, KEY1, KEY2> This;
public: public:
@ -301,8 +301,8 @@ namespace gtsam {
* @param j1 key of the first variable * @param j1 key of the first variable
* @param j2 key of the second variable * @param j2 key of the second variable
*/ */
NonlinearFactor2(const SharedGaussian& noiseModel, Key1 j1, NonlinearFactor2(const SharedGaussian& noiseModel, KEY1 j1,
Key2 j2) : KEY2 j2) :
Base(noiseModel), key1_(j1), key2_(j2) { Base(noiseModel), key1_(j1), key2_(j2) {
this->keys_.push_back(key1_); this->keys_.push_back(key1_);
this->keys_.push_back(key2_); this->keys_.push_back(key2_);
@ -317,13 +317,13 @@ namespace gtsam {
} }
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const NonlinearFactor2<Values,Key1,Key2>& f, double tol = 1e-9) const { bool equals(const NonlinearFactor2<VALUES,KEY1,KEY2>& f, double tol = 1e-9) const {
return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_) return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_)
&& (key2_ == f.key2_); && (key2_ == f.key2_);
} }
/** error function z-h(x1,x2) */ /** error function z-h(x1,x2) */
inline Vector unwhitenedError(const Values& x) const { inline Vector unwhitenedError(const VALUES& x) const {
const X1& x1 = x[key1_]; const X1& x1 = x[key1_];
const X2& x2 = x[key2_]; const X2& x2 = x[key2_];
return evaluateError(x1, x2); return evaluateError(x1, x2);
@ -334,7 +334,7 @@ namespace gtsam {
* Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z * Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z
* Hence b = z - h(x1,x2) = - error_vector(x) * Hence b = z - h(x1,x2) = - error_vector(x)
*/ */
boost::shared_ptr<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const { boost::shared_ptr<GaussianFactor> linearize(const VALUES& c, const Ordering& ordering) const {
const X1& x1 = c[key1_]; const X1& x1 = c[key1_];
const X2& x2 = c[key2_]; const X2& x2 = c[key2_];
Matrix A1, A2; Matrix A1, A2;
@ -371,10 +371,10 @@ namespace gtsam {
} }
/** methods to retrieve both keys */ /** methods to retrieve both keys */
inline const Key1& key1() const { inline const KEY1& key1() const {
return key1_; return key1_;
} }
inline const Key2& key2() const { inline const KEY2& key2() const {
return key2_; return key2_;
} }
@ -391,8 +391,8 @@ namespace gtsam {
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class ARCHIVE>
void serialize(Archive & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor", ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<NonlinearFactor>(*this)); boost::serialization::base_object<NonlinearFactor>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_); ar & BOOST_SERIALIZATION_NVP(key1_);
@ -406,25 +406,25 @@ namespace gtsam {
/** /**
* A Gaussian nonlinear factor that takes 3 parameters * A Gaussian nonlinear factor that takes 3 parameters
*/ */
template<class Values, class Key1, class Key2, class Key3> template<class VALUES, class KEY1, class KEY2, class KEY3>
class NonlinearFactor3: public NonlinearFactor<Values> { class NonlinearFactor3: public NonlinearFactor<VALUES> {
public: public:
// typedefs for value types pulled from keys // typedefs for value types pulled from keys
typedef typename Key1::Value X1; typedef typename KEY1::Value X1;
typedef typename Key2::Value X2; typedef typename KEY2::Value X2;
typedef typename Key3::Value X3; typedef typename KEY3::Value X3;
protected: protected:
// The values of the keys. Not const to allow serialization // The values of the keys. Not const to allow serialization
Key1 key1_; KEY1 key1_;
Key2 key2_; KEY2 key2_;
Key3 key3_; KEY3 key3_;
typedef NonlinearFactor<Values> Base; typedef NonlinearFactor<VALUES> Base;
typedef NonlinearFactor3<Values, Key1, Key2, Key3> This; typedef NonlinearFactor3<VALUES, KEY1, KEY2, KEY3> This;
public: public:
@ -440,7 +440,7 @@ namespace gtsam {
* @param j2 key of the second variable * @param j2 key of the second variable
* @param j3 key of the third variable * @param j3 key of the third variable
*/ */
NonlinearFactor3(const SharedGaussian& noiseModel, Key1 j1, Key2 j2, Key3 j3) : NonlinearFactor3(const SharedGaussian& noiseModel, KEY1 j1, KEY2 j2, KEY3 j3) :
Base(noiseModel), key1_(j1), key2_(j2), key3_(j3) { Base(noiseModel), key1_(j1), key2_(j2), key3_(j3) {
this->keys_.push_back(key1_); this->keys_.push_back(key1_);
this->keys_.push_back(key2_); this->keys_.push_back(key2_);
@ -457,13 +457,13 @@ namespace gtsam {
} }
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const NonlinearFactor3<Values,Key1,Key2,Key3>& f, double tol = 1e-9) const { bool equals(const NonlinearFactor3<VALUES,KEY1,KEY2,KEY3>& f, double tol = 1e-9) const {
return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_) return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_)
&& (key2_ == f.key2_) && (key3_ == f.key3_); && (key2_ == f.key2_) && (key3_ == f.key3_);
} }
/** error function z-h(x1,x2) */ /** error function z-h(x1,x2) */
inline Vector unwhitenedError(const Values& x) const { inline Vector unwhitenedError(const VALUES& x) const {
const X1& x1 = x[key1_]; const X1& x1 = x[key1_];
const X2& x2 = x[key2_]; const X2& x2 = x[key2_];
const X3& x3 = x[key3_]; const X3& x3 = x[key3_];
@ -475,7 +475,7 @@ namespace gtsam {
* Ax-b \approx h(x1+dx1,x2+dx2,x3+dx3)-z = h(x1,x2,x3) + A2*dx1 + A2*dx2 + A3*dx3 - z * Ax-b \approx h(x1+dx1,x2+dx2,x3+dx3)-z = h(x1,x2,x3) + A2*dx1 + A2*dx2 + A3*dx3 - z
* Hence b = z - h(x1,x2,x3) = - error_vector(x) * Hence b = z - h(x1,x2,x3) = - error_vector(x)
*/ */
boost::shared_ptr<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const { boost::shared_ptr<GaussianFactor> linearize(const VALUES& c, const Ordering& ordering) const {
const X1& x1 = c[key1_]; const X1& x1 = c[key1_];
const X2& x2 = c[key2_]; const X2& x2 = c[key2_];
const X3& x3 = c[key3_]; const X3& x3 = c[key3_];
@ -538,13 +538,13 @@ namespace gtsam {
} }
/** methods to retrieve keys */ /** methods to retrieve keys */
inline const Key1& key1() const { inline const KEY1& key1() const {
return key1_; return key1_;
} }
inline const Key2& key2() const { inline const KEY2& key2() const {
return key2_; return key2_;
} }
inline const Key3& key3() const { inline const KEY3& key3() const {
return key3_; return key3_;
} }
@ -563,8 +563,8 @@ namespace gtsam {
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class ARCHIVE>
void serialize(Archive & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor", ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<NonlinearFactor>(*this)); boost::serialization::base_object<NonlinearFactor>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_); ar & BOOST_SERIALIZATION_NVP(key1_);

32
nonlinear/NonlinearFactorGraph-inl.h
View File

@ -34,14 +34,14 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class Values> template<class VALUES>
void NonlinearFactorGraph<Values>::print(const std::string& str) const { void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
Base::print(str); Base::print(str);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Values> template<class VALUES>
Vector NonlinearFactorGraph<Values>::unwhitenedError(const Values& c) const { Vector NonlinearFactorGraph<VALUES>::unwhitenedError(const VALUES& c) const {
list<Vector> errors; list<Vector> errors;
BOOST_FOREACH(const sharedFactor& factor, this->factors_) BOOST_FOREACH(const sharedFactor& factor, this->factors_)
errors.push_back(factor->unwhitenedError(c)); errors.push_back(factor->unwhitenedError(c));
@ -49,8 +49,8 @@ void NonlinearFactorGraph<Values>::print(const std::string& str) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Values> template<class VALUES>
double NonlinearFactorGraph<Values>::error(const Values& c) const { double NonlinearFactorGraph<VALUES>::error(const VALUES& c) const {
double total_error = 0.; double total_error = 0.;
// iterate over all the factors_ to accumulate the log probabilities // iterate over all the factors_ to accumulate the log probabilities
BOOST_FOREACH(const sharedFactor& factor, this->factors_) BOOST_FOREACH(const sharedFactor& factor, this->factors_)
@ -59,8 +59,8 @@ void NonlinearFactorGraph<Values>::print(const std::string& str) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Values> template<class VALUES>
Ordering::shared_ptr NonlinearFactorGraph<Values>::orderingCOLAMD(const Values& config) const { Ordering::shared_ptr NonlinearFactorGraph<VALUES>::orderingCOLAMD(const VALUES& config) const {
// Create symbolic graph and initial (iterator) ordering // Create symbolic graph and initial (iterator) ordering
SymbolicFactorGraph::shared_ptr symbolic; SymbolicFactorGraph::shared_ptr symbolic;
@ -84,9 +84,9 @@ void NonlinearFactorGraph<Values>::print(const std::string& str) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Values> template<class VALUES>
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph<Values>::symbolic( SymbolicFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::symbolic(
const Values& config, const Ordering& ordering) const { const VALUES& config, const Ordering& ordering) const {
// Generate the symbolic factor graph // Generate the symbolic factor graph
SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph); SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
symbolicfg->reserve(this->size()); symbolicfg->reserve(this->size());
@ -97,18 +97,18 @@ void NonlinearFactorGraph<Values>::print(const std::string& str) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Values> template<class VALUES>
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr>
NonlinearFactorGraph<Values>::symbolic(const Values& config) const { NonlinearFactorGraph<VALUES>::symbolic(const VALUES& config) const {
// Generate an initial key ordering in iterator order // Generate an initial key ordering in iterator order
Ordering::shared_ptr ordering(config.orderingArbitrary()); Ordering::shared_ptr ordering(config.orderingArbitrary());
return make_pair(symbolic(config, *ordering), ordering); return make_pair(symbolic(config, *ordering), ordering);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Values> template<class VALUES>
boost::shared_ptr<GaussianFactorGraph> NonlinearFactorGraph<Values>::linearize( boost::shared_ptr<GaussianFactorGraph> NonlinearFactorGraph<VALUES>::linearize(
const Values& config, const Ordering& ordering) const { const VALUES& config, const Ordering& ordering) const {
// create an empty linear FG // create an empty linear FG
GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph); GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);

22
nonlinear/NonlinearFactorGraph.h
View File

@ -38,25 +38,25 @@ namespace gtsam {
* tangent vector space at the linearization point. Because the tangent space is a true * tangent vector space at the linearization point. Because the tangent space is a true
* vector space, the config type will be an VectorValues in that linearized factor graph. * vector space, the config type will be an VectorValues in that linearized factor graph.
*/ */
template<class Values> template<class VALUES>
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<Values> > { class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<VALUES> > {
public: public:
typedef FactorGraph<NonlinearFactor<Values> > Base; typedef FactorGraph<NonlinearFactor<VALUES> > Base;
typedef typename boost::shared_ptr<NonlinearFactor<Values> > sharedFactor; typedef typename boost::shared_ptr<NonlinearFactor<VALUES> > sharedFactor;
/** print just calls base class */ /** print just calls base class */
void print(const std::string& str = "NonlinearFactorGraph: ") const; void print(const std::string& str = "NonlinearFactorGraph: ") const;
/** unnormalized error */ /** unnormalized error */
double error(const Values& c) const; double error(const VALUES& c) const;
/** all individual errors */ /** all individual errors */
Vector unwhitenedError(const Values& c) const; Vector unwhitenedError(const VALUES& c) const;
/** Unnormalized probability. O(n) */ /** Unnormalized probability. O(n) */
double probPrime(const Values& c) const { double probPrime(const VALUES& c) const {
return exp(-0.5 * error(c)); return exp(-0.5 * error(c));
} }
@ -68,7 +68,7 @@ namespace gtsam {
/** /**
* Create a symbolic factor graph using an existing ordering * Create a symbolic factor graph using an existing ordering
*/ */
SymbolicFactorGraph::shared_ptr symbolic(const Values& config, const Ordering& ordering) const; SymbolicFactorGraph::shared_ptr symbolic(const VALUES& config, const Ordering& ordering) const;
/** /**
* Create a symbolic factor graph and initial variable ordering that can * Create a symbolic factor graph and initial variable ordering that can
@ -77,20 +77,20 @@ namespace gtsam {
* ordering is found. * ordering is found.
*/ */
std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr>
symbolic(const Values& config) const; symbolic(const VALUES& config) const;
/** /**
* Compute a fill-reducing ordering using COLAMD. This returns the * Compute a fill-reducing ordering using COLAMD. This returns the
* ordering and a VariableIndex, which can later be re-used to save * ordering and a VariableIndex, which can later be re-used to save
* computation. * computation.
*/ */
Ordering::shared_ptr orderingCOLAMD(const Values& config) const; Ordering::shared_ptr orderingCOLAMD(const VALUES& config) const;
/** /**
* linearize a nonlinear factor graph * linearize a nonlinear factor graph
*/ */
boost::shared_ptr<GaussianFactorGraph> boost::shared_ptr<GaussianFactorGraph>
linearize(const Values& config, const Ordering& ordering) const; linearize(const VALUES& config, const Ordering& ordering) const;
}; };