diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 9dbc2b52d..a5b3e4a9a 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/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 ordering) { - gttic(Scatter_Constructor); - static const size_t none = std::numeric_limits::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(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)) diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index c3cea3f51..50a81b579 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -18,10 +18,10 @@ #pragma once +#include +#include #include #include -#include -#include #include @@ -41,30 +41,6 @@ namespace gtsam { GTSAM_EXPORT std::pair, boost::shared_ptr > 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 { - public: - Scatter() { - } - Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering = boost::none); - }; - /** * @brief A Gaussian factor using the canonical parameters (information form) * diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index bb910aedb..9d0917919 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp new file mode 100644 index 000000000..3efbc2004 --- /dev/null +++ b/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 +#include +#include + +#include + +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 ordering) { + gttic(Scatter_Constructor); + static const size_t none = std::numeric_limits::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(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 diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h new file mode 100644 index 000000000..e212c5867 --- /dev/null +++ b/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 +#include +#include + +#include + +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 { + public: + Scatter(const GaussianFactorGraph& gfg, + boost::optional ordering = boost::none); +}; + +} // \ namespace gtsam diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 3a2cd0fd4..557ba3f36 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testCholeskyFactor.cpp + * @file testHessianFactor.cpp * @author Richard Roberts * @date Dec 15, 2010 */ diff --git a/gtsam/linear/tests/testScatter.cpp b/gtsam/linear/tests/testScatter.cpp new file mode 100644 index 000000000..19d099616 --- /dev/null +++ b/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 +#include +#include + +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); +} +/* ************************************************************************* */