From 82a2f29e4894845644d44d435977f3b58becc0f7 Mon Sep 17 00:00:00 2001 From: Rahul Sawhney Date: Wed, 20 Oct 2010 16:37:47 +0000 Subject: [PATCH] Template arguments capitalized --- linear/Factorization.h | 6 +- linear/GaussianConditional.h | 10 +-- linear/GaussianFactor.cpp | 4 +- linear/GaussianFactor.h | 6 +- linear/GaussianFactorGraph.h | 46 +++++----- linear/GaussianISAM.h | 12 +-- linear/SubgraphSolver-inl.h | 22 ++--- linear/SubgraphSolver.h | 30 +++---- linear/VectorBTree.h | 4 +- linear/VectorMap.h | 4 +- linear/VectorValues.h | 8 +- nonlinear/Key.h | 18 ++-- nonlinear/LieValues.h | 8 +- nonlinear/NonlinearEquality.h | 18 ++-- nonlinear/NonlinearFactor.h | 122 +++++++++++++-------------- nonlinear/NonlinearFactorGraph-inl.h | 32 +++---- nonlinear/NonlinearFactorGraph.h | 22 ++--- 17 files changed, 186 insertions(+), 186 deletions(-) diff --git a/linear/Factorization.h b/linear/Factorization.h index 86f2c3f34..acf8bb29f 100644 --- a/linear/Factorization.h +++ b/linear/Factorization.h @@ -31,7 +31,7 @@ namespace gtsam { /** * A linear system solver using factorization */ - template + template class Factorization { private: boost::shared_ptr ordering_; @@ -53,7 +53,7 @@ namespace gtsam { /** * linearize the non-linear graph around the current config */ - boost::shared_ptr linearize(const NonlinearGraph& g, const Values& config) const { + boost::shared_ptr 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_); } }; diff --git a/linear/GaussianConditional.h b/linear/GaussianConditional.h index 12338e03a..594bc5a83 100644 --- a/linear/GaussianConditional.h +++ b/linear/GaussianConditional.h @@ -100,8 +100,8 @@ public: * Constructor when matrices are already stored in a combined matrix, allows * for multiple frontal variables. */ - template - GaussianConditional(Iterator firstKey, Iterator lastKey, size_t nrFrontals, const VerticalBlockView& matrices, const Vector& sigmas); + template + GaussianConditional(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals, const VerticalBlockView& matrices, const Vector& sigmas); /** print */ void print(const std::string& = "GaussianConditional") const; @@ -162,9 +162,9 @@ private: }; /* ************************************************************************* */ - template - GaussianConditional::GaussianConditional(Iterator firstKey, Iterator lastKey, - size_t nrFrontals, const VerticalBlockView& matrices, + template + GaussianConditional::GaussianConditional(ITERATOR firstKey, ITERATOR lastKey, + size_t nrFrontals, const VerticalBlockView& matrices, const Vector& sigmas) : rsd_(matrix_), sigmas_(sigmas) { nrFrontals_ = nrFrontals; diff --git a/linear/GaussianFactor.cpp b/linear/GaussianFactor.cpp index c9bdcd4f0..b9aada8f9 100644 --- a/linear/GaussianFactor.cpp +++ b/linear/GaussianFactor.cpp @@ -593,9 +593,9 @@ struct _RowSource { template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph&, const GaussianVariableIndex&, const vector&, const vector&, const std::vector >&); template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph&, const GaussianVariableIndex&, const vector&, const vector&, const std::vector >&); -template +template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph& factorGraph, - const GaussianVariableIndex& variableIndex, const vector& factors, + const GaussianVariableIndex& variableIndex, const vector& factors, const vector& variables, const std::vector >& variablePositions) { shared_ptr ret(boost::make_shared()); diff --git a/linear/GaussianFactor.h b/linear/GaussianFactor.h index 5997f8b9f..c97819432 100644 --- a/linear/GaussianFactor.h +++ b/linear/GaussianFactor.h @@ -49,7 +49,7 @@ namespace gtsam { class GaussianFactorGraph; -template class GaussianVariableIndex; +template class GaussianVariableIndex; /** A map from key to dimension, useful in various contexts */ typedef std::map Dimensions; @@ -149,9 +149,9 @@ public: /** Named constructor for combining a set of factors with pre-computed set of * variables. */ - template + template static shared_ptr Combine(const FactorGraph& factorGraph, - const GaussianVariableIndex& variableIndex, const std::vector& factors, + const GaussianVariableIndex& variableIndex, const std::vector& factors, const std::vector& variables, const std::vector >& variablePositions); /** diff --git a/linear/GaussianFactorGraph.h b/linear/GaussianFactorGraph.h index 256c7b453..9697d8cf3 100644 --- a/linear/GaussianFactorGraph.h +++ b/linear/GaussianFactorGraph.h @@ -157,11 +157,11 @@ namespace gtsam { /* ************************************************************************* */ - template - class GaussianVariableIndex : public VariableIndex { + template + class GaussianVariableIndex : public VariableIndex { public: - typedef VariableIndex Base; - typedef typename VariableIndexStorage::template type_factory::type storage_type; + typedef VariableIndex Base; + typedef typename VARIABLEINDEXSTORAGE::template type_factory::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& variableIndex, const GaussianFactorGraph& factorGraph); + GaussianVariableIndex(const VariableIndex& variableIndex, const GaussianFactorGraph& factorGraph); /** * Another constructor to upgrade from the base class using an existing * array of variable dimensions. */ - GaussianVariableIndex(const VariableIndex& variableIndex, const storage_type& dimensions); + GaussianVariableIndex(const VariableIndex& 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 - GaussianVariableIndex::GaussianVariableIndex(const GaussianFactorGraph& factorGraph) : + template + GaussianVariableIndex::GaussianVariableIndex(const GaussianFactorGraph& factorGraph) : Base(factorGraph), dims_(Base::index_.size()) { fillDims(factorGraph); } /* ************************************************************************* */ - template - GaussianVariableIndex::GaussianVariableIndex( - const VariableIndex& variableIndex, const GaussianFactorGraph& factorGraph) : + template + GaussianVariableIndex::GaussianVariableIndex( + const VariableIndex& variableIndex, const GaussianFactorGraph& factorGraph) : Base(variableIndex), dims_(Base::index_.size()) { fillDims(factorGraph); } /* ************************************************************************* */ - template - GaussianVariableIndex::GaussianVariableIndex( - const VariableIndex& variableIndex, const storage_type& dimensions) : + template + GaussianVariableIndex::GaussianVariableIndex( + const VariableIndex& variableIndex, const storage_type& dimensions) : Base(variableIndex), dims_(dimensions) { assert(Base::index_.size() == dims_.size()); } /* ************************************************************************* */ - template - void GaussianVariableIndex::fillDims(const GaussianFactorGraph& factorGraph) { + template + void GaussianVariableIndex::fillDims(const GaussianFactorGraph& factorGraph) { // Store dimensions of each variable assert(dims_.size() == Base::index_.size()); for(Index var=0; var - void GaussianVariableIndex::permute(const Permutation& permutation) { - VariableIndex::permute(permutation); + template + void GaussianVariableIndex::permute(const Permutation& permutation) { + VariableIndex::permute(permutation); storage_type original(this->dims_.size()); this->dims_.swap(original); for(Index j=0; j - void GaussianVariableIndex::augment(const GaussianFactorGraph& factorGraph) { + template + void GaussianVariableIndex::augment(const GaussianFactorGraph& factorGraph) { Base::augment(factorGraph); dims_.resize(Base::index_.size(), 0); BOOST_FOREACH(boost::shared_ptr 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 +// template // std::pair combineFactorsAndCreateMatrix( -// const Factors& factors, +// const FACTORS& factors, // const Ordering& order, const Dimensions& dimensions); } // namespace gtsam diff --git a/linear/GaussianISAM.h b/linear/GaussianISAM.h index 343964e2f..5e2ab7488 100644 --- a/linear/GaussianISAM.h +++ b/linear/GaussianISAM.h @@ -38,14 +38,14 @@ public: GaussianISAM(const GaussianBayesNet& bayesNet) : ISAM(bayesNet) {} /** Override update_internal to also keep track of variable dimensions. */ - template - void update_internal(const FactorGraph& newFactors, Cliques& orphans) { + template + void update_internal(const FACTORGRAPH& newFactors, Cliques& orphans) { ISAM::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 - void update(const FactorGraph& newFactors) { + template + void update(const FACTORGRAPH& newFactors) { Cliques orphans; this->update_internal(newFactors, orphans); } diff --git a/linear/SubgraphSolver-inl.h b/linear/SubgraphSolver-inl.h index 86ebc2ad2..4d101de86 100644 --- a/linear/SubgraphSolver-inl.h +++ b/linear/SubgraphSolver-inl.h @@ -30,21 +30,21 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - template - SubgraphSolver::SubgraphSolver(const Graph& G, const Values& theta0) { + template + SubgraphSolver::SubgraphSolver(const GRAPH& G, const VALUES& theta0) { initialize(G,theta0); } /* ************************************************************************* */ - template - void SubgraphSolver::initialize(const Graph& G, const Values& theta0) { + template + void SubgraphSolver::initialize(const GRAPH& G, const VALUES& theta0) { // generate spanning tree - PredecessorMap tree = gtsam::findMinimumSpanningTree(G); + PredecessorMap tree = gtsam::findMinimumSpanningTree(G); // split the graph if (verbose_) cout << "generating spanning tree and split the graph ..."; - gtsam::split(G, tree, T_, C_) ; + gtsam::split(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 (T_, tree, theta0[root]); + theta_bar_ = composePoses (T_, tree, theta0[root]); } /* ************************************************************************* */ - template - boost::shared_ptr SubgraphSolver::linearize(const Graph& G, const Values& theta_bar) const { + template + boost::shared_ptr SubgraphSolver::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 - VectorValues SubgraphSolver::optimize(SubgraphPreconditioner& system) const { + template + VectorValues SubgraphSolver::optimize(SubgraphPreconditioner& system) const { VectorValues zeros = system.zero(); // Solve the subgraph PCG diff --git a/linear/SubgraphSolver.h b/linear/SubgraphSolver.h index 118af4b99..74bcffd3d 100644 --- a/linear/SubgraphSolver.h +++ b/linear/SubgraphSolver.h @@ -30,13 +30,13 @@ namespace gtsam { * linearize: G * T -> L * solve : L -> VectorValues */ - template + template 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_; /* the solution computed from the first subgraph */ - boost::shared_ptr theta_bar_; + boost::shared_ptr 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() const { return ordering_; } - boost::shared_ptr theta_bar() const { return theta_bar_; } + boost::shared_ptr theta_bar() const { return theta_bar_; } /** * linearize the non-linear graph around the current config and build the subgraph preconditioner systme */ - boost::shared_ptr linearize(const Graph& G, const Values& theta_bar) const; + boost::shared_ptr 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 const size_t SubgraphSolver::maxIterations_; - template const bool SubgraphSolver::verbose_; - template const double SubgraphSolver::epsilon_; - template const double SubgraphSolver::epsilon_abs_; + template const size_t SubgraphSolver::maxIterations_; + template const bool SubgraphSolver::verbose_; + template const double SubgraphSolver::epsilon_; + template const double SubgraphSolver::epsilon_abs_; } // nsamespace gtsam diff --git a/linear/VectorBTree.h b/linear/VectorBTree.h index 3f5ef486e..57bcbe904 100644 --- a/linear/VectorBTree.h +++ b/linear/VectorBTree.h @@ -234,8 +234,8 @@ public: private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) + template + void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(values); } diff --git a/linear/VectorMap.h b/linear/VectorMap.h index b059cb340..c251cd064 100644 --- a/linear/VectorMap.h +++ b/linear/VectorMap.h @@ -145,8 +145,8 @@ namespace gtsam { private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) + template + void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(values); } diff --git a/linear/VectorValues.h b/linear/VectorValues.h index 3e1bdc086..8c9a9ce3e 100644 --- a/linear/VectorValues.h +++ b/linear/VectorValues.h @@ -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 - VectorValues(const Container& dimensions); + template + 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 -inline VectorValues::VectorValues(const Container& dimensions) : varStarts_(dimensions.size()+1) { +template +inline VectorValues::VectorValues(const CONTAINER& dimensions) : varStarts_(dimensions.size()+1) { varStarts_[0] = 0; size_t varStart = 0; Index var = 0; diff --git a/nonlinear/Key.h b/nonlinear/Key.h index 121598658..1b7345c89 100644 --- a/nonlinear/Key.h +++ b/nonlinear/Key.h @@ -79,8 +79,8 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { + template + 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 - void serialize(Archive & ar, const unsigned int version) { + template + 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 Symbol key2symbol(Key key) { + template Symbol key2symbol(KEY key) { return Symbol(key); } - template std::list keys2symbols(std::list keys) { + template std::list keys2symbols(std::list keys) { std::list symbols; - std::transform(keys.begin(), keys.end(), std::back_inserter(symbols), key2symbol ); + std::transform(keys.begin(), keys.end(), std::back_inserter(symbols), key2symbol ); return symbols; } @@ -281,8 +281,8 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { + template + void serialize(ARCHIVE & ar, const unsigned int version) { typedef TypedSymbol Base; ar & boost::serialization::make_nvp("TypedLabeledSymbol", boost::serialization::base_object(*this)); diff --git a/nonlinear/LieValues.h b/nonlinear/LieValues.h index c2cbc48a6..74c3eee8a 100644 --- a/nonlinear/LieValues.h +++ b/nonlinear/LieValues.h @@ -72,8 +72,8 @@ namespace gtsam { LieValues() {} LieValues(const LieValues& config) : values_(config.values_) {} - template - LieValues(const LieValues& other) {} // do nothing when initializing with wrong type + template + LieValues(const LieValues& 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 - void serialize(Archive & ar, const unsigned int version) { + template + void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(values_); } diff --git a/nonlinear/NonlinearEquality.h b/nonlinear/NonlinearEquality.h index ad1a8beac..809e6c05d 100644 --- a/nonlinear/NonlinearEquality.h +++ b/nonlinear/NonlinearEquality.h @@ -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 NonlinearEquality: public NonlinearFactor1 { + template + class NonlinearEquality: public NonlinearFactor1 { 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 Base; + typedef NonlinearFactor1 Base; /** * Constructor - forces exact evaluation */ - NonlinearEquality(const Key& j, const T& feasible, bool (*compare)(const T&, const T&) = compare) : + NonlinearEquality(const KEY& j, const T& feasible, bool (*compare)(const T&, const T&) = compare) : Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), allow_error_(false), error_gain_(std::numeric_limits::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) : + NonlinearEquality(const KEY& j, const T& feasible, double error_gain, bool (*compare)(const T&, const T&) = compare) : 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& f, double tol = 1e-9) const { + bool equals(const NonlinearEquality& 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 linearize(const Values& x, const Ordering& ordering) const { + virtual boost::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const { const T& xj = x[this->key_]; Matrix A; Vector b = evaluateError(xj, A); diff --git a/nonlinear/NonlinearFactor.h b/nonlinear/NonlinearFactor.h index 962b6d748..532fb4661 100644 --- a/nonlinear/NonlinearFactor.h +++ b/nonlinear/NonlinearFactor.h @@ -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 NonlinearFactor: public Testable > { + template + class NonlinearFactor: public Testable > { protected: - typedef NonlinearFactor This; + typedef NonlinearFactor This; SharedGaussian noiseModel_; /** Noise model */ std::list keys_; /** cached keys */ public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > 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& f, double tol = 1e-9) const { + bool equals(const NonlinearFactor& 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 - 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 - void serialize(Archive & ar, const unsigned int version) { + template + 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 NonlinearFactor1: public NonlinearFactor { + template + class NonlinearFactor1: public NonlinearFactor { 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 Base; - typedef NonlinearFactor1 This; + typedef NonlinearFactor Base; + typedef NonlinearFactor1 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& f, double tol = 1e-9) const { + bool equals(const NonlinearFactor1& 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 linearize(const Values& x, const Ordering& ordering) const { + virtual boost::shared_ptr 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 - void serialize(Archive & ar, const unsigned int version) { + template + void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(key_); @@ -270,23 +270,23 @@ namespace gtsam { /** * A Gaussian nonlinear factor that takes 2 parameters */ - template - class NonlinearFactor2: public NonlinearFactor { + template + class NonlinearFactor2: public NonlinearFactor { 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 Base; - typedef NonlinearFactor2 This; + typedef NonlinearFactor Base; + typedef NonlinearFactor2 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& f, double tol = 1e-9) const { + bool equals(const NonlinearFactor2& 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 linearize(const Values& c, const Ordering& ordering) const { + boost::shared_ptr 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 - void serialize(Archive & ar, const unsigned int version) { + template + void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(key1_); @@ -406,25 +406,25 @@ namespace gtsam { /** * A Gaussian nonlinear factor that takes 3 parameters */ - template - class NonlinearFactor3: public NonlinearFactor { + template + class NonlinearFactor3: public NonlinearFactor { 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 Base; - typedef NonlinearFactor3 This; + typedef NonlinearFactor Base; + typedef NonlinearFactor3 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& f, double tol = 1e-9) const { + bool equals(const NonlinearFactor3& 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 linearize(const Values& c, const Ordering& ordering) const { + boost::shared_ptr 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 - void serialize(Archive & ar, const unsigned int version) { + template + void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(key1_); diff --git a/nonlinear/NonlinearFactorGraph-inl.h b/nonlinear/NonlinearFactorGraph-inl.h index 37ee92cb4..33cd2008e 100644 --- a/nonlinear/NonlinearFactorGraph-inl.h +++ b/nonlinear/NonlinearFactorGraph-inl.h @@ -34,14 +34,14 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -template -void NonlinearFactorGraph::print(const std::string& str) const { +template +void NonlinearFactorGraph::print(const std::string& str) const { Base::print(str); } /* ************************************************************************* */ - template - Vector NonlinearFactorGraph::unwhitenedError(const Values& c) const { + template + Vector NonlinearFactorGraph::unwhitenedError(const VALUES& c) const { list errors; BOOST_FOREACH(const sharedFactor& factor, this->factors_) errors.push_back(factor->unwhitenedError(c)); @@ -49,8 +49,8 @@ void NonlinearFactorGraph::print(const std::string& str) const { } /* ************************************************************************* */ - template - double NonlinearFactorGraph::error(const Values& c) const { + template + double NonlinearFactorGraph::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::print(const std::string& str) const { } /* ************************************************************************* */ - template - Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(const Values& config) const { + template + Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(const VALUES& config) const { // Create symbolic graph and initial (iterator) ordering SymbolicFactorGraph::shared_ptr symbolic; @@ -84,9 +84,9 @@ void NonlinearFactorGraph::print(const std::string& str) const { } /* ************************************************************************* */ - template - SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic( - const Values& config, const Ordering& ordering) const { + template + SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::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::print(const std::string& str) const { } /* ************************************************************************* */ - template + template pair - NonlinearFactorGraph::symbolic(const Values& config) const { + NonlinearFactorGraph::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 - boost::shared_ptr NonlinearFactorGraph::linearize( - const Values& config, const Ordering& ordering) const { + template + boost::shared_ptr NonlinearFactorGraph::linearize( + const VALUES& config, const Ordering& ordering) const { // create an empty linear FG GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph); diff --git a/nonlinear/NonlinearFactorGraph.h b/nonlinear/NonlinearFactorGraph.h index 72961aac8..dad79ca22 100644 --- a/nonlinear/NonlinearFactorGraph.h +++ b/nonlinear/NonlinearFactorGraph.h @@ -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 NonlinearFactorGraph: public FactorGraph > { + template + class NonlinearFactorGraph: public FactorGraph > { public: - typedef FactorGraph > Base; - typedef typename boost::shared_ptr > sharedFactor; + typedef FactorGraph > Base; + typedef typename boost::shared_ptr > 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 - 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 - linearize(const Values& config, const Ordering& ordering) const; + linearize(const VALUES& config, const Ordering& ordering) const; };