From 2f228a5da84e801771e1b16d87bdd9bc06fd50a6 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 12 Jul 2013 22:27:36 +0000
Subject: [PATCH] Fixed noise model bugs and default arguments in
 GaussianConditionalUnordered

---
 gtsam/linear/GaussianConditionalUnordered.cpp |  6 +++---
 gtsam/linear/GaussianConditionalUnordered.h   | 17 +++++++++++------
 2 files changed, 14 insertions(+), 9 deletions(-)

diff --git a/gtsam/linear/GaussianConditionalUnordered.cpp b/gtsam/linear/GaussianConditionalUnordered.cpp
index e7cef825e..246493d39 100644
--- a/gtsam/linear/GaussianConditionalUnordered.cpp
+++ b/gtsam/linear/GaussianConditionalUnordered.cpp
@@ -44,13 +44,13 @@ namespace gtsam {
   GaussianConditionalUnordered::GaussianConditionalUnordered(
     Key key, const Vector& d, const Matrix& R,
     Key name1, const Matrix& S, const SharedDiagonal& sigmas) :
-  BaseFactor(key, R, name1, S, d, sigmas) {}
+  BaseFactor(key, R, name1, S, d, sigmas), BaseConditional(1) {}
 
   /* ************************************************************************* */
   GaussianConditionalUnordered::GaussianConditionalUnordered(
     Key key, const Vector& d, const Matrix& R,
     Key name1, const Matrix& S, Key name2, const Matrix& T, const SharedDiagonal& sigmas) :
-  BaseFactor(key, R, name1, S, name2, T, d, sigmas) {}
+  BaseFactor(key, R, name1, S, name2, T, d, sigmas), BaseConditional(1) {}
 
   /* ************************************************************************* */
   void GaussianConditionalUnordered::print(const string &s, const IndexFormatter& formatter) const
@@ -97,7 +97,7 @@ namespace gtsam {
 
     // check if sigmas are equal
     if ((model_ && !c.model_) || (!model_ && c.model_)
-      || !model_->equals(*c.model_, tol))
+      || (model_ && c.model_ && !model_->equals(*c.model_, tol)))
       return false;
 
     return true;
diff --git a/gtsam/linear/GaussianConditionalUnordered.h b/gtsam/linear/GaussianConditionalUnordered.h
index 78240594b..b71f1acb9 100644
--- a/gtsam/linear/GaussianConditionalUnordered.h
+++ b/gtsam/linear/GaussianConditionalUnordered.h
@@ -49,31 +49,35 @@ namespace gtsam {
     /** constructor with no parents
     * |Rx-d|
     */
-    GaussianConditionalUnordered(Key key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas);
+    GaussianConditionalUnordered(Key key, const Vector& d, const Matrix& R,
+      const SharedDiagonal& sigmas = SharedDiagonal());
 
     /** constructor with only one parent
      * |Rx+Sy-d| */
     GaussianConditionalUnordered(Key key, const Vector& d, const Matrix& R,
-      Key name1, const Matrix& S, const SharedDiagonal& sigmas);
+      Key name1, const Matrix& S, const SharedDiagonal& sigmas = SharedDiagonal());
 
     /** constructor with two parents
      * |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);
+      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
      *         collection of parent keys and matrices. */
     template<typename PARENTS>
     GaussianConditionalUnordered(Key key, const Vector& d,
-      const Matrix& R, const PARENTS& parents, const SharedDiagonal& sigmas);
+      const Matrix& R, const PARENTS& parents,
+      const SharedDiagonal& sigmas = SharedDiagonal());
 
     /** Constructor with arbitrary number of frontals and parents.
     *   @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
     *           collection of keys and matrices making up the conditional. */
     template<typename TERMS>
     GaussianConditionalUnordered(const TERMS& terms,
-      size_t nrFrontals, const Vector& d, const SharedDiagonal& sigmas);
+      size_t nrFrontals, const Vector& d,
+      const SharedDiagonal& sigmas = SharedDiagonal());
 
     /** Constructor with arbitrary number keys, and where the augmented matrix is given all together
      *  instead of in block terms.  Note that only the active view of the provided augmented matrix
@@ -81,7 +85,8 @@ namespace gtsam {
      *  factor. */
     template<typename KEYS>
     GaussianConditionalUnordered(
-      const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas);
+      const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix,
+      const SharedDiagonal& sigmas = SharedDiagonal());
 
     /** Combine several GaussianConditional into a single dense GC.  The conditionals enumerated by
     *   \c first and \c last must be in increasing order, meaning that the parents of any