diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 4f97db7e1..c167239ec 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -54,6 +54,10 @@ namespace gtsam { /// a nonlinear 'print' function. Automatically detects plain integer keys /// and Symbol keys. static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; + + + /// The index type for Eigen objects + typedef ptrdiff_t DenseIndex; /** diff --git a/gtsam/inference/ConditionalUnordered.h b/gtsam/inference/ConditionalUnordered.h index 9734c40f4..0346ef5e3 100644 --- a/gtsam/inference/ConditionalUnordered.h +++ b/gtsam/inference/ConditionalUnordered.h @@ -43,10 +43,10 @@ namespace gtsam { size_t nrFrontals_; /** Iterator over keys */ - typedef typename FACTOR::iterator iterator; + using typename FACTOR::iterator; // 'using' instead of typedef to avoid ambiguous symbol from multiple inheritance /** Const iterator over keys */ - typedef typename FACTOR::const_iterator const_iterator; + using typename FACTOR::const_iterator; // 'using' instead of typedef to avoid ambiguous symbol from multiple inheritance public: diff --git a/gtsam/inference/FactorUnordered.h b/gtsam/inference/FactorUnordered.h index a03776284..7a3e36628 100644 --- a/gtsam/inference/FactorUnordered.h +++ b/gtsam/inference/FactorUnordered.h @@ -29,160 +29,142 @@ namespace gtsam { -/** - * This is the base class for all factor types. It is templated on a KEY type, - * which will be the type used to label variables. Key types currently in use - * in gtsam are Index with symbolic (IndexFactor, SymbolicFactorGraph) and - * Gaussian factors (GaussianFactor, JacobianFactor, HessianFactor, GaussianFactorGraph), - * and Key with nonlinear factors (NonlinearFactor, NonlinearFactorGraph). - * though currently only IndexFactor and IndexConditional derive from this - * class, using Index keys. This class does not store any data other than its - * keys. Derived classes store data such as matrices and probability tables. - * - * Note that derived classes *must* redefine the ConditionalType and shared_ptr - * typedefs to refer to the associated conditional and shared_ptr types of the - * derived class. See IndexFactor, JacobianFactor, etc. for examples. - * - * This class is \b not virtual for performance reasons - derived symbolic classes, - * IndexFactor and IndexConditional, need to be created and destroyed quickly - * during symbolic elimination. GaussianFactor and NonlinearFactor are virtual. - * \nosubgrouping - */ -class GTSAM_EXPORT FactorUnordered { - -private: - typedef FactorUnordered This; ///< This class - - /// A shared_ptr to this class, derived classes must redefine this. - typedef boost::shared_ptr shared_ptr; - -public: - /// Iterator over keys - typedef std::vector::iterator iterator; - - /// Const iterator over keys - typedef std::vector::const_iterator const_iterator; - -protected: - - /// The keys involved in this factor - std::vector keys_; - - /// @name Standard Constructors - /// @{ - - /** Default constructor for I/O */ - FactorUnordered() {} - - /** Construct unary factor */ - FactorUnordered(Key key) : keys_(1) { - keys_[0] = key; } - - /** Construct binary factor */ - FactorUnordered(Key key1, Key key2) : keys_(2) { - keys_[0] = key1; keys_[1] = key2; } - - /** Construct ternary factor */ - FactorUnordered(Key key1, Key key2, Key key3) : keys_(3) { - keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; } - - /** Construct 4-way factor */ - FactorUnordered(Key key1, Key key2, Key key3, Key key4) : keys_(4) { - keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; } - - /** Construct 5-way factor */ - FactorUnordered(Key key1, Key key2, Key key3, Key key4, Key key5) : keys_(5) { - keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; } - - /** Construct 6-way factor */ - FactorUnordered(Key key1, Key key2, Key key3, Key key4, Key key5, Key key6) : keys_(6) { - keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; keys_[5] = key6; } - - /// @} - - - /// @name Advanced Constructors - /// @{ - - /** Construct n-way factor from iterator over keys. */ - template static FactorUnordered FromIterator(ITERATOR first, ITERATOR last) { - FactorUnordered result; - result.keys_.assign(first, last); - return result; } - - /** Construct n-way factor from container of keys. */ - template - static FactorUnordered FromKeys(const CONTAINER& keys) { return FromIterator(keys.begin(), keys.end()); } - - /// @} - -public: - /// @name Standard Interface - /// @{ - - /// First key - Key front() const { return keys_.front(); } - - /// Last key - Key back() const { return keys_.back(); } - - /// find - const_iterator find(Key key) const { return std::find(begin(), end(), key); } - - /// Access the factor's involved variable keys - const std::vector& keys() const { return keys_; } - - /** Iterator at beginning of involved variable keys */ - const_iterator begin() const { return keys_.begin(); } - - /** Iterator at end of involved variable keys */ - const_iterator end() const { return keys_.end(); } - /** - * @return the number of variables involved in this factor + * This is the base class for all factor types. It is templated on a KEY type, + * which will be the type used to label variables. Key types currently in use + * in gtsam are Index with symbolic (IndexFactor, SymbolicFactorGraph) and + * Gaussian factors (GaussianFactor, JacobianFactor, HessianFactor, GaussianFactorGraph), + * and Key with nonlinear factors (NonlinearFactor, NonlinearFactorGraph). + * though currently only IndexFactor and IndexConditional derive from this + * class, using Index keys. This class does not store any data other than its + * keys. Derived classes store data such as matrices and probability tables. + * + * Note that derived classes *must* redefine the ConditionalType and shared_ptr + * typedefs to refer to the associated conditional and shared_ptr types of the + * derived class. See IndexFactor, JacobianFactor, etc. for examples. + * + * This class is \b not virtual for performance reasons - derived symbolic classes, + * IndexFactor and IndexConditional, need to be created and destroyed quickly + * during symbolic elimination. GaussianFactor and NonlinearFactor are virtual. + * \nosubgrouping */ - size_t size() const { return keys_.size(); } + class GTSAM_EXPORT FactorUnordered + { - /// @} - + private: + // These typedefs are private because they must be overridden in derived classes. + typedef FactorUnordered This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< A shared_ptr to this class. - /// @name Testable - /// @{ + public: + /// Iterator over keys + typedef std::vector::iterator iterator; - /// print - void print(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const; + /// Const iterator over keys + typedef std::vector::const_iterator const_iterator; - /// print only keys - void printKeys(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const; + protected: - /// check equality - bool equals(const This& other, double tol = 1e-9) const; + /// The keys involved in this factor + std::vector keys_; - /// @} - + /// @name Standard Constructors + /// @{ - /// @name Advanced Interface - /// @{ + /** Default constructor for I/O */ + FactorUnordered() {} - /** @return keys involved in this factor */ - std::vector& keys() { return keys_; } + /** Construct factor from container of keys. This constructor is used internally from derived factor + * constructors, either from a container of keys or from a boost::assign::list_of. */ + template + FactorUnordered(const CONTAINER& keys) : keys_(keys.begin(), keys.end()) {} - /** Iterator at beginning of involved variable keys */ - iterator begin() { return keys_.begin(); } + /** Construct factor from iterator keys. This constructor may be used internally from derived + * factor constructors, although our code currently does not use this. */ + template + FactorUnordered(ITERATOR first, ITERATOR last) : keys_(first, last) {} - /** Iterator at end of involved variable keys */ - iterator end() { return keys_.end(); } + /** Construct factor from container of keys. This is called internally from derived factor static + * factor methods, as a workaround for not being able to call the protected constructors above. */ + template + static FactorUnordered FromKeys(const CONTAINER& keys) { + return FactorUnordered(keys.begin(), keys.end()); } -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(keys_); - } + /** Construct factor from iterator keys. This is called internally from derived factor static + * factor methods, as a workaround for not being able to call the protected constructors above. */ + template + static FactorUnordered FromIterators(ITERATOR first, ITERATOR last) { + return FactorUnordered(first, last); } - /// @} + /// @} -}; + public: + /// @name Standard Interface + /// @{ + + /// First key + Key front() const { return keys_.front(); } + + /// Last key + Key back() const { return keys_.back(); } + + /// find + const_iterator find(Key key) const { return std::find(begin(), end(), key); } + + /// Access the factor's involved variable keys + const std::vector& keys() const { return keys_; } + + /** Iterator at beginning of involved variable keys */ + const_iterator begin() const { return keys_.begin(); } + + /** Iterator at end of involved variable keys */ + const_iterator end() const { return keys_.end(); } + + /** + * @return the number of variables involved in this factor + */ + size_t size() const { return keys_.size(); } + + /// @} + + + /// @name Testable + /// @{ + + /// print + void print(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const; + + /// print only keys + void printKeys(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const; + + /// check equality + bool equals(const This& other, double tol = 1e-9) const; + + /// @} + + + /// @name Advanced Interface + /// @{ + + /** @return keys involved in this factor */ + std::vector& keys() { return keys_; } + + /** Iterator at beginning of involved variable keys */ + iterator begin() { return keys_.begin(); } + + /** Iterator at end of involved variable keys */ + iterator end() { return keys_.end(); } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(keys_); + } + + /// @} + + }; } diff --git a/gtsam/linear/linearExceptions.h b/gtsam/linear/linearExceptions.h index 8b036df0a..f44e2c18a 100644 --- a/gtsam/linear/linearExceptions.h +++ b/gtsam/linear/linearExceptions.h @@ -114,4 +114,40 @@ on gtsam::IndeterminantLinearSystemException for more information.\n"; } }; -} + /* ************************************************************************* */ + /** An exception indicating that the noise model dimension passed into a + * JacobianFactor has a different dimensionality than the factor. */ + class InvalidNoiseModel : public std::exception { + public: + const DenseIndex factorDims; ///< The dimensionality of the factor + const DenseIndex noiseModelDims; ///< The dimensionality of the noise model + + InvalidNoiseModel(DenseIndex factorDims, DenseIndex noiseModelDims) : + factorDims(factorDims), noiseModelDims(noiseModelDims) {} + virtual ~InvalidNoiseModel() throw() {} + + virtual const char* what() const throw(); + + private: + mutable std::string description_; + }; + + /* ************************************************************************* */ + /** An exception indicating that a matrix block passed into a + * JacobianFactor has a different dimensionality than the factor. */ + class InvalidMatrixBlock : public std::exception { + public: + const DenseIndex factorRows; ///< The dimensionality of the factor + const DenseIndex blockRows; ///< The dimensionality of the noise model + + InvalidMatrixBlock(DenseIndex factorRows, DenseIndex blockRows) : + factorRows(factorRows), blockRows(noiseModelDims) {} + virtual ~InvalidMatrixBlock() throw() {} + + virtual const char* what() const throw(); + + private: + mutable std::string description_; + }; + + } diff --git a/gtsam/symbolic/SymbolicConditionalUnordered.h b/gtsam/symbolic/SymbolicConditionalUnordered.h index 188203e6b..6cbd7a6dc 100644 --- a/gtsam/symbolic/SymbolicConditionalUnordered.h +++ b/gtsam/symbolic/SymbolicConditionalUnordered.h @@ -32,12 +32,14 @@ namespace gtsam { * class for conditionals. * \nosubgrouping */ - class GTSAM_EXPORT SymbolicConditionalUnordered : public SymbolicFactorUnordered, public ConditionalUnordered { + class GTSAM_EXPORT SymbolicConditionalUnordered : + public SymbolicFactorUnordered, + public ConditionalUnordered { public: typedef SymbolicConditionalUnordered This; /// Typedef to this class typedef SymbolicFactorUnordered BaseFactor; /// Typedef to the factor base class - typedef ConditionalUnordered BaseConditional; /// Typedef to the conditional base class + typedef ConditionalUnordered BaseConditional; /// Typedef to the conditional base class typedef boost::shared_ptr shared_ptr; /// Boost shared_ptr to this class typedef BaseFactor::iterator iterator; /// iterator to keys typedef BaseFactor::const_iterator const_iterator; /// const_iterator to keys diff --git a/gtsam/symbolic/SymbolicFactorUnordered.h b/gtsam/symbolic/SymbolicFactorUnordered.h index aca3ca63f..76aeec834 100644 --- a/gtsam/symbolic/SymbolicFactorUnordered.h +++ b/gtsam/symbolic/SymbolicFactorUnordered.h @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -54,41 +55,47 @@ namespace gtsam { SymbolicFactorUnordered() {} /** Construct unary factor */ - SymbolicFactorUnordered(Key j) : Base(j) {} + SymbolicFactorUnordered(Key j) : + Base(boost::assign::cref_list_of<1>(j)) {} /** Construct binary factor */ - SymbolicFactorUnordered(Key j1, Key j2) : Base(j1, j2) {} + SymbolicFactorUnordered(Key j1, Key j2) : + Base(boost::assign::cref_list_of<2>(j1)(j2)) {} /** Construct ternary factor */ - SymbolicFactorUnordered(Key j1, Key j2, Key j3) : Base(j1, j2, j3) {} + SymbolicFactorUnordered(Key j1, Key j2, Key j3) : + Base(boost::assign::cref_list_of<3>(j1)(j2)(j3)) {} /** Construct 4-way factor */ - SymbolicFactorUnordered(Key j1, Key j2, Key j3, Key j4) : Base(j1, j2, j3, j4) {} + SymbolicFactorUnordered(Key j1, Key j2, Key j3, Key j4) : + Base(boost::assign::cref_list_of<4>(j1)(j2)(j3)(j4)) {} /** Construct 5-way factor */ - SymbolicFactorUnordered(Key j1, Key j2, Key j3, Key j4, Key j5) : Base(j1, j2, j3, j4, j5) {} + SymbolicFactorUnordered(Key j1, Key j2, Key j3, Key j4, Key j5) : + Base(boost::assign::cref_list_of<5>(j1)(j2)(j3)(j4)(j5)) {} /** Construct 6-way factor */ - SymbolicFactorUnordered(Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) : Base(j1, j2, j3, j4, j5, j6) {} + SymbolicFactorUnordered(Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) : + Base(boost::assign::cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)) {} /// @} /// @name Advanced Constructors /// @{ + private: + explicit SymbolicFactorUnordered(const Base& base) : + Base(base) {} + public: /** Constructor from a collection of keys */ template static SymbolicFactorUnordered FromIterator(KEYITERATOR beginKey, KEYITERATOR endKey) { - SymbolicFactorUnordered result; - (Base&)result = Base::FromIterator(beginKey, endKey); - return result; } + return SymbolicFactorUnordered(Base::FromIterators(beginKey, endKey)); } /** Constructor from a collection of keys */ template static SymbolicFactorUnordered FromKeys(const CONTAINER& keys) { - SymbolicFactorUnordered result; - (Base&)result = Base::FromKeys(keys); - return result; } + return SymbolicFactorUnordered(Base::FromKeys(keys)); } /// @}