From bdbc09ba425175fa4b7faef581711d30bd58b74e Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 4 Feb 2011 03:56:47 +0000
Subject: [PATCH] Added GaussianFactorGraph::sparseJacobian function to create
 a sparse Jacobian matrix suitable for Matlab

---
 gtsam/linear/GaussianFactorGraph.cpp | 29 +++++++++++++++++++++++
 gtsam/linear/GaussianFactorGraph.h   |  9 +++++++
 gtsam/linear/JacobianFactor.cpp      | 35 +++++++++++++---------------
 gtsam/linear/JacobianFactor.h        |  6 ++---
 4 files changed, 57 insertions(+), 22 deletions(-)

diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 3be157d05..b442572b2 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -27,6 +27,7 @@
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/inference/FactorGraph-inl.h>
 #include <gtsam/linear/iterative.h>
+#include <gtsam/linear/HessianFactor.h>
 
 
 using namespace std;
@@ -82,6 +83,34 @@ namespace gtsam {
     return fg;
   }
 
+  /* ************************************************************************* */
+  std::vector<boost::tuple<size_t,size_t,double> >
+  GaussianFactorGraph::sparseJacobian(const std::vector<size_t>& columnIndices) const {
+    std::vector<boost::tuple<size_t,size_t,double> > entries;
+    size_t i = 0;
+    BOOST_FOREACH(const sharedFactor& factor, *this) {
+      // Convert to JacobianFactor if necessary
+      JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast<JacobianFactor>(factor));
+      if(!jacobianFactor) {
+        HessianFactor::shared_ptr hessianFactor(boost::dynamic_pointer_cast<HessianFactor>(factor));
+        if(hessianFactor)
+          jacobianFactor.reset(new JacobianFactor(*hessianFactor));
+        else
+          throw invalid_argument("GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor.");
+      }
+
+      // Add entries, adjusting the row index i
+      std::vector<boost::tuple<size_t,size_t,double> > factorEntries(jacobianFactor->sparse(columnIndices));
+      entries.reserve(entries.size() + factorEntries.size());
+      for(size_t entry=0; entry<factorEntries.size(); ++entry)
+        entries.push_back(boost::make_tuple(factorEntries[entry].get<0>()+i, factorEntries[entry].get<1>(), factorEntries[entry].get<2>()));
+
+      // Increment row index
+      i += jacobianFactor->size1();
+    }
+    return entries;
+  }
+
 //  VectorValues GaussianFactorGraph::allocateVectorValuesb() const {
 //    std::vector<size_t> dimensions(size()) ;
 //    Index i = 0 ;
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index b86b58696..4d260852a 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -20,6 +20,7 @@
 #pragma once
 
 #include <boost/shared_ptr.hpp>
+#include <boost/tuple/tuple.hpp>
 
 #include <gtsam/base/FastSet.h>
 #include <gtsam/inference/FactorGraph.h>
@@ -120,6 +121,14 @@ namespace gtsam {
      */
     void combine(const GaussianFactorGraph &lfg);
 
+    /**
+     * Return vector of i, j, and s to generate an m-by-n sparse Jacobain matrix
+     * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse.
+     * The standard deviations are baked into A and b
+     * @param first column index for each variable
+     */
+    std::vector<boost::tuple<size_t,size_t,double> > sparseJacobian(const std::vector<size_t>& columnIndices) const;
+
     // get b
 //    void getb(VectorValues &b) const ;
 //    VectorValues getb() const ;
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index e13da78e1..bf3f193cd 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -337,31 +337,28 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  boost::tuple<list<int>, list<int>, list<double> >
-  JacobianFactor::sparse(const map<Index,size_t>& columnIndices) const {
+  std::vector<boost::tuple<size_t, size_t, double> >
+  JacobianFactor::sparse(const std::vector<size_t>& columnIndices) const {
 
-    // declare return values
-    list<int> I,J;
-    list<double> S;
+    std::vector<boost::tuple<size_t, size_t, double> > entries;
 
-    // iterate over all matrices in the factor
-    for(size_t pos=0; pos<keys_.size(); ++pos) {
-      constABlock A(Ab_(pos));
+    // iterate over all variables in the factor
+    for(const_iterator var=begin(); var<end(); ++var) {
+      Matrix whitenedA(model_->Whiten(getA(var)));
       // find first column index for this key
-      int column_start = columnIndices.at(keys_[pos]);
-      for (size_t i = 0; i < A.size1(); i++) {
-        double sigma_i = model_->sigma(i);
-        for (size_t j = 0; j < A.size2(); j++)
-          if (A(i, j) != 0.0) {
-            I.push_back(i + 1);
-            J.push_back(j + column_start);
-            S.push_back(A(i, j) / sigma_i);
-          }
-      }
+      size_t column_start = columnIndices[*var];
+      for (size_t i = 0; i < whitenedA.size1(); i++)
+        for (size_t j = 0; j < whitenedA.size2(); j++)
+          entries.push_back(boost::make_tuple(i, column_start+j, whitenedA(i,j)));
     }
 
+    Vector whitenedb(model_->whiten(getb()));
+    size_t bcolumn = columnIndices.back();
+    for (size_t i = 0; i < whitenedb.size(); i++)
+      entries.push_back(boost::make_tuple(i, bcolumn, whitenedb(i)));
+
     // return the result
-    return boost::tuple<list<int>, list<int>, list<double> >(I,J,S);
+    return entries;
   }
 
   /* ************************************************************************* */
diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h
index caf678171..5b8e769ce 100644
--- a/gtsam/linear/JacobianFactor.h
+++ b/gtsam/linear/JacobianFactor.h
@@ -178,13 +178,13 @@ namespace gtsam {
     Matrix matrix_augmented(bool weight = true) const;
 
     /**
-     * Return vectors i, j, and s to generate an m-by-n sparse matrix
+     * Return vector of i, j, and s to generate an m-by-n sparse matrix
      * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse.
      * As above, the standard deviations are baked into A and b
      * @param first column index for each variable
      */
-    boost::tuple<std::list<int>, std::list<int>, std::list<double> >
-    sparse(const std::map<Index, size_t>& columnIndices) const;
+    std::vector<boost::tuple<size_t, size_t, double> >
+    sparse(const std::vector<size_t>& columnIndices) const;
 
     /**
      * Return a whitened version of the factor, i.e. with unit diagonal noise