From ee2f2e26be8406bf8ed01faef8333766b26016b6 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 18 Mar 2013 19:27:58 +0000 Subject: [PATCH] Moved findScatterAndDims to Scatter class constructor, moved code to combine multiple Hessians into Hessian constructor, and removed need to separately provide a vector of dimensions to this constructor - instead pulls dimensions out of Scatter. --- gtsam/linear/GaussianFactorGraph.cpp | 60 +----------------------- gtsam/linear/HessianFactor.cpp | 35 ++++++++++++-- gtsam/linear/HessianFactor.h | 13 +++-- gtsam/linear/tests/testHessianFactor.cpp | 10 +--- 4 files changed, 44 insertions(+), 74 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 1cac85693..3a494a2b4 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -326,51 +326,10 @@ break; return make_pair(gbn, jointFactor); } - /* ************************************************************************* */ - static FastMap findScatterAndDims(const FactorGraph& factors) { - - const bool debug = ISDEBUG("findScatterAndDims"); - - // 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. - - Scatter scatter; - - // First do the set union. - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - if(factor) { - for(GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) { - scatter.insert(make_pair(*variable, SlotEntry(0, factor->getDim(variable)))); - } - } - } - - // Next fill in the slot indices (we can only get these after doing the set - // union. - size_t slot = 0; - BOOST_FOREACH(Scatter::value_type& var_slot, scatter) { - var_slot.second.slot = (slot ++); - if(debug) - cout << "scatter[" << var_slot.first << "] = (slot " << var_slot.second.slot << ", dim " << var_slot.second.dimension << ")" << endl; - } - - return scatter; - } - /* ************************************************************************* */ Matrix GaussianFactorGraph::augmentedHessian() const { - - Scatter scatter = findScatterAndDims(*this); - - vector dims; dims.reserve(scatter.size() + 1); - BOOST_FOREACH(const Scatter::value_type& index_entry, scatter) { - dims.push_back(index_entry.second.dimension); - } - dims.push_back(1); - // combine all factors and get upper-triangular part of Hessian - HessianFactor combined(*this, dims, scatter); + HessianFactor combined(*this); Matrix result = combined.info(); // Fill in lower-triangular part of Hessian result.triangularView() = result.transpose(); @@ -391,24 +350,9 @@ break; const bool debug = ISDEBUG("EliminateCholesky"); - // Find the scatter and variable dimensions - gttic(find_scatter); - Scatter scatter(findScatterAndDims(factors)); - gttoc(find_scatter); - - // Pull out keys and dimensions - gttic(keys); - vector dimensions(scatter.size() + 1); - BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { - dimensions[var_slot.second.slot] = var_slot.second.dimension; - } - // This is for the r.h.s. vector - dimensions.back() = 1; - gttoc(keys); - // Form Ab' * Ab gttic(combine); - HessianFactor::shared_ptr combinedFactor(new HessianFactor(factors, dimensions, scatter)); + HessianFactor::shared_ptr combinedFactor(new HessianFactor(factors)); gttoc(combine); // Do Cholesky, note that after this, the lower triangle still contains diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 814d4eede..8ad026dfd 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -49,6 +49,25 @@ string SlotEntry::toString() const { return oss.str(); } +/* ************************************************************************* */ +Scatter::Scatter(const FactorGraph& gfg) { + // 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) { + this->insert(make_pair(*variable, SlotEntry(0, factor->getDim(variable)))); + } + } + } + + // Next fill in the slot indices (we can only get these after doing the set + // union. + size_t slot = 0; + BOOST_FOREACH(value_type& var_slot, *this) { + var_slot.second.slot = (slot ++); + } +} + /* ************************************************************************* */ void HessianFactor::assertInvariants() const { GaussianFactor::assertInvariants(); // The base class checks for unique keys @@ -245,9 +264,19 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) : info_(matrix_) { } /* ************************************************************************* */ -HessianFactor::HessianFactor(const FactorGraph& factors, - const vector& dimensions, const Scatter& scatter) : - info_(matrix_) { +HessianFactor::HessianFactor(const FactorGraph& factors) : info_(matrix_) +{ + Scatter scatter(factors); + + // Pull out keys and dimensions + gttic(keys); + vector dimensions(scatter.size() + 1); + BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { + dimensions[var_slot.second.slot] = var_slot.second.dimension; + } + // This is for the r.h.s. vector + dimensions.back() = 1; + gttoc(keys); const bool debug = ISDEBUG("EliminateCholesky"); // Form Ab' * Ab diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index bbd3064c6..cf187d371 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -39,6 +39,9 @@ namespace gtsam { // Definition of Scatter, which 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. struct GTSAM_EXPORT SlotEntry { size_t slot; size_t dimension; @@ -46,7 +49,10 @@ namespace gtsam { : slot(_slot), dimension(_dimension) {} std::string toString() const; }; - typedef FastMap Scatter; + class Scatter : public FastMap { + public: + Scatter(const FactorGraph& gfg); + }; /** * @brief A Gaussian factor using the canonical parameters (information form) @@ -187,9 +193,8 @@ namespace gtsam { /** Convert from a JacobianFactor (computes A^T * A) or HessianFactor */ explicit HessianFactor(const GaussianFactor& factor); - /** Special constructor used in EliminateCholesky which combines the given factors */ - HessianFactor(const FactorGraph& factors, - const std::vector& dimensions, const Scatter& scatter); + /** Combine a set of factors into a single dense HessianFactor */ + HessianFactor(const FactorGraph& factors); /** Destructor */ virtual ~HessianFactor() {} diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index caaff6748..dccbd17da 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -615,16 +615,8 @@ TEST(HessianFactor, combine) { FactorGraph factors; factors.push_back(f); - vector dimensions; - dimensions += 2, 2, 2, 1; - - Scatter scatter; - scatter += make_pair(0, SlotEntry(0, 2)); - scatter += make_pair(1, SlotEntry(1, 2)); - scatter += make_pair(2, SlotEntry(2, 2)); - // Form Ab' * Ab - HessianFactor actual(factors, dimensions, scatter); + HessianFactor actual(factors); Matrix expected = Matrix_(7, 7, 125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000,