From c695b23e36e5eca277de97f89930cd6d5afcfecc Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 18 Mar 2012 05:13:40 +0000 Subject: [PATCH] In progress - updating dogleg computations incrementally --- gtsam/linear/GaussianConditional.cpp | 36 -------------------------- gtsam/linear/VectorValues.h | 38 ++++++++++++++++++++++++++++ gtsam/nonlinear/ISAM2-impl.cpp | 35 +++++++++++++++++++++++++ gtsam/nonlinear/ISAM2-impl.h | 3 +++ gtsam/nonlinear/ISAM2.cpp | 4 +-- gtsam/nonlinear/ISAM2.h | 10 ++++---- 6 files changed, 83 insertions(+), 43 deletions(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 233dc4b34..0d6c8fc73 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -27,42 +27,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -// Helper function used only in this file - extracts vectors with variable indices -// in the first and last iterators, and concatenates them in that order into the -// output. -template -static Vector extractVectorValuesSlices(const VALUES& values, ITERATOR first, ITERATOR last) { - // Find total dimensionality - int dim = 0; - for(ITERATOR j = first; j != last; ++j) - dim += values[*j].rows(); - - // Copy vectors - Vector ret(dim); - int varStart = 0; - for(ITERATOR j = first; j != last; ++j) { - ret.segment(varStart, values[*j].rows()) = values[*j]; - varStart += values[*j].rows(); - } - return ret; -} - -/* ************************************************************************* */ -// Helper function used only in this file - writes to the variables in values -// with indices iterated over by first and last, interpreting vector as the -// concatenated vectors to write. -template -static void writeVectorValuesSlices(const VECTOR& vector, VALUES& values, ITERATOR first, ITERATOR last) { - // Copy vectors - int varStart = 0; - for(ITERATOR j = first; j != last; ++j) { - values[*j] = vector.segment(varStart, values[*j].rows()); - varStart += values[*j].rows(); - } - assert(varStart == vector.rows()); -} - /* ************************************************************************* */ GaussianConditional::GaussianConditional() : rsd_(matrix_) {} diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 0be2cad19..479710885 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -402,4 +402,42 @@ namespace gtsam { #endif } + namespace internal { + /* ************************************************************************* */ + // Helper function, extracts vectors with variable indices + // in the first and last iterators, and concatenates them in that order into the + // output. + template + Vector extractVectorValuesSlices(const VALUES& values, ITERATOR first, ITERATOR last) { + // Find total dimensionality + int dim = 0; + for(ITERATOR j = first; j != last; ++j) + dim += values[*j].rows(); + + // Copy vectors + Vector ret(dim); + int varStart = 0; + for(ITERATOR j = first; j != last; ++j) { + ret.segment(varStart, values[*j].rows()) = values[*j]; + varStart += values[*j].rows(); + } + return ret; + } + + /* ************************************************************************* */ + // Helper function, writes to the variables in values + // with indices iterated over by first and last, interpreting vector as the + // concatenated vectors to write. + template + void writeVectorValuesSlices(const VECTOR& vector, VALUES& values, ITERATOR first, ITERATOR last) { + // Copy vectors + int varStart = 0; + for(ITERATOR j = first; j != last; ++j) { + values[*j] = vector.segment(varStart, values[*j].rows()); + varStart += values[*j].rows(); + } + assert(varStart == vector.rows()); + } + } + } // \namespace gtsam diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 807111753..13029493e 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -299,4 +299,39 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std: return lastBacksubVariableCount; } +/* ************************************************************************* */ +namespace internal { +size_t updateDoglegDeltas(const boost::shared_ptr& clique, std::vector& replacedKeys, + const VectorValues& grad, Permuted& deltaNewton, Permuted& RgProd, vector& updated) { + + + + // Update the current variable + // Get VectorValues slice corresponding to current variables + Vector gR = internal::extractVectorValuesSlices(grad, (*clique)->beginFrontals(), (*clique)->endFrontals()); + Vector gS = internal::extractVectorValuesSlices(grad, (*clique)->beginParents(), (*clique)->endParents()); + + // Compute R*g and S*g for this clique + Vector RSgProd = ((*clique)->get_R() * (*clique)->permutation().transpose()) * gR + (*clique)->get_S() * gS; + + // Write into RgProd vector + internal::writeVectorValuesSlices(RSgProd, RgProd, (*clique)->begin(), (*clique)->end()); +} +} + +/* ************************************************************************* */ +size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, std::vector& replacedKeys, + Permuted& deltaNewton, Permuted& RgProd) { + + // Keep a set of flags for whether each variable has been updated. + vector updated(replacedKeys.size()); + + // Get gradient + VectorValues grad = *allocateVectorValues(isam); + gradientAtZero(isam, grad); + + // Update variables + return internal::updateDoglegDeltas(root, replacedKeys, grad, deltaNewton, RgProd, updated); +} + } diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index be3f010e3..a82623349 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -126,6 +126,9 @@ struct ISAM2::Impl { static size_t UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold); + static size_t UpdateDoglegDeltas(const ISAM2& isam, std::vector& replacedKeys, + const VectorValues& grad, Permuted& deltaNewton, Permuted& RgProd); + }; } diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index f302e174e..6b66c1ce3 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -660,7 +660,7 @@ void optimizeGradientSearchInPlace(const ISAM2& Rd, VectorValues& grad) { } /* ************************************************************************* */ -VectorValues gradient(const BayesTree& bayesTree, const VectorValues& x0) { +VectorValues gradient(const ISAM2& bayesTree, const VectorValues& x0) { return gradient(FactorGraph(bayesTree), x0); } @@ -682,7 +682,7 @@ static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, } /* ************************************************************************* */ -void gradientAtZero(const BayesTree& bayesTree, VectorValues& g) { +void gradientAtZero(const ISAM2& bayesTree, VectorValues& g) { // Zero-out gradient g.setZero(); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 535f21c75..8d5c49268 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -293,9 +293,9 @@ protected: mutable Permuted delta_; VectorValues deltaNewtonUnpermuted_; - mutable Permuted deltaNewtonUnpermuted_; - VectorValues deltaGradSearchUnpermuted_; - mutable Permuted deltaGradSearchUnpermuted_; + mutable Permuted deltaNewton_; + VectorValues RgProdUnpermuted_; + mutable Permuted RgProd_; /** Indicates whether the current delta is up-to-date, only used * internally - delta will always be updated if necessary when it is @@ -527,7 +527,7 @@ int calculate_nnz(const boost::shared_ptr& clique); * @param x0 The center about which to compute the gradient * @return The gradient as a VectorValues */ -VectorValues gradient(const BayesTree& bayesTree, const VectorValues& x0); +VectorValues gradient(const ISAM2& bayesTree, const VectorValues& x0); /** * Compute the gradient of the energy function, @@ -540,7 +540,7 @@ VectorValues gradient(const BayesTree& bayesTr * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues * @return The gradient as a VectorValues */ -void gradientAtZero(const BayesTree& bayesTree, VectorValues& g); +void gradientAtZero(const ISAM2& bayesTree, VectorValues& g); } /// namespace gtsam