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
*/
template <class NonlinearGraph, class Values>
template <class NONLINEARGRAPH, class VALUES>
class Factorization {
private:
boost::shared_ptr<Ordering> ordering_;
@ -53,7 +53,7 @@ namespace gtsam {
/**
* 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_);
}
@ -65,7 +65,7 @@ namespace gtsam {
}
/** 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_);
}
};

10
linear/GaussianConditional.h
View File

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

6
linear/GaussianFactor.h
View File

@ -49,7 +49,7 @@
namespace gtsam {
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 */
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
* variables. */
template<class Storage>
template<class STORAGE>
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);
/**

46
linear/GaussianFactorGraph.h
View File

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

12
linear/GaussianISAM.h
View File

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

22
linear/SubgraphSolver-inl.h
View File

@ -30,21 +30,21 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
template<class Graph, class Values>
SubgraphSolver<Graph, Values>::SubgraphSolver(const Graph& G, const Values& theta0) {
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) {
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);
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_) ;
gtsam::split<GRAPH,Key,Constraint>(G, tree, T_, C_) ;
if (verbose_) cout << ",with " << T_.size() << " and " << C_.size() << " factors" << endl;
// make the ordering
@ -56,12 +56,12 @@ namespace gtsam {
T_.addHardConstraint(root, theta0[root]);
// 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>
boost::shared_ptr<SubgraphPreconditioner> SubgraphSolver<Graph, Values>::linearize(const Graph& G, const Values& theta_bar) const {
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
@ -78,8 +78,8 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class Graph, class Values>
VectorValues SubgraphSolver<Graph, Values>::optimize(SubgraphPreconditioner& system) const {
template<class GRAPH, class VALUES>
VectorValues SubgraphSolver<GRAPH, VALUES>::optimize(SubgraphPreconditioner& system) const {
VectorValues zeros = system.zero();
// Solve the subgraph PCG

30
linear/SubgraphSolver.h
View File

@ -30,13 +30,13 @@ namespace gtsam {
* linearize: G * T -> L
* solve : L -> VectorValues
*/
template<class Graph, class Values>
template<class GRAPH, class VALUES>
class SubgraphSolver {
private:
typedef typename Values::Key Key;
typedef typename Graph::Constraint Constraint;
typedef typename Graph::Pose Pose;
typedef typename VALUES::Key Key;
typedef typename GRAPH::Constraint Constraint;
typedef typename GRAPH::Pose Pose;
// TODO not hardcode
static const size_t maxIterations_=100;
@ -47,25 +47,25 @@ namespace gtsam {
boost::shared_ptr<Ordering> ordering_;
/* 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:
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<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
*/
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 */
Values expmap(const Values& config, const VectorValues& delta) const {
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 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_;
} // nsamespace gtsam

4
linear/VectorBTree.h
View File

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

4
linear/VectorMap.h
View File

@ -145,8 +145,8 @@ namespace gtsam {
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version)
{
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_) {}
/** Construct from a container of variable dimensions (in variable order). */
template<class Container>
VectorValues(const Container& dimensions);
template<class CONTAINER>
VectorValues(const CONTAINER& dimensions);
/** Construct to hold nVars vectors of varDim dimension each. */
VectorValues(Index nVars, size_t varDim);
@ -213,8 +213,8 @@ public:
// values_.resize(varStarts_.back(), false);
//}
template<class Container>
inline VectorValues::VectorValues(const Container& dimensions) : varStarts_(dimensions.size()+1) {
template<class CONTAINER>
inline VectorValues::VectorValues(const CONTAINER& dimensions) : varStarts_(dimensions.size()+1) {
varStarts_[0] = 0;
size_t varStart = 0;
Index var = 0;

18
nonlinear/Key.h
View File

@ -79,8 +79,8 @@ namespace gtsam {
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(j_);
}
};
@ -171,8 +171,8 @@ namespace gtsam {
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(c_);
ar & BOOST_SERIALIZATION_NVP(j_);
}
@ -180,13 +180,13 @@ namespace gtsam {
// Conversion utilities
template<class Key> Symbol key2symbol(Key key) {
template<class KEY> Symbol key2symbol(KEY 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::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;
}
@ -281,8 +281,8 @@ namespace gtsam {
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
typedef TypedSymbol<T,C> Base;
ar & boost::serialization::make_nvp("TypedLabeledSymbol",
boost::serialization::base_object<Base>(*this));

8
nonlinear/LieValues.h
View File

@ -72,8 +72,8 @@ namespace gtsam {
LieValues() {}
LieValues(const LieValues& config) :
values_(config.values_) {}
template<class J_alt>
LieValues(const LieValues<J_alt>& other) {} // do nothing when initializing with wrong type
template<class J_ALT>
LieValues(const LieValues<J_ALT>& other) {} // do nothing when initializing with wrong type
virtual ~LieValues() {}
/** print */
@ -197,8 +197,8 @@ namespace gtsam {
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
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
* - ONLY_EXACT : throws error at linearization if not at exact feasible point, and infinite error
*/
template<class Values, class Key>
class NonlinearEquality: public NonlinearFactor1<Values, Key> {
template<class VALUES, class KEY>
class NonlinearEquality: public NonlinearFactor1<VALUES, KEY> {
public:
typedef typename Key::Value T;
typedef typename KEY::Value T;
private:
@ -65,12 +65,12 @@ namespace gtsam {
*/
bool (*compare_)(const T& a, const T& b);
typedef NonlinearFactor1<Values, Key> Base;
typedef NonlinearFactor1<VALUES, KEY> Base;
/**
* 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),
allow_error_(false), error_gain_(std::numeric_limits<double>::infinity()),
compare_(compare) {
@ -79,7 +79,7 @@ namespace gtsam {
/**
* 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),
allow_error_(true), error_gain_(error_gain),
compare_(compare) {
@ -92,13 +92,13 @@ namespace gtsam {
}
/** 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;
return compare_(feasible_, f.feasible_);
}
/** actual error function calculation */
virtual double error(const Values& c) const {
virtual double error(const VALUES& c) const {
const T& xj = c[this->key_];
Vector e = this->unwhitenedError(c);
if (allow_error_ || !compare_(xj, feasible_)) {
@ -125,7 +125,7 @@ namespace gtsam {
}
// 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_];
Matrix 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,
* which are objects in non-linear manifolds (Lie groups).
*/
template<class Values>
class NonlinearFactor: public Testable<NonlinearFactor<Values> > {
template<class VALUES>
class NonlinearFactor: public Testable<NonlinearFactor<VALUES> > {
protected:
typedef NonlinearFactor<Values> This;
typedef NonlinearFactor<VALUES> This;
SharedGaussian noiseModel_; /** Noise model */
std::list<Symbol> keys_; /** cached keys */
public:
typedef boost::shared_ptr<NonlinearFactor<Values> > shared_ptr;
typedef boost::shared_ptr<NonlinearFactor<VALUES> > shared_ptr;
/** Default constructor for I/O only */
NonlinearFactor() {
@ -84,7 +84,7 @@ namespace gtsam {
}
/** 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);
}
@ -92,7 +92,7 @@ namespace gtsam {
* calculate the error of the factor
* 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));
}
@ -123,16 +123,16 @@ namespace gtsam {
}
/** 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 whitenedError(const Values& c) const {
Vector whitenedError(const VALUES& c) const {
return noiseModel_->whiten(unwhitenedError(c));
}
/** linearize to a 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
@ -144,8 +144,8 @@ namespace gtsam {
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
// TODO NoiseModel
}
@ -160,21 +160,21 @@ namespace gtsam {
* 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.
*/
template<class Values, class Key>
class NonlinearFactor1: public NonlinearFactor<Values> {
template<class VALUES, class KEY>
class NonlinearFactor1: public NonlinearFactor<VALUES> {
public:
// typedefs for value types pulled from keys
typedef typename Key::Value X;
typedef typename KEY::Value X;
protected:
// The value of the key. Not const to allow serialization
Key key_;
KEY key_;
typedef NonlinearFactor<Values> Base;
typedef NonlinearFactor1<Values, Key> This;
typedef NonlinearFactor<VALUES> Base;
typedef NonlinearFactor1<VALUES, KEY> This;
public:
@ -182,7 +182,7 @@ namespace gtsam {
NonlinearFactor1() {
}
inline const Key& key() const {
inline const KEY& key() const {
return key_;
}
@ -192,7 +192,7 @@ namespace gtsam {
* @param key by which to look up X value in Values
*/
NonlinearFactor1(const SharedGaussian& noiseModel,
const Key& key1) :
const KEY& key1) :
Base(noiseModel), key_(key1) {
this->keys_.push_back(key_);
}
@ -205,13 +205,13 @@ namespace gtsam {
}
/** 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_);
}
/** error function h(x)-z, unwhitened !!! */
inline Vector unwhitenedError(const Values& x) const {
const Key& j = key_;
inline Vector unwhitenedError(const VALUES& x) const {
const KEY& j = key_;
const X& xj = x[j];
return evaluateError(xj);
}
@ -221,7 +221,7 @@ namespace gtsam {
* Ax-b \approx h(x0+dx)-z = h(x0) + A*dx - z
* 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_];
Matrix A;
Vector b = - evaluateError(xj, A);
@ -258,8 +258,8 @@ namespace gtsam {
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<NonlinearFactor>(*this));
ar & BOOST_SERIALIZATION_NVP(key_);
@ -270,23 +270,23 @@ namespace gtsam {
/**
* A Gaussian nonlinear factor that takes 2 parameters
*/
template<class Values, class Key1, class Key2>
class NonlinearFactor2: public NonlinearFactor<Values> {
template<class VALUES, class KEY1, class KEY2>
class NonlinearFactor2: public NonlinearFactor<VALUES> {
public:
// typedefs for value types pulled from keys
typedef typename Key1::Value X1;
typedef typename Key2::Value X2;
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
protected:
// The values of the keys. Not const to allow serialization
Key1 key1_;
Key2 key2_;
KEY1 key1_;
KEY2 key2_;
typedef NonlinearFactor<Values> Base;
typedef NonlinearFactor2<Values, Key1, Key2> This;
typedef NonlinearFactor<VALUES> Base;
typedef NonlinearFactor2<VALUES, KEY1, KEY2> This;
public:
@ -301,8 +301,8 @@ namespace gtsam {
* @param j1 key of the first variable
* @param j2 key of the second variable
*/
NonlinearFactor2(const SharedGaussian& noiseModel, Key1 j1,
Key2 j2) :
NonlinearFactor2(const SharedGaussian& noiseModel, KEY1 j1,
KEY2 j2) :
Base(noiseModel), key1_(j1), key2_(j2) {
this->keys_.push_back(key1_);
this->keys_.push_back(key2_);
@ -317,13 +317,13 @@ namespace gtsam {
}
/** 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_)
&& (key2_ == f.key2_);
}
/** 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 X2& x2 = x[key2_];
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
* 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 X2& x2 = c[key2_];
Matrix A1, A2;
@ -371,10 +371,10 @@ namespace gtsam {
}
/** methods to retrieve both keys */
inline const Key1& key1() const {
inline const KEY1& key1() const {
return key1_;
}
inline const Key2& key2() const {
inline const KEY2& key2() const {
return key2_;
}
@ -391,8 +391,8 @@ namespace gtsam {
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<NonlinearFactor>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
@ -406,25 +406,25 @@ namespace gtsam {
/**
* A Gaussian nonlinear factor that takes 3 parameters
*/
template<class Values, class Key1, class Key2, class Key3>
class NonlinearFactor3: public NonlinearFactor<Values> {
template<class VALUES, class KEY1, class KEY2, class KEY3>
class NonlinearFactor3: public NonlinearFactor<VALUES> {
public:
// typedefs for value types pulled from keys
typedef typename Key1::Value X1;
typedef typename Key2::Value X2;
typedef typename Key3::Value X3;
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef typename KEY3::Value X3;
protected:
// The values of the keys. Not const to allow serialization
Key1 key1_;
Key2 key2_;
Key3 key3_;
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
typedef NonlinearFactor<Values> Base;
typedef NonlinearFactor3<Values, Key1, Key2, Key3> This;
typedef NonlinearFactor<VALUES> Base;
typedef NonlinearFactor3<VALUES, KEY1, KEY2, KEY3> This;
public:
@ -440,7 +440,7 @@ namespace gtsam {
* @param j2 key of the second 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) {
this->keys_.push_back(key1_);
this->keys_.push_back(key2_);
@ -457,13 +457,13 @@ namespace gtsam {
}
/** 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_)
&& (key2_ == f.key2_) && (key3_ == f.key3_);
}
/** 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 X2& x2 = x[key2_];
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
* 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 X2& x2 = c[key2_];
const X3& x3 = c[key3_];
@ -538,13 +538,13 @@ namespace gtsam {
}
/** methods to retrieve keys */
inline const Key1& key1() const {
inline const KEY1& key1() const {
return key1_;
}
inline const Key2& key2() const {
inline const KEY2& key2() const {
return key2_;
}
inline const Key3& key3() const {
inline const KEY3& key3() const {
return key3_;
}
@ -563,8 +563,8 @@ namespace gtsam {
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<NonlinearFactor>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);

32
nonlinear/NonlinearFactorGraph-inl.h
View File

@ -34,14 +34,14 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
template<class Values>
void NonlinearFactorGraph<Values>::print(const std::string& str) const {
template<class VALUES>
void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
Base::print(str);
}
/* ************************************************************************* */
template<class Values>
Vector NonlinearFactorGraph<Values>::unwhitenedError(const Values& c) const {
template<class VALUES>
Vector NonlinearFactorGraph<VALUES>::unwhitenedError(const VALUES& c) const {
list<Vector> errors;
BOOST_FOREACH(const sharedFactor& factor, this->factors_)
errors.push_back(factor->unwhitenedError(c));
@ -49,8 +49,8 @@ void NonlinearFactorGraph<Values>::print(const std::string& str) const {
}
/* ************************************************************************* */
template<class Values>
double NonlinearFactorGraph<Values>::error(const Values& c) const {
template<class VALUES>
double NonlinearFactorGraph<VALUES>::error(const VALUES& c) const {
double total_error = 0.;
// iterate over all the factors_ to accumulate the log probabilities
BOOST_FOREACH(const sharedFactor& factor, this->factors_)
@ -59,8 +59,8 @@ void NonlinearFactorGraph<Values>::print(const std::string& str) const {
}
/* ************************************************************************* */
template<class Values>
Ordering::shared_ptr NonlinearFactorGraph<Values>::orderingCOLAMD(const Values& config) const {
template<class VALUES>
Ordering::shared_ptr NonlinearFactorGraph<VALUES>::orderingCOLAMD(const VALUES& config) const {
// Create symbolic graph and initial (iterator) ordering
SymbolicFactorGraph::shared_ptr symbolic;
@ -84,9 +84,9 @@ void NonlinearFactorGraph<Values>::print(const std::string& str) const {
}
/* ************************************************************************* */
template<class Values>
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph<Values>::symbolic(
const Values& config, const Ordering& ordering) const {
template<class VALUES>
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::symbolic(
const VALUES& config, const Ordering& ordering) const {
// Generate the symbolic factor graph
SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
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>
NonlinearFactorGraph<Values>::symbolic(const Values& config) const {
NonlinearFactorGraph<VALUES>::symbolic(const VALUES& config) const {
// Generate an initial key ordering in iterator order
Ordering::shared_ptr ordering(config.orderingArbitrary());
return make_pair(symbolic(config, *ordering), ordering);
}
/* ************************************************************************* */
template<class Values>
boost::shared_ptr<GaussianFactorGraph> NonlinearFactorGraph<Values>::linearize(
const Values& config, const Ordering& ordering) const {
template<class VALUES>
boost::shared_ptr<GaussianFactorGraph> NonlinearFactorGraph<VALUES>::linearize(
const VALUES& config, const Ordering& ordering) const {
// create an empty linear FG
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
* vector space, the config type will be an VectorValues in that linearized factor graph.
*/
template<class Values>
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<Values> > {
template<class VALUES>
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<VALUES> > {
public:
typedef FactorGraph<NonlinearFactor<Values> > Base;
typedef typename boost::shared_ptr<NonlinearFactor<Values> > sharedFactor;
typedef FactorGraph<NonlinearFactor<VALUES> > Base;
typedef typename boost::shared_ptr<NonlinearFactor<VALUES> > sharedFactor;
/** print just calls base class */
void print(const std::string& str = "NonlinearFactorGraph: ") const;
/** unnormalized error */
double error(const Values& c) const;
double error(const VALUES& c) const;
/** all individual errors */
Vector unwhitenedError(const Values& c) const;
Vector unwhitenedError(const VALUES& c) const;
/** Unnormalized probability. O(n) */
double probPrime(const Values& c) const {
double probPrime(const VALUES& c) const {
return exp(-0.5 * error(c));
}
@ -68,7 +68,7 @@ namespace gtsam {
/**
* 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
@ -77,20 +77,20 @@ namespace gtsam {
* ordering is found.
*/
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
* ordering and a VariableIndex, which can later be re-used to save
* computation.
*/
Ordering::shared_ptr orderingCOLAMD(const Values& config) const;
Ordering::shared_ptr orderingCOLAMD(const VALUES& config) const;
/**
* linearize a nonlinear factor graph
*/
boost::shared_ptr<GaussianFactorGraph>
linearize(const Values& config, const Ordering& ordering) const;
linearize(const VALUES& config, const Ordering& ordering) const;
};