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

Tweaks to factor constructors and other small cleanups

release/4.3a0
Richard Roberts 2013-06-27 23:03:36 +00:00
parent 083c7213b5
commit b9016adbd6
6 changed files with 188 additions and 157 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/base/types.h
View File

@ -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;
/**

4
gtsam/inference/ConditionalUnordered.h
View File

@ -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:

262
gtsam/inference/FactorUnordered.h
View File

@ -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<FactorUnordered> shared_ptr;
public:
/// Iterator over keys
typedef std::vector<Key>::iterator iterator;
/// Const iterator over keys
typedef std::vector<Key>::const_iterator const_iterator;
protected:
/// The keys involved in this factor
std::vector<Key> 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<typename ITERATOR> 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<class CONTAINER>
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<Key>& 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<FactorUnordered> shared_ptr; ///< A shared_ptr to this class.
/// @name Testable
/// @{
public:
/// Iterator over keys
typedef std::vector<Key>::iterator iterator;
/// print
void print(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// Const iterator over keys
typedef std::vector<Key>::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<Key> keys_;
/// @}
/// @name Standard Constructors
/// @{
/// @name Advanced Interface
/// @{
/** Default constructor for I/O */
FactorUnordered() {}
/** @return keys involved in this factor */
std::vector<Key>& 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<typename CONTAINER>
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<typename ITERATOR>
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<typename CONTAINER>
static FactorUnordered FromKeys(const CONTAINER& keys) {
return FactorUnordered(keys.begin(), keys.end()); }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
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<typename ITERATOR>
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<Key>& 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<Key>& 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<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(keys_);
}
/// @}
};
}

38
gtsam/linear/linearExceptions.h
View File

@ -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_;
};
}

6
gtsam/symbolic/SymbolicConditionalUnordered.h
View File

@ -32,12 +32,14 @@ namespace gtsam {
* class for conditionals.
* \nosubgrouping
*/
class GTSAM_EXPORT SymbolicConditionalUnordered : public SymbolicFactorUnordered, public ConditionalUnordered<SymbolicFactorUnordered,SymbolicConditionalUnordered> {
class GTSAM_EXPORT SymbolicConditionalUnordered :
public SymbolicFactorUnordered,
public ConditionalUnordered<SymbolicFactorUnordered, SymbolicConditionalUnordered> {
public:
typedef SymbolicConditionalUnordered This; /// Typedef to this class
typedef SymbolicFactorUnordered BaseFactor; /// Typedef to the factor base class
typedef ConditionalUnordered<SymbolicFactorUnordered,SymbolicConditionalUnordered> BaseConditional; /// Typedef to the conditional base class
typedef ConditionalUnordered<BaseFactor, This> BaseConditional; /// Typedef to the conditional base class
typedef boost::shared_ptr<This> 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

31
gtsam/symbolic/SymbolicFactorUnordered.h
View File

@ -19,6 +19,7 @@
#include <utility>
#include <boost/shared_ptr.hpp>
#include <boost/assign/list_of.hpp>
#include <gtsam/inference/FactorUnordered.h>
#include <gtsam/inference/Key.h>
@ -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<typename KEYITERATOR>
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<class CONTAINER>
static SymbolicFactorUnordered FromKeys(const CONTAINER& keys) {
SymbolicFactorUnordered result;
(Base&)result = Base::FromKeys(keys);
return result; }
return SymbolicFactorUnordered(Base::FromKeys(keys)); }
/// @}