From 223f89d931cad2a3fb8e1cb0be68031a5d6c2353 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 16 Jul 2013 20:22:07 +0000
Subject: [PATCH] Allow starting with empty VectorValues in
 JF::transposeMultiplyAdd

---
 gtsam/linear/JacobianFactorUnordered.cpp | 11 +++++++++--
 gtsam/linear/JacobianFactorUnordered.h   |  3 ++-
 2 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/gtsam/linear/JacobianFactorUnordered.cpp b/gtsam/linear/JacobianFactorUnordered.cpp
index 3d0dfa6e2..3fbe3043e 100644
--- a/gtsam/linear/JacobianFactorUnordered.cpp
+++ b/gtsam/linear/JacobianFactorUnordered.cpp
@@ -423,11 +423,18 @@ namespace gtsam {
 
   /* ************************************************************************* */
   void JacobianFactorUnordered::transposeMultiplyAdd(double alpha, const Vector& e,
-      VectorValuesUnordered& x) const {
+      VectorValuesUnordered& x) const
+  {
     Vector E = alpha * model_->whiten(e);
     // Just iterate over all A matrices and insert Ai^e into VectorValues
     for(size_t pos=0; pos<size(); ++pos)
-      gtsam::transposeMultiplyAdd(1.0, Ab_(pos), E, x[keys_[pos]]);
+    {
+      // Create the value as a zero vector if it does not exist.
+      pair<VectorValuesUnordered::iterator, bool> xi = x.tryInsert(keys_[pos], Vector());
+      if(xi.second)
+        xi.first->second = Vector::Zero(getDim(begin() + pos));
+      gtsam::transposeMultiplyAdd(1.0, Ab_(pos), E, xi.first->second);
+    }
   }
 
   /* ************************************************************************* */
diff --git a/gtsam/linear/JacobianFactorUnordered.h b/gtsam/linear/JacobianFactorUnordered.h
index d72dd568c..56b0922b7 100644
--- a/gtsam/linear/JacobianFactorUnordered.h
+++ b/gtsam/linear/JacobianFactorUnordered.h
@@ -245,7 +245,8 @@ namespace gtsam {
     /** Return A*x */
     Vector operator*(const VectorValuesUnordered& x) const;
 
-    /** x += A'*e */
+    /** x += A'*e.  If x is initially missing any values, they are created and assumed to start as
+     *  zero vectors. */
     void transposeMultiplyAdd(double alpha, const Vector& e, VectorValuesUnordered& x) const;
 
     /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */