Tweaks to factor constructors and other small cleanups

gtsam/base/types.h

@@ -56,6 +56,10 @@ namespace gtsam {
   static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
 
 
+  /// The index type for Eigen objects
+  typedef ptrdiff_t DenseIndex;
+
+
   /**
    * Helper class that uses templates to select between two types based on
    * whether TEST_TYPE is const or not.

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:
 

gtsam/inference/FactorUnordered.h

@@ -48,13 +48,13 @@ namespace gtsam {
    * during symbolic elimination.  GaussianFactor and NonlinearFactor are virtual.
    * \nosubgrouping
    */
-class GTSAM_EXPORT FactorUnordered {
+  class GTSAM_EXPORT FactorUnordered
+  {
 
   private:
+    // These typedefs are private because they must be overridden in derived classes.
     typedef FactorUnordered This; ///< This class
-
-  /// A shared_ptr to this class, derived classes must redefine this.
-  typedef boost::shared_ptr<FactorUnordered> shared_ptr;
+    typedef boost::shared_ptr<FactorUnordered> shared_ptr; ///< A shared_ptr to this class.
 
   public:
     /// Iterator over keys
@@ -74,45 +74,27 @@ protected:
     /** Default constructor for I/O */
     FactorUnordered() {}
 
-  /** Construct unary factor */
-  FactorUnordered(Key key) : keys_(1) {
-    keys_[0] = key; }
+    /** 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()) {}
 
-  /** Construct binary factor */
-  FactorUnordered(Key key1, Key key2) : keys_(2) {
-    keys_[0] = key1; keys_[1] = key2; }
+    /** 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) {}
 
-  /** Construct ternary factor */
-  FactorUnordered(Key key1, Key key2, Key key3) : keys_(3) {
-    keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; }
+    /** 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()); }
 
-  /** 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()); }
+    /** 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); }
 
     /// @}
 

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

gtsam/symbolic/SymbolicConditionalUnordered.h

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

gtsam/symbolic/SymbolicFactorUnordered.h

@@ -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)); }
 
     /// @}