Interface tweaks

gtsam/inference/FactorGraphUnordered.h

@@ -130,6 +130,10 @@ namespace gtsam {
     template<class CLIQUE>
     void push_back_bayesTree(const BayesTreeUnordered<CLIQUE>& bayesTree);
 
+    template<class DERIVEDFACTOR>
+    void push_back(const DERIVEDFACTOR& factor) {
+      add(factor); }
+
   public:
     /** += syntax for push_back, e.g. graph += f1, f2, f3 */
     template<class T>

gtsam/linear/GaussianConditionalUnordered-inl.h

@@ -28,7 +28,7 @@ namespace gtsam {
   template<typename PARENTS>
   GaussianConditionalUnordered::GaussianConditionalUnordered(Index key, const Vector& d,
     const Matrix& R, const PARENTS& parents, const SharedDiagonal& sigmas) :
-  BaseFactor(boost::join(boost::assign::cref_list_of<1>(std::make_pair(key, R)), parents), d, sigmas),
+  BaseFactor(boost::join(boost::assign::cref_list_of<1,typename PARENTS::value_type>(std::make_pair(key, R)), parents), d, sigmas),
     BaseConditional(1) {}
 
   /* ************************************************************************* */

gtsam/linear/GaussianConditionalUnordered.h

@@ -46,26 +46,22 @@ namespace gtsam {
     /** default constructor needed for serialization */
     GaussianConditionalUnordered() {}
 
-    /** constructor with no parents
+    /** constructor with no parents |Rx-d| */
-    * |Rx-d|
-    */
     GaussianConditionalUnordered(Key key, const Vector& d, const Matrix& R,
       const SharedDiagonal& sigmas = SharedDiagonal());
 
-    /** constructor with only one parent
+    /** constructor with only one parent |Rx+Sy-d| */
-     * |Rx+Sy-d| */
     GaussianConditionalUnordered(Key key, const Vector& d, const Matrix& R,
       Key name1, const Matrix& S, const SharedDiagonal& sigmas = SharedDiagonal());
 
-    /** constructor with two parents
+    /** constructor with two parents |Rx+Sy+Tz-d| */
-     * |Rx+Sy+Tz-d| */
     GaussianConditionalUnordered(Key key, const Vector& d, const Matrix& R,
       Key name1, const Matrix& S, Key name2, const Matrix& T,
       const SharedDiagonal& sigmas = SharedDiagonal());
 
     /** Constructor with number of arbitrary parents.  \f$ |Rx+sum(Ai*xi)-d| \f$
-     * @tparam PARENTS A container whose value type is std::pair<Key, Matrix>, specifying the
+     *  @tparam PARENTS A container whose value type is std::pair<Key, Matrix>, specifying the
-     *         collection of parent keys and matrices. */
+     *          collection of parent keys and matrices. */
     template<typename PARENTS>
     GaussianConditionalUnordered(Key key, const Vector& d,
       const Matrix& R, const PARENTS& parents,
@@ -111,6 +107,12 @@ namespace gtsam {
     /** Get a view of the parent blocks. */
     constABlock get_S() const { return Ab_.range(nrFrontals(), size()); }
 
+    /** Get a view of the S matrix for the variable pointed to by the given key iterator */
+    constABlock get_S(const_iterator variable) const { return BaseFactor::getA(variable); }
+
+    /** Get a view of the r.h.s. vector d */
+    const constBVector get_d() const { return BaseFactor::getb(); }
+
     /**
     * Solves a conditional Gaussian and writes the solution into the entries of
     * \c x for each frontal variable of the conditional.  The parents are

gtsam/linear/JacobianFactorUnordered.h

@@ -213,11 +213,6 @@ namespace gtsam {
      */
     size_t rows() const { return Ab_.rows(); }
 
-    /**
-     * return the number of rows in the corresponding linear system
-     */
-    size_t numberOfRows() const { return rows(); }
-
     /**
      * return the number of columns in the corresponding linear system
      */

gtsam/linear/VectorValuesUnordered.h

@@ -110,6 +110,17 @@ namespace gtsam {
     /** Merge two VectorValues into one, this is more efficient than inserting elements one by one. */
     VectorValuesUnordered(const VectorValuesUnordered& first, const VectorValuesUnordered& second);
 
+    /** Create from another container holding pair<Key,Vector>. */
+    template<class CONTAINER>
+    explicit VectorValuesUnordered(const CONTAINER& c) : values_(c.begin(), c.end()) {}
+
+    /** Implicit copy constructor to specialize the explicit constructor from any container. */
+    VectorValuesUnordered(const VectorValuesUnordered& c) : values_(c.values_) {}
+
+    /** Create from a pair of iterators over pair<Key,Vector>. */
+    template<typename ITERATOR>
+    VectorValuesUnordered(ITERATOR first, ITERATOR last) : values_(first, last) {}
+
     /** Create a VectorValues with the same structure as \c other, but filled with zeros. */
     static VectorValuesUnordered Zero(const VectorValuesUnordered& other);
 
@@ -157,9 +168,20 @@ namespace gtsam {
      * @param j The index with which the value will be associated.
      */
     void insert(Key j, const Vector& value) {
-      if(!values_.insert(std::make_pair(j, value)).second)
+      insert(std::pair<Key, const Vector&>(j, value)); // Note only passing a reference to the Vector
+    }
+
+    /** Insert a vector \c value with key \c j.  Throws an invalid_argument exception if the key \c j is already used.
+     * @param value The vector to be inserted.
+     * @param j The index with which the value will be associated.
+     */
+    void insert(std::pair<Key, const Vector&> key_value) {
+      // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as
+      // it is inserted into the values_ map.
+      if(!values_.insert(key_value).second)
         throw std::invalid_argument(
-        "Requested to insert variable '" + DefaultKeyFormatter(j) + "' already in this VectorValues.");
+        "Requested to insert variable '" + DefaultKeyFormatter(key_value.first)
+        + "' already in this VectorValues.");
     }
 
     /** Insert all values from \c values.  Throws an invalid_argument exception if any keys to be