diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 9806b435c..e05fbe45c 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -213,6 +214,16 @@ double determinant(const GaussianBayesNet& bayesNet) { return exp(logDet); } +/* ************************************************************************* */ +VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0) { + return gradient(FactorGraph(bayesNet), x0); +} + +/* ************************************************************************* */ +void gradientAtZero(const GaussianBayesNet& bayesNet, VectorValues& g) { + gradientAtZero(FactorGraph(bayesNet), g); +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 171d061ee..455536a4f 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -64,19 +64,19 @@ namespace gtsam { */ boost::shared_ptr optimize_(const GaussianBayesNet& bn); - /* + /** * Backsubstitute * (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) * @param y is the RHS of the system */ VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& y); - /* + /** * Backsubstitute in place, y starts as RHS and is replaced with solution */ void backSubstituteInPlace(const GaussianBayesNet& bn, VectorValues& y); - /* + /** * Transpose Backsubstitute * gy=inv(L)*gx by solving L*gy=gx. * gy=inv(R'*inv(Sigma))*gx @@ -108,4 +108,26 @@ namespace gtsam { */ double determinant(const GaussianBayesNet& bayesNet); + /** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, + * centered around \f$ x = x_0 \f$. + * The gradient is \f$ R^T(Rx-d) \f$. + * @param bayesNet The Gaussian Bayes net $(R,d)$ + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues + */ + VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0); + + /** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, + * centered around zero. + * The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&). + * @param bayesNet The Gaussian Bayes net $(R,d)$ + * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues + * @return The gradient as a VectorValues + */ + void gradientAtZero(const GaussianBayesNet& bayesNet, VectorValues& g); + } /// namespace gtsam diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index c20dab3c1..3347fa4f9 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -582,17 +582,28 @@ namespace gtsam { } /* ************************************************************************* */ - VectorValues gradient(const FactorGraph& fg, const VectorValues& x) { + VectorValues gradient(const FactorGraph& fg, const VectorValues& x0) { // It is crucial for performance to make a zero-valued clone of x - VectorValues g = VectorValues::Zero(x); + VectorValues g = VectorValues::Zero(x0); Errors e; BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { - e.push_back(factor->error_vector(x)); + e.push_back(factor->error_vector(x0)); } transposeMultiplyAdd(fg, 1.0, e, g); return g; } + /* ************************************************************************* */ + void gradientAtZero(const FactorGraph& fg, VectorValues& g) { + // Zero-out the gradient + g.setZero(); + Errors e; + BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { + e.push_back(-factor->getb()); + } + transposeMultiplyAdd(fg, 1.0, e, g); + } + /* ************************************************************************* */ void residual(const FactorGraph& fg, const VectorValues &x, VectorValues &r) { Index i = 0 ; diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 09089df20..35a1bfa9d 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -318,11 +318,26 @@ namespace gtsam { void transposeMultiplyAdd(const FactorGraph& fg, double alpha, const Errors& e, VectorValues& x); /** - * Calculate Gradient of A^(A*x-b) for a given config - * @param x: VectorValues specifying where to calculate gradient - * @return gradient, as a VectorValues as well + * Compute the gradient of the energy function, + * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, + * centered around \f$ x = x_0 \f$. + * The gradient is \f$ A^T(Ax-b) \f$. + * @param fg The Jacobian factor graph $(A,b)$ + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues */ - VectorValues gradient(const FactorGraph& fg, const VectorValues& x); + VectorValues gradient(const FactorGraph& fg, const VectorValues& x0); + + /** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, + * centered around zero. + * The gradient is \f$ A^T(Ax-b) \f$. + * @param fg The Jacobian factor graph $(A,b)$ + * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues + * @return The gradient as a VectorValues + */ + void gradientAtZero(const FactorGraph& fg, VectorValues& g); // matrix-vector operations void residual(const FactorGraph& fg, const VectorValues &x, VectorValues &r); diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index 5f5801918..28b4185ca 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -233,14 +233,14 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( template VectorValues DoglegOptimizerImpl::ComputeSteepestDescentPoint(const M& Rd) { - // Compute gradient - // Convert to JacobianFactor's to use existing gradient function - FactorGraph jfg(Rd); - VectorValues grad = gradient(jfg, VectorValues::Zero(*allocateVectorValues(Rd))); + // Compute gradient (call gradientAtZero function, which is defined for various linear systems) + VectorValues grad = *allocateVectorValues(Rd); + gradientAtZero(Rd, grad); double gradientSqNorm = grad.dot(grad); // Compute R * g - Errors Rg = jfg * grad; + FactorGraph Rd_jfg(Rd); + Errors Rg = Rd_jfg * grad; // Compute minimizing step size double step = -gradientSqNorm / dot(Rg, Rg); diff --git a/gtsam/nonlinear/GaussianISAM2-inl.h b/gtsam/nonlinear/GaussianISAM2-inl.h index 7fcd77873..d0d37d886 100644 --- a/gtsam/nonlinear/GaussianISAM2-inl.h +++ b/gtsam/nonlinear/GaussianISAM2-inl.h @@ -1,10 +1,22 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussianISAM2 * @brief Full non-linear ISAM * @author Michael Kaess */ -#include +#include +#include using namespace std; using namespace gtsam; @@ -13,137 +25,137 @@ using namespace gtsam; namespace gtsam { -/* ************************************************************************* */ + /* ************************************************************************* */ template -void optimize2(const boost::shared_ptr& clique, double threshold, - vector& changed, const vector& replaced, Permuted& delta, int& count) { - // if none of the variables in this clique (frontal and separator!) changed - // significantly, then by the running intersection property, none of the - // cliques in the children need to be processed + void optimize2(const boost::shared_ptr& clique, double threshold, + vector& changed, const vector& replaced, Permuted& delta, int& count) { + // if none of the variables in this clique (frontal and separator!) changed + // significantly, then by the running intersection property, none of the + // cliques in the children need to be processed - // Are any clique variables part of the tree that has been redone? - bool cliqueReplaced = replaced[(*clique)->frontals().front()]; + // Are any clique variables part of the tree that has been redone? + bool cliqueReplaced = replaced[(*clique)->frontals().front()]; #ifndef NDEBUG - BOOST_FOREACH(Index frontal, (*clique)->frontals()) { - assert(cliqueReplaced == replaced[frontal]); - } + BOOST_FOREACH(Index frontal, (*clique)->frontals()) { + assert(cliqueReplaced == replaced[frontal]); + } #endif - // If not redone, then has one of the separator variables changed significantly? - bool recalculate = cliqueReplaced; - if(!recalculate) { - BOOST_FOREACH(Index parent, (*clique)->parents()) { - if(changed[parent]) { - recalculate = true; - break; - } - } - } - - // Solve clique if it was replaced, or if any parents were changed above the - // threshold or themselves replaced. - if(recalculate) { - - // Temporary copy of the original values, to check how much they change - vector originalValues((*clique)->nrFrontals()); - GaussianConditional::const_iterator it; - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { - originalValues[it - (*clique)->beginFrontals()] = delta[*it]; - } - - // Back-substitute - (*clique)->rhs(delta); - (*clique)->solveInPlace(delta); - count += (*clique)->nrFrontals(); - - // Whether the values changed above a threshold, or always true if the - // clique was replaced. - bool valuesChanged = cliqueReplaced; - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { - if(!valuesChanged) { - const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]); - const SubVector& newValue(delta[*it]); - if((oldValue - newValue).lpNorm() >= threshold) { - valuesChanged = true; + // If not redone, then has one of the separator variables changed significantly? + bool recalculate = cliqueReplaced; + if(!recalculate) { + BOOST_FOREACH(Index parent, (*clique)->parents()) { + if(changed[parent]) { + recalculate = true; break; } - } else - break; + } } - // If the values were above the threshold or this clique was replaced - if(valuesChanged) { - // Set changed flag for each frontal variable and leave the new values - BOOST_FOREACH(Index frontal, (*clique)->frontals()) { - changed[frontal] = true; - } - } else { - // Replace with the old values + // Solve clique if it was replaced, or if any parents were changed above the + // threshold or themselves replaced. + if(recalculate) { + + // Temporary copy of the original values, to check how much they change + vector originalValues((*clique)->nrFrontals()); + GaussianConditional::const_iterator it; for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { - delta[*it] = originalValues[it - (*clique)->beginFrontals()]; + originalValues[it - (*clique)->beginFrontals()] = delta[*it]; + } + + // Back-substitute + (*clique)->rhs(delta); + (*clique)->solveInPlace(delta); + count += (*clique)->nrFrontals(); + + // Whether the values changed above a threshold, or always true if the + // clique was replaced. + bool valuesChanged = cliqueReplaced; + for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { + if(!valuesChanged) { + const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]); + const SubVector& newValue(delta[*it]); + if((oldValue - newValue).lpNorm() >= threshold) { + valuesChanged = true; + break; + } + } else + break; + } + + // If the values were above the threshold or this clique was replaced + if(valuesChanged) { + // Set changed flag for each frontal variable and leave the new values + BOOST_FOREACH(Index frontal, (*clique)->frontals()) { + changed[frontal] = true; + } + } else { + // Replace with the old values + for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { + delta[*it] = originalValues[it - (*clique)->beginFrontals()]; + } + } + + // Recurse to children + BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { + optimize2(child, threshold, changed, replaced, delta, count); } } + } - // Recurse to children + /* ************************************************************************* */ + // fast full version without threshold + template + void optimize2(const boost::shared_ptr& clique, VectorValues& delta) { + + // parents are assumed to already be solved and available in result + (*clique)->rhs(delta); + (*clique)->solveInPlace(delta); + + // Solve chilren recursively BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { - optimize2(child, threshold, changed, replaced, delta, count); + optimize2(child, delta); } - } -} + } -/* ************************************************************************* */ -// fast full version without threshold + ///* ************************************************************************* */ + //boost::shared_ptr optimize2(const GaussianISAM2::sharedClique& root) { + // boost::shared_ptr delta(new VectorValues()); + // set changed; + // // starting from the root, call optimize on each conditional + // optimize2(root, delta); + // return delta; + //} + + /* ************************************************************************* */ template -void optimize2(const boost::shared_ptr& clique, VectorValues& delta) { + int optimize2(const boost::shared_ptr& root, double threshold, const vector& keys, Permuted& delta) { + vector changed(keys.size(), false); + int count = 0; + // starting from the root, call optimize on each conditional + optimize2(root, threshold, changed, keys, delta, count); + return count; + } - // parents are assumed to already be solved and available in result - (*clique)->rhs(delta); - (*clique)->solveInPlace(delta); - - // Solve chilren recursively - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { - optimize2(child, delta); - } -} - -///* ************************************************************************* */ -//boost::shared_ptr optimize2(const GaussianISAM2::sharedClique& root) { -// boost::shared_ptr delta(new VectorValues()); -// set changed; -// // starting from the root, call optimize on each conditional -// optimize2(root, delta); -// return delta; -//} - -/* ************************************************************************* */ + /* ************************************************************************* */ template -int optimize2(const boost::shared_ptr& root, double threshold, const vector& keys, Permuted& delta) { - vector changed(keys.size(), false); - int count = 0; - // starting from the root, call optimize on each conditional - optimize2(root, threshold, changed, keys, delta, count); - return count; -} + void nnz_internal(const boost::shared_ptr& clique, int& result) { + int dimR = (*clique)->dim(); + int dimSep = (*clique)->get_S().cols() - dimR; + result += ((dimR+1)*dimR)/2 + dimSep*dimR; + // traverse the children + BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { + nnz_internal(child, result); + } + } -/* ************************************************************************* */ + /* ************************************************************************* */ template -void nnz_internal(const boost::shared_ptr& clique, int& result) { - int dimR = (*clique)->dim(); - int dimSep = (*clique)->get_S().cols() - dimR; - result += ((dimR+1)*dimR)/2 + dimSep*dimR; - // traverse the children - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { - nnz_internal(child, result); - } -} - -/* ************************************************************************* */ - template -int calculate_nnz(const boost::shared_ptr& clique) { - int result = 0; - // starting from the root, add up entries of frontal and conditional matrices of each conditional - nnz_internal(clique, result); - return result; -} + int calculate_nnz(const boost::shared_ptr& clique) { + int result = 0; + // starting from the root, add up entries of frontal and conditional matrices of each conditional + nnz_internal(clique, result); + return result; + } } /// namespace gtsam diff --git a/gtsam/nonlinear/GaussianISAM2.cpp b/gtsam/nonlinear/GaussianISAM2.cpp new file mode 100644 index 000000000..d56de0055 --- /dev/null +++ b/gtsam/nonlinear/GaussianISAM2.cpp @@ -0,0 +1,61 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussianISAM2 + * @brief Full non-linear ISAM + * @author Michael Kaess + */ + +#include +#include + +using namespace std; +using namespace gtsam; + +#include + +namespace gtsam { + + /* ************************************************************************* */ + VectorValues gradient(const BayesTree& bayesTree, const VectorValues& x0) { + return gradient(FactorGraph(bayesTree), x0); + } + + /* ************************************************************************* */ + void gradientAtZero(const BayesTree& bayesTree, VectorValues& g) { + gradientAtZero(FactorGraph(bayesTree), g); + } + + /* ************************************************************************* */ + VectorValues gradient(const BayesTree >& bayesTree, const VectorValues& x0) { + return gradient(FactorGraph(bayesTree)); + } + + /* ************************************************************************* */ + void gradientAtZero(const BayesTree >& bayesTree, VectorValues& g) { + // Zero-out gradient + g.setZero(); + + // Sum up contributions for each clique + typedef boost::shared_ptr > sharedClique; + BOOST_FOREACH(const sharedClique& clique, bayesTree.nodes()) { + // Loop through variables in each clique, adding contributions + int variablePosition = 0; + for(GaussianConditional::const_iterator jit = clique->conditional()->beginFrontals(); jit != clique->conditional()->endFrontals(); ++jit) { + const int dim = clique->conditional()->dim(jit); + x0[*jit] += clique->gradientContribution().segment(variablePosition, dim); + variablePosition += dim; + } + } + } + +} diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index cb6f39865..f233d4c86 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussianISAM * @brief Full non-linear ISAM. @@ -41,26 +52,74 @@ public: } }; -// optimize the BayesTree, starting from the root +/** optimize the BayesTree, starting from the root */ template void optimize2(const boost::shared_ptr& root, VectorValues& delta); -// optimize the BayesTree, starting from the root; "replaced" needs to contain -// all variables that are contained in the top of the Bayes tree that has been -// redone; "delta" is the current solution, an offset from the linearization -// point; "threshold" is the maximum change against the PREVIOUS delta for -// non-replaced variables that can be ignored, ie. the old delta entry is kept -// and recursive backsubstitution might eventually stop if none of the changed -// variables are contained in the subtree. -// returns the number of variables that were solved for +/// optimize the BayesTree, starting from the root; "replaced" needs to contain +/// all variables that are contained in the top of the Bayes tree that has been +/// redone; "delta" is the current solution, an offset from the linearization +/// point; "threshold" is the maximum change against the PREVIOUS delta for +/// non-replaced variables that can be ignored, ie. the old delta entry is kept +/// and recursive backsubstitution might eventually stop if none of the changed +/// variables are contained in the subtree. +/// returns the number of variables that were solved for template int optimize2(const boost::shared_ptr& root, double threshold, const std::vector& replaced, Permuted& delta); -// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) +/// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) template int calculate_nnz(const boost::shared_ptr& clique); +/** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, + * centered around \f$ x = x_0 \f$. + * The gradient is \f$ R^T(Rx-d) \f$. + * @param bayesTree The Gaussian Bayes Tree $(R,d)$ + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues + */ +VectorValues gradient(const BayesTree& bayesTree, const VectorValues& x0); + +/** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, + * centered around zero. + * The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&). + * @param bayesTree The Gaussian Bayes Tree $(R,d)$ + * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues + * @return The gradient as a VectorValues + */ +VectorValues gradientAtZero(const BayesTree& bayesTree, VectorValues& g); + +/** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, + * centered around \f$ x = x_0 \f$. + * The gradient is \f$ R^T(Rx-d) \f$. + * This specialized version is used with ISAM2, where each clique stores its + * gradient contribution. + * @param bayesTree The Gaussian Bayes Tree $(R,d)$ + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues + */ +VectorValues gradient(const BayesTree >& bayesTree, const VectorValues& x0); + +/** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, + * centered around zero. + * The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&). + * This specialized version is used with ISAM2, where each clique stores its + * gradient contribution. + * @param bayesTree The Gaussian Bayes Tree $(R,d)$ + * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues + * @return The gradient as a VectorValues + */ +VectorValues gradientAtZero(const BayesTree >& bayesTree, VectorValues& g); + }/// namespace gtsam #include diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index b944b105d..58503466c 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -30,7 +30,8 @@ headers += DoglegOptimizer.h DoglegOptimizer-inl.h # Nonlinear iSAM(2) headers += NonlinearISAM.h NonlinearISAM-inl.h headers += ISAM2.h ISAM2-inl.h ISAM2-impl-inl.h -headers += GaussianISAM2.h GaussianISAM2-inl.h +sources += GaussianISAM2.cpp +headers += GaussianISAM2-inl.h # Nonlinear constraints headers += NonlinearEquality.h