From 0550932235a2266b1f724541565651f70bd2d93b Mon Sep 17 00:00:00 2001
From: Mike Bosse <mike.bosse@mavt.ethz.ch>
Date: Wed, 4 Mar 2015 11:12:16 +0100
Subject: [PATCH] fixed sparseJacobian() to use a std::map such that it works
 with symbol keys

---
 gtsam/linear/GaussianFactorGraph.cpp | 34 ++++++++++++++++------------
 1 file changed, 20 insertions(+), 14 deletions(-)

diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 6953d2969..cd60a3eb9 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -123,26 +123,32 @@ namespace gtsam {
   /* ************************************************************************* */
   vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const {
     // First find dimensions of each variable
-    vector<size_t> dims;
+    typedef std::map<Key, size_t> KeySizeMap;
+    KeySizeMap dims;
     BOOST_FOREACH(const sharedFactor& factor, *this) {
-      for (GaussianFactor::const_iterator pos = factor->begin();
-          pos != factor->end(); ++pos) {
-        if (dims.size() <= *pos)
-          dims.resize(*pos + 1, 0);
-        dims[*pos] = factor->getDim(pos);
+      if (!static_cast<bool>(factor)) continue;
+
+      for (GaussianFactor::const_iterator key = factor->begin();
+          key != factor->end(); ++key) {
+        dims[*key] = factor->getDim(key);
       }
     }
 
     // Compute first scalar column of each variable
-    vector<size_t> columnIndices(dims.size() + 1, 0);
-    for (size_t j = 1; j < dims.size() + 1; ++j)
-      columnIndices[j] = columnIndices[j - 1] + dims[j - 1];
+    size_t currentColIndex = 0;
+    KeySizeMap columnIndices = dims;
+    BOOST_FOREACH(const KeySizeMap::value_type& col, dims) {
+      columnIndices[col.first] = currentColIndex;
+      currentColIndex += dims[col.first];
+    }
 
     // Iterate over all factors, adding sparse scalar entries
     typedef boost::tuple<size_t, size_t, double> triplet;
     vector<triplet> entries;
     size_t row = 0;
     BOOST_FOREACH(const sharedFactor& factor, *this) {
+      if (!static_cast<bool>(factor)) continue;
+
       // Convert to JacobianFactor if necessary
       JacobianFactor::shared_ptr jacobianFactor(
           boost::dynamic_pointer_cast<JacobianFactor>(factor));
@@ -159,11 +165,11 @@ namespace gtsam {
       // Whiten the factor and add entries for it
       // iterate over all variables in the factor
       const JacobianFactor whitened(jacobianFactor->whiten());
-      for (JacobianFactor::const_iterator pos = whitened.begin();
-          pos < whitened.end(); ++pos) {
-        JacobianFactor::constABlock whitenedA = whitened.getA(pos);
+      for (JacobianFactor::const_iterator key = whitened.begin();
+          key < whitened.end(); ++key) {
+        JacobianFactor::constABlock whitenedA = whitened.getA(key);
         // find first column index for this key
-        size_t column_start = columnIndices[*pos];
+        size_t column_start = columnIndices[*key];
         for (size_t i = 0; i < (size_t) whitenedA.rows(); i++)
           for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) {
             double s = whitenedA(i, j);
@@ -173,7 +179,7 @@ namespace gtsam {
       }
 
       JacobianFactor::constBVector whitenedb(whitened.getb());
-      size_t bcolumn = columnIndices.back();
+      size_t bcolumn = currentColIndex;
       for (size_t i = 0; i < (size_t) whitenedb.size(); i++)
         entries.push_back(boost::make_tuple(row + i, bcolumn, whitenedb(i)));