Scatter class in separate compilation unit

gtsam/linear/HessianFactor.cpp

@@ -53,55 +53,6 @@ namespace br { using namespace boost::range; using namespace boost::adaptors; }
 
 namespace gtsam {
 
-/* ************************************************************************* */
-string SlotEntry::toString() const {
-  ostringstream oss;
-  oss << "SlotEntry: slot=" << slot << ", dim=" << dimension;
-  return oss.str();
-}
-
-/* ************************************************************************* */
-Scatter::Scatter(const GaussianFactorGraph& gfg,
-    boost::optional<const Ordering&> ordering) {
-  gttic(Scatter_Constructor);
-  static const size_t none = std::numeric_limits<size_t>::max();
-
-  // First do the set union.
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) {
-    if (factor) {
-      for (GaussianFactor::const_iterator variable = factor->begin();
-          variable != factor->end(); ++variable) {
-        // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers
-        const JacobianFactor* asJacobian =
-            dynamic_cast<const JacobianFactor*>(factor.get());
-        if (!asJacobian || asJacobian->cols() > 1)
-          insert(
-              make_pair(*variable, SlotEntry(none, factor->getDim(variable))));
-      }
-    }
-  }
-
-  // If we have an ordering, pre-fill the ordered variables first
-  size_t slot = 0;
-  if (ordering) {
-    BOOST_FOREACH(Key key, *ordering) {
-      const_iterator entry = find(key);
-      if (entry == end())
-        throw std::invalid_argument(
-            "The ordering provided to the HessianFactor Scatter constructor\n"
-                "contained extra variables that did not appear in the factors to combine.");
-      at(key).slot = (slot++);
-    }
-  }
-
-  // Next fill in the slot indices (we can only get these after doing the set
-  // union.
-  BOOST_FOREACH(value_type& var_slot, *this) {
-    if (var_slot.second.slot == none)
-      var_slot.second.slot = (slot++);
-  }
-}
-
 /* ************************************************************************* */
 HessianFactor::HessianFactor() :
                           info_(cref_list_of<1>(1))

gtsam/linear/HessianFactor.h

@@ -18,10 +18,10 @@
 
 #pragma once
 
+#include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/linear/Scatter.h>
 #include <gtsam/base/SymmetricBlockMatrix.h>
 #include <gtsam/base/FastVector.h>
-#include <gtsam/base/FastMap.h>
-#include <gtsam/linear/GaussianFactor.h>
 
 #include <boost/make_shared.hpp>
 
@@ -41,30 +41,6 @@ namespace gtsam {
   GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
     EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys);
 
-  /**
-   * One SlotEntry stores the slot index for a variable, as well its dimension.
-   */
-  struct GTSAM_EXPORT SlotEntry {
-    size_t slot, dimension;
-    SlotEntry(size_t _slot, size_t _dimension)
-    : slot(_slot), dimension(_dimension) {}
-    std::string toString() const;
-  };
-
-  /**
-   * Scatter is an intermediate data structure used when building a HessianFactor
-   * incrementally, to get the keys in the right order. The "scatter" is a map from
-   * global variable indices to slot indices in the union of involved variables.
-   * We also include the dimensionality of the variable.
-   */
-  class Scatter: public FastMap<Key, SlotEntry> {
-  public:
-    Scatter() {
-    }
-    Scatter(const GaussianFactorGraph& gfg,
-        boost::optional<const Ordering&> ordering = boost::none);
-  };
-
   /**
    * @brief A Gaussian factor using the canonical parameters (information form)
    *

gtsam/linear/JacobianFactor.cpp

@@ -20,7 +20,7 @@
 #include <gtsam/linear/linearExceptions.h>
 #include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/linear/JacobianFactor.h>
-#include <gtsam/linear/HessianFactor.h>
+#include <gtsam/linear/Scatter.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/inference/VariableSlots.h>

gtsam/linear/Scatter.cpp

@@ -0,0 +1,77 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    HessianFactor.cpp
+ * @author  Richard Roberts
+ * @date    Dec 8, 2010
+ */
+
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/Scatter.h>
+#include <gtsam/inference/Ordering.h>
+
+#include <boost/foreach.hpp>
+
+using namespace std;
+
+namespace gtsam {
+
+/* ************************************************************************* */
+string SlotEntry::toString() const {
+  ostringstream oss;
+  oss << "SlotEntry: slot=" << slot << ", dim=" << dimension;
+  return oss.str();
+}
+
+/* ************************************************************************* */
+Scatter::Scatter(const GaussianFactorGraph& gfg,
+    boost::optional<const Ordering&> ordering) {
+  gttic(Scatter_Constructor);
+  static const size_t none = std::numeric_limits<size_t>::max();
+
+  // First do the set union.
+  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) {
+    if (factor) {
+      for (GaussianFactor::const_iterator variable = factor->begin();
+          variable != factor->end(); ++variable) {
+        // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers
+        const JacobianFactor* asJacobian =
+            dynamic_cast<const JacobianFactor*>(factor.get());
+        if (!asJacobian || asJacobian->cols() > 1)
+          insert(
+              make_pair(*variable, SlotEntry(none, factor->getDim(variable))));
+      }
+    }
+  }
+
+  // If we have an ordering, pre-fill the ordered variables first
+  size_t slot = 0;
+  if (ordering) {
+    BOOST_FOREACH(Key key, *ordering) {
+      const_iterator entry = find(key);
+      if (entry == end())
+        throw std::invalid_argument(
+            "The ordering provided to the HessianFactor Scatter constructor\n"
+                "contained extra variables that did not appear in the factors to combine.");
+      at(key).slot = (slot++);
+    }
+  }
+
+  // Next fill in the slot indices (we can only get these after doing the set
+  // union.
+  BOOST_FOREACH(value_type& var_slot, *this) {
+    if (var_slot.second.slot == none)
+      var_slot.second.slot = (slot++);
+  }
+}
+
+} // gtsam

gtsam/linear/Scatter.h

@@ -0,0 +1,56 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    Scatter.h
+ * @brief   Maps global variable indices to slot indices
+ * @author  Richard Roberts
+ * @author  Frank Dellaert
+ * @date    Dec 8, 2010
+ */
+
+#pragma once
+
+#include <gtsam/inference/Key.h>
+#include <gtsam/base/FastMap.h>
+#include <gtsam/dllexport.h>
+
+#include <boost/optional.hpp>
+
+namespace gtsam {
+
+class GaussianFactorGraph;
+class Ordering;
+
+/**
+ * One SlotEntry stores the slot index for a variable, as well its dimension.
+ */
+struct GTSAM_EXPORT SlotEntry {
+  size_t slot, dimension;
+  SlotEntry(size_t _slot, size_t _dimension)
+      : slot(_slot), dimension(_dimension) {}
+  std::string toString() const;
+};
+
+/**
+ * Scatter is an intermediate data structure used when building a HessianFactor
+ * incrementally, to get the keys in the right order. The "scatter" is a map
+ * from
+ * global variable indices to slot indices in the union of involved variables.
+ * We also include the dimensionality of the variable.
+ */
+class Scatter : public FastMap<Key, SlotEntry> {
+ public:
+  Scatter(const GaussianFactorGraph& gfg,
+          boost::optional<const Ordering&> ordering = boost::none);
+};
+
+}  // \ namespace gtsam

gtsam/linear/tests/testHessianFactor.cpp

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    testCholeskyFactor.cpp
+ * @file    testHessianFactor.cpp
  * @author  Richard Roberts
  * @date    Dec 15, 2010
  */

gtsam/linear/tests/testScatter.cpp

@@ -0,0 +1,63 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testScatter.cpp
+ * @author  Frank Dellaert
+ * @date    June, 2015
+ */
+
+#include <gtsam/linear/Scatter.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST(HessianFactor, CombineAndEliminate) {
+  static const size_t m = 3, n = 3;
+  Matrix A01 =
+      (Matrix(m, n) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished();
+  Vector3 b0(1.5, 1.5, 1.5);
+  Vector3 s0(1.6, 1.6, 1.6);
+
+  Matrix A10 =
+      (Matrix(m, n) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0).finished();
+  Matrix A11 = (Matrix(m, n) << -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, -2.0)
+                   .finished();
+  Vector3 b1(2.5, 2.5, 2.5);
+  Vector3 s1(2.6, 2.6, 2.6);
+
+  Matrix A21 =
+      (Matrix(m, n) << 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 3.0).finished();
+  Vector3 b2(3.5, 3.5, 3.5);
+  Vector3 s2(3.6, 3.6, 3.6);
+
+  GaussianFactorGraph gfg;
+  gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
+  gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
+  gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
+
+  Scatter scatter(gfg);
+  EXPECT_LONGS_EQUAL(2, scatter.size());
+  EXPECT_LONGS_EQUAL(0, scatter.at(0).slot);
+  EXPECT_LONGS_EQUAL(1, scatter.at(1).slot);
+  EXPECT_LONGS_EQUAL(n, scatter.at(0).dimension);
+  EXPECT_LONGS_EQUAL(n, scatter.at(1).dimension);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */