diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 49d31abc0..b484b801a 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -60,16 +60,16 @@ public: * Typedef to the conditional type obtained by eliminating this factor. * Derived classes must redefine this. */ - typedef gtsam::Conditional ConditionalType; + typedef Conditional ConditionalType; /** A shared_ptr to this class. Derived classes must redefine this. */ typedef boost::shared_ptr shared_ptr; /** Iterator over keys */ - typedef std::vector::iterator iterator; + typedef typename std::vector::iterator iterator; /** Const iterator over keys */ - typedef std::vector::const_iterator const_iterator; + typedef typename std::vector::const_iterator const_iterator; protected: diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 8f55e9bcf..c1649dfd4 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -142,8 +142,8 @@ bool GaussianConditional::equals(const GaussianConditional &c, double tol) const } /* ************************************************************************* */ -GaussianFactor::shared_ptr GaussianConditional::toFactor() const { - return GaussianFactor::shared_ptr(new JacobianFactor(*this)); +JacobianFactor::shared_ptr GaussianConditional::toFactor() const { + return JacobianFactor::shared_ptr(new JacobianFactor(*this)); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index c6cc7ef23..e6f9a80c1 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -26,6 +26,7 @@ #include #include +#include #include #include #include @@ -122,7 +123,7 @@ public: * Copy to a Factor (this creates a JacobianFactor and returns it as its * base class GaussianFactor. */ - boost::shared_ptr toFactor() const; + boost::shared_ptr toFactor() const; /** * solve a conditional Gaussian diff --git a/gtsam/linear/GaussianFactor.cpp b/gtsam/linear/GaussianFactor.cpp index 8a306d73a..19bb1234d 100644 --- a/gtsam/linear/GaussianFactor.cpp +++ b/gtsam/linear/GaussianFactor.cpp @@ -32,6 +32,9 @@ using namespace std; namespace gtsam { + /* ************************************************************************* */ + GaussianFactor::GaussianFactor(const GaussianConditional& c) : IndexFactor(c) {} + /* ************************************************************************* */ pair GaussianFactor::CombineAndEliminate( const FactorGraph& factors, size_t nrFrontals, SolveMethod solveMethod) { diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 436940d0c..ba11a1e93 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -21,7 +21,7 @@ #pragma once #include -#include +#include #include #include @@ -29,8 +29,11 @@ namespace gtsam { + // Forward declarations class VectorValues; class Permutation; + class GaussianConditional; + template class BayesNet; /** * Base Class for a linear factor. @@ -45,7 +48,7 @@ namespace gtsam { GaussianFactor(const This& f) : IndexFactor(f) {} /** Construct from derived type */ - GaussianFactor(const GaussianConditional& c) : IndexFactor(c) {} + GaussianFactor(const GaussianConditional& c); /** Constructor from a collection of keys */ template GaussianFactor(KeyIterator beginKey, KeyIterator endKey) : @@ -96,7 +99,7 @@ namespace gtsam { /** * Combine and eliminate several factors. */ - static std::pair CombineAndEliminate( + static std::pair >, shared_ptr> CombineAndEliminate( const FactorGraph& factors, size_t nrFrontals=1, SolveMethod solveMethod=SOLVE_QR); }; // GaussianFactor diff --git a/gtsam/linear/GaussianISAM.h b/gtsam/linear/GaussianISAM.h index abbfb6a5a..732742f8c 100644 --- a/gtsam/linear/GaussianISAM.h +++ b/gtsam/linear/GaussianISAM.h @@ -19,7 +19,7 @@ #pragma once -#include +#include #include #include diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 839b714b1..39d0ebfbe 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include @@ -29,6 +30,8 @@ namespace gtsam { // Forward declarations class JacobianFactor; class SharedDiagonal; + class GaussianConditional; + template class BayesNet; // Definition of Scatter struct SlotEntry { @@ -47,7 +50,7 @@ namespace gtsam { BlockInfo info_; // The block view of the full information matrix. void assertInvariants() const; - GaussianBayesNet::shared_ptr splitEliminatedFactor(size_t nrFrontals, const std::vector& keys); + boost::shared_ptr > splitEliminatedFactor(size_t nrFrontals, const std::vector& keys); void updateATA(const JacobianFactor& update, const Scatter& scatter); void updateATA(const HessianFactor& update, const Scatter& scatter); @@ -123,7 +126,7 @@ namespace gtsam { /** * Combine and eliminate several factors. */ - static std::pair CombineAndEliminate( + static std::pair >, shared_ptr> CombineAndEliminate( const FactorGraph& factors, size_t nrFrontals=1); // Friend unit test classes diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 5b8e769ce..46c238de0 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -36,6 +37,8 @@ namespace gtsam { // Forward declarations class HessianFactor; class VariableSlots; + class GaussianConditional; + template class BayesNet; class JacobianFactor : public GaussianFactor { @@ -194,12 +197,12 @@ namespace gtsam { /** * Combine and eliminate several factors. */ - static std::pair CombineAndEliminate( + static std::pair >, shared_ptr> CombineAndEliminate( const FactorGraph& factors, size_t nrFrontals=1); - GaussianConditional::shared_ptr eliminateFirst(); + boost::shared_ptr eliminateFirst(); - GaussianBayesNet::shared_ptr eliminate(size_t nrFrontals = 1); + boost::shared_ptr > eliminate(size_t nrFrontals = 1); // Friend HessianFactor to facilitate convertion constructors friend class HessianFactor; diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 09587cf22..433489e05 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -26,6 +26,7 @@ #include #include +#include #include #include #include @@ -54,14 +55,13 @@ namespace gtsam { * which are objects in non-linear manifolds (Lie groups). */ template - class NonlinearFactor: public Testable > { + class NonlinearFactor: public Factor, public Testable > { protected: typedef NonlinearFactor This; SharedGaussian noiseModel_; /** Noise model */ - std::list keys_; /** cached keys */ public: @@ -100,32 +100,16 @@ namespace gtsam { return 0.5 * noiseModel_->Mahalanobis(unwhitenedError(c)); } - /** return keys */ - const std::list& keys() const { - return keys_; - } - /** get the dimension of the factor (number of rows on linearization) */ size_t dim() const { return noiseModel_->dim(); } - /* return the begin iterator of keys */ - std::list::const_iterator begin() const { return keys_.begin(); } - - /* return the end iterator of keys */ - std::list::const_iterator end() const { return keys_.end(); } - /** access to the noise model */ SharedGaussian get_noiseModel() const { return noiseModel_; } - /** get the size of the factor */ - std::size_t size() const { - return keys_.size(); - } - /** Vector of errors, unwhitened ! */ virtual Vector unwhitenedError(const VALUES& c) const = 0; @@ -310,6 +294,7 @@ namespace gtsam { NonlinearFactor2(const SharedGaussian& noiseModel, KEY1 j1, KEY2 j2) : Base(noiseModel), key1_(j1), key2_(j2) { + this->keys_.reserve(2); this->keys_.push_back(key1_); this->keys_.push_back(key2_); } @@ -450,6 +435,7 @@ namespace gtsam { */ NonlinearFactor3(const SharedGaussian& noiseModel, KEY1 j1, KEY2 j2, KEY3 j3) : Base(noiseModel), key1_(j1), key2_(j2), key3_(j3) { + this->keys_.reserve(3); this->keys_.push_back(key1_); this->keys_.push_back(key2_); this->keys_.push_back(key3_);