diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 3ace299cb..98e74abd7 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -367,10 +367,8 @@ namespace gtsam { // Pull out keys and dimensions tic(2, "keys"); - vector keys(scatter.size()); vector dimensions(scatter.size() + 1); BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { - keys[var_slot.second.slot] = var_slot.first; dimensions[var_slot.second.slot] = var_slot.second.dimension; } // This is for the r.h.s. vector @@ -401,7 +399,7 @@ namespace gtsam { // Extract conditionals and fill in details of the remaining factor tic(5, "split"); GaussianConditional::shared_ptr conditionals = - combinedFactor->splitEliminatedFactor(nrFrontals, keys); + combinedFactor->splitEliminatedFactor(nrFrontals); if (debug) { conditionals->print("Extracted conditionals: "); combinedFactor->print("Eliminated factor (L piece): "); @@ -572,10 +570,8 @@ namespace gtsam { // Pull out keys and dimensions tic(2, "keys"); - vector keys(scatter.size()); vector dimensions(scatter.size() + 1); BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { - keys[var_slot.second.slot] = var_slot.first; dimensions[var_slot.second.slot] = var_slot.second.dimension; } // This is for the r.h.s. vector @@ -609,7 +605,7 @@ namespace gtsam { // Extract conditionals and fill in details of the remaining factor tic(5, "split"); GaussianConditional::shared_ptr conditionals = - combinedFactor->splitEliminatedFactor(nrFrontals, keys, permutation); + combinedFactor->splitEliminatedFactor(nrFrontals, permutation); if (debug) { conditionals->print("Extracted conditionals: "); combinedFactor->print("Eliminated factor (L piece): "); diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index da57e302b..5cba4f4dc 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -193,6 +194,9 @@ HessianFactor::HessianFactor(const FactorGraph& factors, // Form Ab' * Ab tic(1, "allocate"); info_.resize(dimensions.begin(), dimensions.end(), false); + // Fill in keys + keys_.resize(scatter.size()); + std::transform(scatter.begin(), scatter.end(), keys_.begin(), boost::bind(&Scatter::value_type::first, ::_1)); toc(1, "allocate"); tic(2, "zero"); matrix_.noalias() = Matrix::Zero(matrix_.rows(),matrix_.cols()); @@ -406,7 +410,7 @@ Eigen::LDLT::TranspositionType HessianFactor::partialLDL(size_t nrFronta /* ************************************************************************* */ GaussianConditional::shared_ptr -HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector& keys, const Eigen::LDLT::TranspositionType& permutation) { +HessianFactor::splitEliminatedFactor(size_t nrFrontals, const Eigen::LDLT::TranspositionType& permutation) { static const bool debug = false; @@ -423,7 +427,7 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector& key // Because of the pivoting permutation when using LDL, treating each variable separately doesn't make sense. tic(2, "construct cond"); Vector sigmas = Vector::Ones(varDim); - conditionals = boost::make_shared(keys.begin(), keys.end(), nrFrontals, Ab, sigmas, permutation); + conditionals = boost::make_shared(keys_.begin(), keys_.end(), nrFrontals, Ab, sigmas, permutation); toc(2, "construct cond"); if(debug) conditionals->print("Extracted conditional: "); @@ -433,7 +437,9 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector& key tic(2, "remaining factor"); info_.blockStart() = nrFrontals; // Assign the keys - keys_.assign(keys.begin() + nrFrontals, keys.end()); + vector remainingKeys(keys_.size() - nrFrontals); + remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end()); + keys_.swap(remainingKeys); toc(2, "remaining factor"); return conditionals; diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 88e4364ae..2af208a59 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -276,7 +276,7 @@ namespace gtsam { /** split partially eliminated factor */ boost::shared_ptr splitEliminatedFactor( - size_t nrFrontals, const std::vector& keys, const Eigen::LDLT::TranspositionType& permutation = Eigen::LDLT::TranspositionType()); + size_t nrFrontals, const Eigen::LDLT::TranspositionType& permutation = Eigen::LDLT::TranspositionType()); /** assert invariants */ void assertInvariants() const;