Merge pull request #677 from borglab/feature/sparseJacobian3

additional `sparseJacobian` functions (new)

gtsam/linear/SparseEigen.h

@@ -0,0 +1,119 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file SparseEigen.h
+ *
+ * @brief Utilities for creating Eigen sparse matrices (gtsam::SparseEigen)
+ *
+ * @date Aug 2019
+ * @author Mandy Xie
+ * @author Fan Jiang
+ * @author Gerry Chen
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/VectorValues.h>
+
+#include <Eigen/Sparse>
+
+namespace gtsam {
+
+typedef Eigen::SparseMatrix<double> SparseEigen;
+
+/// Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph
+SparseEigen sparseJacobianEigen(
+    const GaussianFactorGraph &gfg, const Ordering &ordering) {
+  // TODO(gerry): eliminate copy/pasta by making GaussianFactorGraph version
+  // more general, or by creating an Eigen::Triplet compatible wrapper for
+  // boost::tuple return type
+
+  // First find dimensions of each variable
+  std::map<Key, size_t> dims;
+  for (const boost::shared_ptr<GaussianFactor> &factor : gfg) {
+    if (!static_cast<bool>(factor)) continue;
+
+    for (auto it = factor->begin(); it != factor->end(); ++it) {
+      dims[*it] = factor->getDim(it);
+    }
+  }
+
+  // Compute first scalar column of each variable
+  size_t currentColIndex = 0;
+  std::map<Key, size_t> columnIndices;
+  for (const auto key : ordering) {
+    columnIndices[key] = currentColIndex;
+    currentColIndex += dims[key];
+  }
+
+  // Iterate over all factors, adding sparse scalar entries
+  std::vector<Eigen::Triplet<double>> entries;
+  entries.reserve(60 * gfg.size());
+
+  size_t row = 0;
+  for (const boost::shared_ptr<GaussianFactor> &factor : gfg) {
+    if (!static_cast<bool>(factor)) continue;
+
+    // Convert to JacobianFactor if necessary
+    JacobianFactor::shared_ptr jacobianFactor(
+        boost::dynamic_pointer_cast<JacobianFactor>(factor));
+    if (!jacobianFactor) {
+      HessianFactor::shared_ptr hessian(
+          boost::dynamic_pointer_cast<HessianFactor>(factor));
+      if (hessian)
+        jacobianFactor.reset(new JacobianFactor(*hessian));
+      else
+        throw std::invalid_argument(
+            "GaussianFactorGraph contains a factor that is neither a "
+            "JacobianFactor nor a HessianFactor.");
+    }
+
+    // Whiten the factor and add entries for it
+    // iterate over all variables in the factor
+    const JacobianFactor whitened(jacobianFactor->whiten());
+    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[*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);
+          if (std::abs(s) > 1e-12)
+            entries.emplace_back(row + i, column_start + j, s);
+        }
+    }
+
+    JacobianFactor::constBVector whitenedb(whitened.getb());
+    size_t bcolumn = currentColIndex;
+    for (size_t i = 0; i < (size_t)whitenedb.size(); i++) {
+      double s = whitenedb(i);
+      if (std::abs(s) > 1e-12) entries.emplace_back(row + i, bcolumn, s);
+    }
+
+    // Increment row index
+    row += jacobianFactor->rows();
+  }
+
+  // ...and make a sparse matrix with it.
+  SparseEigen Ab(row, currentColIndex + 1);
+  Ab.setFromTriplets(entries.begin(), entries.end());
+  return Ab;
+}
+
+SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg) {
+  return sparseJacobianEigen(gfg, Ordering(gfg.keys()));
+}
+
+}  // namespace gtsam

gtsam/linear/tests/testGaussianFactorGraph.cpp

@@ -36,9 +36,18 @@ using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
 
-// static SharedDiagonal
+typedef boost::tuple<size_t, size_t, double> BoostTriplet;
-//  sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2),
+bool triplet_equal(BoostTriplet a, BoostTriplet b) {
-//  constraintModel = noiseModel::Constrained::All(2);
+  if (a.get<0>() == b.get<0>() && a.get<1>() == b.get<1>() &&
+      a.get<2>() == b.get<2>()) return true;
+
+  cout << "not equal:" << endl;
+  cout << "\texpected: "
+      "(" << a.get<0>() << ", " << a.get<1>() << ") = " << a.get<2>() << endl;
+  cout << "\tactual:   "
+      "(" << b.get<0>() << ", " << b.get<1>() << ") = " << b.get<2>() << endl;
+  return false;
+}
 
 /* ************************************************************************* */
 TEST(GaussianFactorGraph, initialization) {
@@ -74,8 +83,8 @@ TEST(GaussianFactorGraph, sparseJacobian) {
   //  9 10  0 11 12 13
   //  0  0  0 14 15 16
 
-  // Expected - NOTE that we transpose this!
+  // Expected
-  Matrix expectedT = (Matrix(16, 3) <<
+  Matrix expected = (Matrix(16, 3) <<
       1., 1., 2.,
       1., 2., 4.,
       1., 3., 6.,
@@ -93,17 +102,32 @@ TEST(GaussianFactorGraph, sparseJacobian) {
       3., 6.,26.,
       4., 6.,32.).finished();
 
-  Matrix expected = expectedT.transpose();
+  // expected: in matlab format - NOTE the transpose!)
+  Matrix expectedMatlab = expected.transpose();
 
   GaussianFactorGraph gfg;
   SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
-  gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.).finished(), Vector2(4., 8.), model);
+  const Key x123 = 0, x45 = 1;
-  gfg.add(0, (Matrix(2, 3) << 9., 10., 0., 0., 0., 0.).finished(), 1,
+  gfg.add(x123, (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(),
-          (Matrix(2, 2) << 11., 12., 14., 15.).finished(), Vector2(13., 16.), model);
+          Vector2(4, 8), model);
+  gfg.add(x123, (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(),
+          x45,  (Matrix(2, 2) << 11, 12, 14, 15.).finished(),
+          Vector2(13, 16), model);
 
   Matrix actual = gfg.sparseJacobian_();
 
-  EXPECT(assert_equal(expected, actual));
+  EXPECT(assert_equal(expectedMatlab, actual));
+
+  // BoostTriplets
+  auto boostActual = gfg.sparseJacobian();
+  // check the triplets size...
+  EXPECT_LONGS_EQUAL(16, boostActual.size());
+  // check content
+  for (int i = 0; i < 16; i++) {
+    EXPECT(triplet_equal(
+        BoostTriplet(expected(i, 0) - 1, expected(i, 1) - 1, expected(i, 2)),
+        boostActual.at(i)));
+  }
 }
 
 /* ************************************************************************* */

gtsam/linear/tests/testSparseEigen.cpp

@@ -0,0 +1,72 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testSparseMatrix.cpp
+ * @author  Mandy Xie
+ * @author  Fan Jiang
+ * @author  Gerry Chen
+ * @author  Frank Dellaert
+ * @date    Jan, 2021
+ */
+
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/SparseEigen.h>
+
+#include <boost/assign/list_of.hpp>
+using boost::assign::list_of;
+
+#include <gtsam/base/TestableAssertions.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST(SparseEigen, sparseJacobianEigen) {
+  GaussianFactorGraph gfg;
+  SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
+  const Key x123 = 0, x45 = 1;
+  gfg.add(x123, (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(),
+          Vector2(4, 8), model);
+  gfg.add(x123, (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(),
+          x45,  (Matrix(2, 2) << 11, 12, 14, 15.).finished(),
+          Vector2(13, 16), model);
+
+  // Sparse Matrix
+  auto sparseResult = sparseJacobianEigen(gfg);
+  EXPECT_LONGS_EQUAL(16, sparseResult.nonZeros());
+  EXPECT(assert_equal(4, sparseResult.rows()));
+  EXPECT(assert_equal(6, sparseResult.cols()));
+  EXPECT(assert_equal(gfg.augmentedJacobian(), Matrix(sparseResult)));
+
+  // Call sparseJacobian with optional ordering...
+  auto ordering = Ordering(list_of(x45)(x123));
+
+  // Eigen Sparse with optional ordering
+  EXPECT(assert_equal(gfg.augmentedJacobian(ordering),
+                      Matrix(sparseJacobianEigen(gfg, ordering))));
+
+  // Check matrix dimensions when zero rows / cols
+  gfg.add(x123, Matrix23::Zero(), Vector2::Zero(), model);  // zero row
+  gfg.add(2, Matrix21::Zero(), Vector2::Zero(), model);     // zero col
+  sparseResult = sparseJacobianEigen(gfg);
+  EXPECT_LONGS_EQUAL(16, sparseResult.nonZeros());
+  EXPECT(assert_equal(8, sparseResult.rows()));
+  EXPECT(assert_equal(7, sparseResult.cols()));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */