From 92bd4e280de0197a0d362a9e894d1914823f61c9 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 16 Mar 2012 16:16:13 +0000 Subject: [PATCH 02/16] Consistent 'optimize', 'optimizeInPlace', 'optimizeGradientSearch', and 'optimizeGradientSearchInPlace' functions for GBN, GBT, and ISAM2. Reorganized some existing ones and added some new ones to do this. --- gtsam/linear/GaussianBayesNet.cpp | 36 ++++++++++ gtsam/linear/GaussianBayesNet.h | 37 ++++++++++ gtsam/linear/GaussianBayesTree-inl.h | 27 -------- gtsam/linear/GaussianBayesTree.cpp | 99 +++++++++++++++++++++++++++ gtsam/linear/GaussianBayesTree.h | 63 +++++++++++++++-- gtsam/linear/GaussianConditional.h | 2 +- gtsam/nonlinear/DoglegOptimizerImpl.h | 54 +-------------- gtsam/nonlinear/GaussianISAM2-inl.h | 38 ++++++++++ gtsam/nonlinear/GaussianISAM2.cpp | 10 --- gtsam/nonlinear/GaussianISAM2.h | 73 +++++++++++++------- gtsam/nonlinear/ISAM2-impl-inl.h | 12 ++++ gtsam/nonlinear/ISAM2-inl.h | 9 --- gtsam/nonlinear/ISAM2.h | 4 -- tests/testDoglegOptimizer.cpp | 15 ++-- 14 files changed, 338 insertions(+), 141 deletions(-) create mode 100644 gtsam/linear/GaussianBayesTree.cpp diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 13a3d4e0e..5721a4760 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -122,6 +122,42 @@ VectorValues backSubstituteTranspose(const GaussianBayesNet& bn, return gy; } +/* ************************************************************************* */ +VectorValues optimizeGradientSearch(const GaussianBayesNet& Rd) { + tic(0, "Allocate VectorValues"); + VectorValues grad = *allocateVectorValues(Rd); + toc(0, "Allocate VectorValues"); + + optimizeGradientSearchInPlace(Rd, grad); + + return grad; +} + +/* ************************************************************************* */ +void optimizeGradientSearchInPlace(const GaussianBayesNet& Rd, VectorValues& grad) { + tic(1, "Compute Gradient"); + // Compute gradient (call gradientAtZero function, which is defined for various linear systems) + gradientAtZero(Rd, grad); + double gradientSqNorm = grad.dot(grad); + toc(1, "Compute Gradient"); + + tic(2, "Compute R*g"); + // Compute R * g + FactorGraph Rd_jfg(Rd); + Errors Rg = Rd_jfg * grad; + toc(2, "Compute R*g"); + + tic(3, "Compute minimizing step size"); + // Compute minimizing step size + double step = -gradientSqNorm / dot(Rg, Rg); + toc(3, "Compute minimizing step size"); + + tic(4, "Compute point"); + // Compute steepest descent point + scal(step, grad); + toc(4, "Compute point"); +} + /* ************************************************************************* */ pair matrix(const GaussianBayesNet& bn) { diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index dda579741..1ef9081d4 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -69,6 +69,43 @@ namespace gtsam { */ void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x); + /** + * Optimize along the gradient direction, with a closed-form computation to + * perform the line search. The gradient is computed about \f$ \delta x=0 \f$. + * + * This function returns \f$ \delta x \f$ that minimizes a reparametrized + * problem. The error function of a GaussianBayesNet is + * + * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f] + * + * with gradient and Hessian + * + * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] + * + * This function performs the line search in the direction of the + * gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size + * \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$: + * + * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] + * + * Optimizing by setting the derivative to zero yields + * \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function + * evaluates the denominator without computing the Hessian \f$ G \f$, returning + * + * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] + * + * @param bn The GaussianBayesNet on which to perform this computation + * @return The resulting \f$ \delta x \f$ as described above + */ + VectorValues optimizeGradientSearch(const GaussianBayesNet& bn); + + /** In-place version of optimizeGradientSearch(const GaussianBayesNet&) requiring pre-allocated VectorValues \c grad + * + * @param bn The GaussianBayesNet on which to perform this computation + * @param [out] grad The resulting \f$ \delta x \f$ as described in optimizeGradientSearch(const GaussianBayesNet&) + * */ + void optimizeGradientSearchInPlace(const GaussianBayesNet& bn, VectorValues& grad); + /** * Transpose Backsubstitute * gy=inv(L)*gx by solving L*gy=gx. diff --git a/gtsam/linear/GaussianBayesTree-inl.h b/gtsam/linear/GaussianBayesTree-inl.h index 1b936b638..9cb327bec 100644 --- a/gtsam/linear/GaussianBayesTree-inl.h +++ b/gtsam/linear/GaussianBayesTree-inl.h @@ -25,31 +25,4 @@ namespace gtsam { -/* ************************************************************************* */ -namespace internal { -template -inline static void optimizeInPlace(const boost::shared_ptr& clique, VectorValues& result) { - // parents are assumed to already be solved and available in result - clique->conditional()->solveInPlace(result); - - // starting from the root, call optimize on each conditional - BOOST_FOREACH(const boost::shared_ptr& child, clique->children_) - optimizeInPlace(child, result); -} -} - -/* ************************************************************************* */ -template -VectorValues optimize(const BayesTree& bayesTree) { - VectorValues result = *allocateVectorValues(bayesTree); - internal::optimizeInPlace(bayesTree.root(), result); - return result; -} - -/* ************************************************************************* */ -template -void optimizeInPlace(const BayesTree& bayesTree, VectorValues& result) { - internal::optimizeInPlace(bayesTree.root(), result); -} - } diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp new file mode 100644 index 000000000..4243657e5 --- /dev/null +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -0,0 +1,99 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussianBayesTree.cpp + * @brief Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree + * @brief GaussianBayesTree + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +namespace internal { +void optimizeInPlace(const boost::shared_ptr >& clique, VectorValues& result) { + // parents are assumed to already be solved and available in result + clique->conditional()->solveInPlace(result); + + // starting from the root, call optimize on each conditional + BOOST_FOREACH(const boost::shared_ptr >& child, clique->children_) + optimizeInPlace(child, result); +} +} + +/* ************************************************************************* */ +VectorValues optimize(const GaussianBayesTree& bayesTree) { + VectorValues result = *allocateVectorValues(bayesTree); + internal::optimizeInPlace(bayesTree.root(), result); + return result; +} + +/* ************************************************************************* */ +VectorValues optimizeGradientSearch(const GaussianBayesTree& Rd) { + tic(0, "Allocate VectorValues"); + VectorValues grad = *allocateVectorValues(Rd); + toc(0, "Allocate VectorValues"); + + optimizeGradientSearchInPlace(Rd, grad); + + return grad; +} + +/* ************************************************************************* */ +void optimizeGradientSearchInPlace(const GaussianBayesTree& Rd, VectorValues& grad) { + tic(1, "Compute Gradient"); + // Compute gradient (call gradientAtZero function, which is defined for various linear systems) + gradientAtZero(Rd, grad); + double gradientSqNorm = grad.dot(grad); + toc(1, "Compute Gradient"); + + tic(2, "Compute R*g"); + // Compute R * g + FactorGraph Rd_jfg(Rd); + Errors Rg = Rd_jfg * grad; + toc(2, "Compute R*g"); + + tic(3, "Compute minimizing step size"); + // Compute minimizing step size + double step = -gradientSqNorm / dot(Rg, Rg); + toc(3, "Compute minimizing step size"); + + tic(4, "Compute point"); + // Compute steepest descent point + scal(step, grad); + toc(4, "Compute point"); +} + +/* ************************************************************************* */ +void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result) { + internal::optimizeInPlace(bayesTree.root(), result); +} + +/* ************************************************************************* */ +VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0) { + return gradient(FactorGraph(bayesTree), x0); +} + +/* ************************************************************************* */ +void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g) { + gradientAtZero(FactorGraph(bayesTree), g); +} + +} + + + + diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h index 227cc94f9..bb15eb063 100644 --- a/gtsam/linear/GaussianBayesTree.h +++ b/gtsam/linear/GaussianBayesTree.h @@ -21,18 +21,73 @@ #include #include +#include namespace gtsam { typedef BayesTree GaussianBayesTree; /// optimize the BayesTree, starting from the root -template -VectorValues optimize(const BayesTree& bayesTree); +VectorValues optimize(const GaussianBayesTree& bayesTree); /// recursively optimize this conditional and all subtrees -template -void optimizeInPlace(const BayesTree& clique, VectorValues& result); +void optimizeInPlace(const GaussianBayesTree& clique, VectorValues& result); + +namespace internal { +void optimizeInPlace(const boost::shared_ptr >& clique, VectorValues& result); +} + +/** + * Optimize along the gradient direction, with a closed-form computation to + * perform the line search. The gradient is computed about \f$ \delta x=0 \f$. + * + * This function returns \f$ \delta x \f$ that minimizes a reparametrized + * problem. The error function of a GaussianBayesNet is + * + * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f] + * + * with gradient and Hessian + * + * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] + * + * This function performs the line search in the direction of the + * gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size + * \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$: + * + * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] + * + * Optimizing by setting the derivative to zero yields + * \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function + * evaluates the denominator without computing the Hessian \f$ G \f$, returning + * + * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] + */ +VectorValues optimizeGradientSearch(const GaussianBayesTree& bn); + +/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */ +void optimizeGradientSearchInPlace(const GaussianBayesTree& bn, VectorValues& grad); + +/** + * 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 GaussianBayesTree& 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 + */ +void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g); } diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 248ac7a9e..b33098061 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -43,7 +43,7 @@ class JacobianFactor; /** * A conditional Gaussian functions as the node in a Bayes network * It has a set of parents y,z, etc. and implements a probability density on x. - * The negative log-probability is given by \f$ |Rx - (d - Sy - Tz - ...)|^2 \f$ + * The negative log-probability is given by \f$ \frac{1}{2} |Rx - (d - Sy - Tz - ...)|^2 \f$ */ class GaussianConditional : public IndexConditional { diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index af1e345b6..5026a155f 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -115,29 +115,6 @@ struct DoglegOptimizerImpl { */ static VectorValues ComputeDoglegPoint(double Delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose=false); - /** Compute the minimizer \f$ \delta x_u \f$ of the line search along the gradient direction \f$ g \f$ of - * the function - * \f[ - * M(\delta x) = (R \delta x - d)^T (R \delta x - d) - * \f] - * where \f$ R \f$ is an upper-triangular matrix and \f$ d \f$ is a vector. - * Together \f$ (R,d) \f$ are either a Bayes' net or a Bayes' tree. - * - * The same quadratic error function written as a Taylor expansion of the original - * non-linear error function is - * \f[ - * M(\delta x) = f(x_0) + g(x_0) + \frac{1}{2} \delta x^T G(x_0) \delta x, - * \f] - * @tparam M The type of the Bayes' net or tree, currently - * either BayesNet (or GaussianBayesNet) or BayesTree. - * @param Rd The Bayes' net or tree \f$ (R,d) \f$ as described above, currently - * this must be of type BayesNet (or GaussianBayesNet) or - * BayesTree. - * @return The minimizer \f$ \delta x_u \f$ along the gradient descent direction. - */ - template - static VectorValues ComputeSteepestDescentPoint(const M& Rd); - /** Compute the point on the line between the steepest descent point and the * Newton's method point intersecting the trust region boundary. * Mathematically, computes \f$ \tau \f$ such that \f$ 0<\tau<1 \f$ and @@ -159,7 +136,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( // Compute steepest descent and Newton's method points tic(0, "Steepest Descent"); - VectorValues dx_u = ComputeSteepestDescentPoint(Rd); + VectorValues dx_u = optimizeGradientSearch(Rd); toc(0, "Steepest Descent"); tic(1, "optimize"); VectorValues dx_n = optimize(Rd); @@ -274,33 +251,4 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( return result; } -/* ************************************************************************* */ -template -VectorValues DoglegOptimizerImpl::ComputeSteepestDescentPoint(const M& Rd) { - - tic(0, "Compute Gradient"); - // Compute gradient (call gradientAtZero function, which is defined for various linear systems) - VectorValues grad = *allocateVectorValues(Rd); - gradientAtZero(Rd, grad); - double gradientSqNorm = grad.dot(grad); - toc(0, "Compute Gradient"); - - tic(1, "Compute R*g"); - // Compute R * g - FactorGraph Rd_jfg(Rd); - Errors Rg = Rd_jfg * grad; - toc(1, "Compute R*g"); - - tic(2, "Compute minimizing step size"); - // Compute minimizing step size - double step = -gradientSqNorm / dot(Rg, Rg); - toc(2, "Compute minimizing step size"); - - tic(3, "Compute point"); - // Compute steepest descent point - scal(step, grad); - toc(3, "Compute point"); - return grad; -} - } diff --git a/gtsam/nonlinear/GaussianISAM2-inl.h b/gtsam/nonlinear/GaussianISAM2-inl.h index b52e22629..57801fbe4 100644 --- a/gtsam/nonlinear/GaussianISAM2-inl.h +++ b/gtsam/nonlinear/GaussianISAM2-inl.h @@ -114,6 +114,44 @@ namespace gtsam { return count; } + /* ************************************************************************* */ + template + VectorValues optimizeGradientSearch(const ISAM2& isam) { + tic(0, "Allocate VectorValues"); + VectorValues grad = *allocateVectorValues(isam); + toc(0, "Allocate VectorValues"); + + optimizeGradientSearchInPlace(isam, grad); + + return grad; + } + + /* ************************************************************************* */ + template + void optimizeGradientSearchInPlace(const ISAM2& Rd, VectorValues& grad) { + tic(1, "Compute Gradient"); + // Compute gradient (call gradientAtZero function, which is defined for various linear systems) + gradientAtZero(Rd, grad); + double gradientSqNorm = grad.dot(grad); + toc(1, "Compute Gradient"); + + tic(2, "Compute R*g"); + // Compute R * g + FactorGraph Rd_jfg(Rd); + Errors Rg = Rd_jfg * grad; + toc(2, "Compute R*g"); + + tic(3, "Compute minimizing step size"); + // Compute minimizing step size + double step = -gradientSqNorm / dot(Rg, Rg); + toc(3, "Compute minimizing step size"); + + tic(4, "Compute point"); + // Compute steepest descent point + scal(step, grad); + toc(4, "Compute point"); + } + /* ************************************************************************* */ template void nnz_internal(const boost::shared_ptr& clique, int& result) { diff --git a/gtsam/nonlinear/GaussianISAM2.cpp b/gtsam/nonlinear/GaussianISAM2.cpp index 34456ad36..624c5d4f0 100644 --- a/gtsam/nonlinear/GaussianISAM2.cpp +++ b/gtsam/nonlinear/GaussianISAM2.cpp @@ -26,16 +26,6 @@ using namespace gtsam; 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), x0); diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index 7b2aa625d..a602d81a6 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -63,44 +63,65 @@ public: }; -/// optimize the BayesTree, starting from the root; "replaced" needs to contain +/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ +template +VectorValues optimize(const ISAM2& isam) { + VectorValues delta = *allocateVectorValues(isam); + internal::optimizeInPlace(isam.root(), delta); + return delta; +} + +/// Optimize the BayesTree, starting from the root. +/// @param 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 +/// redone. +/// @param delta The current solution, an offset from the linearization +/// point. +/// @param threshold 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 +/// @return The number of variables that were solved for template int optimizeWildfire(const boost::shared_ptr& root, double threshold, const std::vector& replaced, Permuted& delta); +/** + * Optimize along the gradient direction, with a closed-form computation to + * perform the line search. The gradient is computed about \f$ \delta x=0 \f$. + * + * This function returns \f$ \delta x \f$ that minimizes a reparametrized + * problem. The error function of a GaussianBayesNet is + * + * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f] + * + * with gradient and Hessian + * + * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] + * + * This function performs the line search in the direction of the + * gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size + * \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$: + * + * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] + * + * Optimizing by setting the derivative to zero yields + * \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function + * evaluates the denominator without computing the Hessian \f$ G \f$, returning + * + * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] + */ +template +VectorValues optimizeGradientSearch(const ISAM2& isam); + +/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */ +template +void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad); + /// 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 - */ -void 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$, diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index 08b73df21..40e87a09b 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -361,6 +361,18 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, return result; } +/* ************************************************************************* */ +namespace internal { +inline static void optimizeInPlace(const boost::shared_ptr >& clique, VectorValues& result) { + // parents are assumed to already be solved and available in result + clique->conditional()->solveInPlace(result); + + // starting from the root, call optimize on each conditional + BOOST_FOREACH(const boost::shared_ptr >& child, clique->children_) + optimizeInPlace(child, result); +} +} + /* ************************************************************************* */ template size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr >& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold) { diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 674496512..0d5f190a3 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -557,7 +557,6 @@ ISAM2Result ISAM2::update( return result; } - /* ************************************************************************* */ template void ISAM2::updateDelta(bool forceFullSolve) const { @@ -630,13 +629,5 @@ const Permuted& ISAM2::getDelta() const { return delta_; } -/* ************************************************************************* */ -template -VectorValues optimize(const ISAM2& isam) { - VectorValues delta = *allocateVectorValues(isam); - internal::optimizeInPlace(isam.root(), delta); - return delta; -} - } /// namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 1117a1a48..208f0748f 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -462,10 +462,6 @@ private: }; // ISAM2 -/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ -template -VectorValues optimize(const ISAM2& isam); - } /// namespace gtsam #include diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 27070b801..b16ac0cdc 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -102,7 +103,7 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPoint) { VectorValues expected = gradientValues; scal(step, expected); // Compute the steepest descent point with the dogleg function - VectorValues actual = DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn); + VectorValues actual = optimizeGradientSearch(gbn); // Check that points agree EXPECT(assert_equal(expected, actual, 1e-5)); @@ -290,7 +291,7 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) { expectedFromBN[4] = Vector_(2, 0.300134, 0.423233); // Compute the steepest descent point with the dogleg function - VectorValues actual = DoglegOptimizerImpl::ComputeSteepestDescentPoint(bt); + VectorValues actual = optimizeGradientSearch(bt); // Check that points agree EXPECT(assert_equal(expected, actual, 1e-5)); @@ -324,7 +325,7 @@ TEST(DoglegOptimizer, ComputeBlend) { 4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2))); // Compute steepest descent point - VectorValues xu = DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn); + VectorValues xu = optimizeGradientSearch(gbn); // Compute Newton's method point VectorValues xn = optimize(gbn); @@ -362,18 +363,18 @@ TEST(DoglegOptimizer, ComputeDoglegPoint) { // Compute dogleg point for different deltas double Delta1 = 0.5; // Less than steepest descent - VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn), optimize(gbn)); + VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, optimizeGradientSearch(gbn), optimize(gbn)); DOUBLES_EQUAL(Delta1, actual1.vector().norm(), 1e-5); double Delta2 = 1.5; // Between steepest descent and Newton's method - VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn), optimize(gbn)); - VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn), optimize(gbn)); + VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, optimizeGradientSearch(gbn), optimize(gbn)); + VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, optimizeGradientSearch(gbn), optimize(gbn)); DOUBLES_EQUAL(Delta2, actual2.vector().norm(), 1e-5); EXPECT(assert_equal(expected2, actual2)); double Delta3 = 5.0; // Larger than Newton's method point VectorValues expected3 = optimize(gbn); - VectorValues actual3 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta3, DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn), optimize(gbn)); + VectorValues actual3 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta3, optimizeGradientSearch(gbn), optimize(gbn)); EXPECT(assert_equal(expected3, actual3)); } From 99c3371474b2535d8910e0ac3055011c729f31e6 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 16 Mar 2012 20:55:21 +0000 Subject: [PATCH 03/16] In progress --- .cproject | 2 +- .project | 2 +- gtsam/nonlinear/GaussianISAM2-inl.h | 176 ----- gtsam/nonlinear/GaussianISAM2.cpp | 60 -- gtsam/nonlinear/GaussianISAM2.h | 153 ---- .../{ISAM2-impl-inl.h => ISAM2-impl.cpp} | 149 +--- gtsam/nonlinear/ISAM2-impl.h | 127 ++++ gtsam/nonlinear/ISAM2-inl.h | 686 +++-------------- gtsam/nonlinear/ISAM2.cpp | 687 ++++++++++++++++++ gtsam/nonlinear/ISAM2.h | 110 ++- 10 files changed, 1022 insertions(+), 1130 deletions(-) delete mode 100644 gtsam/nonlinear/GaussianISAM2-inl.h delete mode 100644 gtsam/nonlinear/GaussianISAM2.cpp delete mode 100644 gtsam/nonlinear/GaussianISAM2.h rename gtsam/nonlinear/{ISAM2-impl-inl.h => ISAM2-impl.cpp} (60%) create mode 100644 gtsam/nonlinear/ISAM2-impl.h create mode 100644 gtsam/nonlinear/ISAM2.cpp diff --git a/.cproject b/.cproject index 2032850b4..c6c10d6db 100644 --- a/.cproject +++ b/.cproject @@ -21,7 +21,7 @@ - + diff --git a/.project b/.project index 9856df2ea..c4d531b04 100644 --- a/.project +++ b/.project @@ -23,7 +23,7 @@ org.eclipse.cdt.make.core.buildArguments - -j5 + org.eclipse.cdt.make.core.buildCommand diff --git a/gtsam/nonlinear/GaussianISAM2-inl.h b/gtsam/nonlinear/GaussianISAM2-inl.h deleted file mode 100644 index 57801fbe4..000000000 --- a/gtsam/nonlinear/GaussianISAM2-inl.h +++ /dev/null @@ -1,176 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - -namespace gtsam { - - using namespace std; - - /* ************************************************************************* */ - namespace internal { - template - void optimizeWildfire(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()]; -#ifndef NDEBUG - 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)->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_) { - optimizeWildfire(child, threshold, changed, replaced, delta, count); - } - } - } - } - - /* ************************************************************************* */ - template - int optimizeWildfire(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 - if(root) - internal::optimizeWildfire(root, threshold, changed, keys, delta, count); - return count; - } - - /* ************************************************************************* */ - template - VectorValues optimizeGradientSearch(const ISAM2& isam) { - tic(0, "Allocate VectorValues"); - VectorValues grad = *allocateVectorValues(isam); - toc(0, "Allocate VectorValues"); - - optimizeGradientSearchInPlace(isam, grad); - - return grad; - } - - /* ************************************************************************* */ - template - void optimizeGradientSearchInPlace(const ISAM2& Rd, VectorValues& grad) { - tic(1, "Compute Gradient"); - // Compute gradient (call gradientAtZero function, which is defined for various linear systems) - gradientAtZero(Rd, grad); - double gradientSqNorm = grad.dot(grad); - toc(1, "Compute Gradient"); - - tic(2, "Compute R*g"); - // Compute R * g - FactorGraph Rd_jfg(Rd); - Errors Rg = Rd_jfg * grad; - toc(2, "Compute R*g"); - - tic(3, "Compute minimizing step size"); - // Compute minimizing step size - double step = -gradientSqNorm / dot(Rg, Rg); - toc(3, "Compute minimizing step size"); - - tic(4, "Compute point"); - // Compute steepest descent point - scal(step, grad); - toc(4, "Compute point"); - } - - /* ************************************************************************* */ - 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; - } - -} /// namespace gtsam diff --git a/gtsam/nonlinear/GaussianISAM2.cpp b/gtsam/nonlinear/GaussianISAM2.cpp deleted file mode 100644 index 624c5d4f0..000000000 --- a/gtsam/nonlinear/GaussianISAM2.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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; - -#include - -namespace gtsam { - - /* ************************************************************************* */ - VectorValues gradient(const BayesTree >& bayesTree, const VectorValues& x0) { - return gradient(FactorGraph(bayesTree), x0); - } - - /* ************************************************************************* */ - static void gradientAtZeroTreeAdder(const boost::shared_ptr >& root, VectorValues& g) { - // Loop through variables in each clique, adding contributions - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = root->conditional()->begin(); jit != root->conditional()->end(); ++jit) { - const int dim = root->conditional()->dim(jit); - g[*jit] += root->gradientContribution().segment(variablePosition, dim); - variablePosition += dim; - } - - // Recursively add contributions from children - typedef boost::shared_ptr > sharedClique; - BOOST_FOREACH(const sharedClique& child, root->children()) { - gradientAtZeroTreeAdder(child, g); - } - } - - /* ************************************************************************* */ - void gradientAtZero(const BayesTree >& bayesTree, VectorValues& g) { - // Zero-out gradient - g.setZero(); - - // Sum up contributions for each clique - gradientAtZeroTreeAdder(bayesTree.root(), g); - } - -} diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h deleted file mode 100644 index a602d81a6..000000000 --- a/gtsam/nonlinear/GaussianISAM2.h +++ /dev/null @@ -1,153 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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. - * @author Michael Kaess - */ - -// \callgraph - -#pragma once - -#include -#include - -namespace gtsam { - -/** - * @ingroup ISAM2 - * @brief The main ISAM2 class that is exposed to gtsam users, see ISAM2 for usage. - * - * This is a thin wrapper around an ISAM2 class templated on - * GaussianConditional, and the values on which that GaussianISAM2 is - * templated. - * - * @tparam VALUES The Values or TupleValues\Emph{N} that contains the - * variables. - * @tparam GRAPH The NonlinearFactorGraph structure to store factors. Defaults to standard NonlinearFactorGraph - */ -template -class GaussianISAM2 : public ISAM2 { - typedef ISAM2 Base; -public: - - /// @name Standard Constructors - /// @{ - - /** Create an empty ISAM2 instance */ - GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} - - /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ - GaussianISAM2() : ISAM2() {} - - /// @} - /// @name Advanced Interface - /// @{ - - void cloneTo(boost::shared_ptr& newGaussianISAM2) const { - boost::shared_ptr isam2 = boost::static_pointer_cast(newGaussianISAM2); - Base::cloneTo(isam2); - } - - /// @} - -}; - -/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ -template -VectorValues optimize(const ISAM2& isam) { - VectorValues delta = *allocateVectorValues(isam); - internal::optimizeInPlace(isam.root(), delta); - return delta; -} - -/// Optimize the BayesTree, starting from the root. -/// @param replaced Needs to contain -/// all variables that are contained in the top of the Bayes tree that has been -/// redone. -/// @param delta The current solution, an offset from the linearization -/// point. -/// @param threshold 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. -/// @return The number of variables that were solved for -template -int optimizeWildfire(const boost::shared_ptr& root, - double threshold, const std::vector& replaced, Permuted& delta); - -/** - * Optimize along the gradient direction, with a closed-form computation to - * perform the line search. The gradient is computed about \f$ \delta x=0 \f$. - * - * This function returns \f$ \delta x \f$ that minimizes a reparametrized - * problem. The error function of a GaussianBayesNet is - * - * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f] - * - * with gradient and Hessian - * - * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] - * - * This function performs the line search in the direction of the - * gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size - * \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$: - * - * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] - * - * Optimizing by setting the derivative to zero yields - * \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function - * evaluates the denominator without computing the Hessian \f$ G \f$, returning - * - * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] - */ -template -VectorValues optimizeGradientSearch(const ISAM2& isam); - -/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */ -template -void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad); - -/// 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$. - * 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 - */ -void gradientAtZero(const BayesTree >& bayesTree, VectorValues& g); - -}/// namespace gtsam - -#include diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl.cpp similarity index 60% rename from gtsam/nonlinear/ISAM2-impl-inl.h rename to gtsam/nonlinear/ISAM2-impl.cpp index 40e87a09b..8dce6934a 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -10,126 +10,17 @@ * -------------------------------------------------------------------------- */ /** - * @file ISAM2-impl-inl.h + * @file ISAM2-impl.cpp * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. * @author Michael Kaess, Richard Roberts */ -#include +#include namespace gtsam { -using namespace std; - -template -struct ISAM2::Impl { - - typedef ISAM2 ISAM2Type; - - struct PartialSolveResult { - typename ISAM2Type::sharedClique bayesTree; - Permutation fullReordering; - Permutation fullReorderingInverse; - }; - - struct ReorderingMode { - size_t nFullSystemVars; - enum { /*AS_ADDED,*/ COLAMD } algorithm; - enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain; - boost::optional&> constrainedKeys; - }; - - /** - * Add new variables to the ISAM2 system. - * @param newTheta Initial values for new variables - * @param theta Current solution to be augmented with new initialization - * @param delta Current linear delta to be augmented with zeros - * @param ordering Current ordering to be augmented with new variables - * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables - * @param keyFormatter Formatter for printing nonlinear keys during debugging - */ - static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, - Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); - - /** - * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol - * in each NonlinearFactor, obtains the index by calling ordering[symbol]. - * @param ordering The current ordering from which to obtain the variable indices - * @param factors The factors from which to extract the variables - * @return The set of variables indices from the factors - */ - static FastSet IndicesFromFactors(const Ordering& ordering, const GRAPH& factors); - - /** - * Find the set of variables to be relinearized according to relinearizeThreshold. - * Any variables in the VectorValues delta whose vector magnitude is greater than - * or equal to relinearizeThreshold are returned. - * @param delta The linear delta to check against the threshold - * @param keyFormatter Formatter for printing nonlinear keys during debugging - * @return The set of variable indices in delta whose magnitude is greater than or - * equal to relinearizeThreshold - */ - static FastSet CheckRelinearization(const Permuted& delta, const Ordering& ordering, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); - - /** - * Recursively search this clique and its children for marked keys appearing - * in the separator, and add the *frontal* keys of any cliques whose - * separator contains any marked keys to the set \c keys. The purpose of - * this is to discover the cliques that need to be redone due to information - * propagating to them from cliques that directly contain factors being - * relinearized. - * - * The original comment says this finds all variables directly connected to - * the marked ones by measurements. Is this true, because it seems like this - * would also pull in variables indirectly connected through other frontal or - * separator variables? - * - * Alternatively could we trace up towards the root for each variable here? - */ - static void FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask); - - /** - * Apply expmap to the given values, but only for indices appearing in - * \c markedRelinMask. Values are expmapped in-place. - * \param [in][out] values The value to expmap in-place - * \param delta The linear delta with which to expmap - * \param ordering The ordering - * \param mask Mask on linear indices, only \c true entries are expmapped - * \param invalidateIfDebug If this is true, *and* NDEBUG is not defined, - * expmapped deltas will be set to an invalid value (infinity) to catch bugs - * where we might expmap something twice, or expmap it but then not - * recalculate its delta. - * @param keyFormatter Formatter for printing nonlinear keys during debugging - */ - static void ExpmapMasked(Values& values, const Permuted& delta, - const Ordering& ordering, const std::vector& mask, - boost::optional&> invalidateIfDebug = boost::optional&>(), - const KeyFormatter& keyFormatter = DefaultKeyFormatter); - - /** - * Reorder and eliminate factors. These factors form a subset of the full - * problem, so along with the BayesTree we get a partial reordering of the - * problem that needs to be applied to the other data in ISAM2, which is the - * VariableIndex, the delta, the ordering, and the orphans (including cached - * factors). - * \param factors The factors to eliminate, representing part of the full - * problem. This is permuted during use and so is cleared when this function - * returns in order to invalidate it. - * \param keys The set of indices used in \c factors. - * \return The eliminated BayesTree and the permutation to be applied to the - * rest of the ISAM2 data. - */ - static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, - const ReorderingMode& reorderingMode); - - static size_t UpdateDelta(const boost::shared_ptr >& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold); - -}; - /* ************************************************************************* */ -template -void ISAM2::Impl::AddVariables( +void ISAM2::Impl::AddVariables( const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, Ordering& ordering,typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); @@ -163,8 +54,7 @@ void ISAM2::Impl::AddVariables( } /* ************************************************************************* */ -template -FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) { +FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) { FastSet indices; BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) { BOOST_FOREACH(Key key, factor->keys()) { @@ -175,8 +65,7 @@ FastSet ISAM2::Impl::IndicesFromFactors(const Ordering } /* ************************************************************************* */ -template -FastSet ISAM2::Impl::CheckRelinearization(const Permuted& delta, const Ordering& ordering, +FastSet ISAM2::Impl::CheckRelinearization(const Permuted& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { FastSet relinKeys; @@ -204,8 +93,7 @@ FastSet ISAM2::Impl::CheckRelinearization(const Permut } /* ************************************************************************* */ -template -void ISAM2::Impl::FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask) { +void ISAM2::Impl::FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask) { static const bool debug = false; // does the separator contain any of the variables? bool found = false; @@ -219,14 +107,13 @@ void ISAM2::Impl::FindAll(typename ISAM2Clique:: if(debug) clique->print("Key(s) marked in clique "); if(debug) cout << "so marking key " << (*clique)->keys().front() << endl; } - BOOST_FOREACH(const typename ISAM2Clique::shared_ptr& child, clique->children_) { + BOOST_FOREACH(const typename ISAM2Clique::shared_ptr& child, clique->children_) { FindAll(child, keys, markedMask); } } /* ************************************************************************* */ -template -void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted& delta, const Ordering& ordering, +void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted& delta, const Ordering& ordering, const vector& mask, boost::optional&> invalidateIfDebug, const KeyFormatter& keyFormatter) { // If debugging, invalidate if requested, otherwise do not invalidate. // Invalidating means setting expmapped entries to Inf, to trigger assertions @@ -262,9 +149,8 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const Permute } /* ************************************************************************* */ -template -typename ISAM2::Impl::PartialSolveResult -ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, +ISAM2::Impl::PartialSolveResult +ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, const ReorderingMode& reorderingMode) { static const bool debug = ISDEBUG("ISAM2 recalculate"); @@ -340,14 +226,8 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, // eliminate into a Bayes net tic(7,"eliminate"); - JunctionTree jt(factors, affectedFactorsIndex); + JunctionTree jt(factors, affectedFactorsIndex); result.bayesTree = jt.eliminate(EliminatePreferLDL); - if(debug && result.bayesTree) { - if(boost::dynamic_pointer_cast >(result.bayesTree)) - cout << "Is an ISAM2 clique" << endl; - cout << "Re-eliminated BT:\n"; - result.bayesTree->printTree(""); - } toc(7,"eliminate"); tic(8,"permute eliminated"); @@ -363,19 +243,18 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, /* ************************************************************************* */ namespace internal { -inline static void optimizeInPlace(const boost::shared_ptr >& clique, VectorValues& result) { +inline static void optimizeInPlace(const boost::shared_ptr& clique, VectorValues& result) { // parents are assumed to already be solved and available in result clique->conditional()->solveInPlace(result); // starting from the root, call optimize on each conditional - BOOST_FOREACH(const boost::shared_ptr >& child, clique->children_) + BOOST_FOREACH(const boost::shared_ptr& child, clique->children_) optimizeInPlace(child, result); } } /* ************************************************************************* */ -template -size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr >& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold) { +size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold) { size_t lastBacksubVariableCount; diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h new file mode 100644 index 000000000..3dcc15aa8 --- /dev/null +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -0,0 +1,127 @@ +/* ---------------------------------------------------------------------------- + + * 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 ISAM2-impl.h + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. + * @author Michael Kaess, Richard Roberts + */ + +#include + +namespace gtsam { + +using namespace std; + +struct ISAM2::Impl { + + struct PartialSolveResult { + typename ISAM2::sharedClique bayesTree; + Permutation fullReordering; + Permutation fullReorderingInverse; + }; + + struct ReorderingMode { + size_t nFullSystemVars; + enum { /*AS_ADDED,*/ COLAMD } algorithm; + enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain; + boost::optional&> constrainedKeys; + }; + + /** + * Add new variables to the ISAM2 system. + * @param newTheta Initial values for new variables + * @param theta Current solution to be augmented with new initialization + * @param delta Current linear delta to be augmented with zeros + * @param ordering Current ordering to be augmented with new variables + * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables + * @param keyFormatter Formatter for printing nonlinear keys during debugging + */ + static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, + Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + + /** + * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol + * in each NonlinearFactor, obtains the index by calling ordering[symbol]. + * @param ordering The current ordering from which to obtain the variable indices + * @param factors The factors from which to extract the variables + * @return The set of variables indices from the factors + */ + static FastSet IndicesFromFactors(const Ordering& ordering, const GRAPH& factors); + + /** + * Find the set of variables to be relinearized according to relinearizeThreshold. + * Any variables in the VectorValues delta whose vector magnitude is greater than + * or equal to relinearizeThreshold are returned. + * @param delta The linear delta to check against the threshold + * @param keyFormatter Formatter for printing nonlinear keys during debugging + * @return The set of variable indices in delta whose magnitude is greater than or + * equal to relinearizeThreshold + */ + static FastSet CheckRelinearization(const Permuted& delta, const Ordering& ordering, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + + /** + * Recursively search this clique and its children for marked keys appearing + * in the separator, and add the *frontal* keys of any cliques whose + * separator contains any marked keys to the set \c keys. The purpose of + * this is to discover the cliques that need to be redone due to information + * propagating to them from cliques that directly contain factors being + * relinearized. + * + * The original comment says this finds all variables directly connected to + * the marked ones by measurements. Is this true, because it seems like this + * would also pull in variables indirectly connected through other frontal or + * separator variables? + * + * Alternatively could we trace up towards the root for each variable here? + */ + static void FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask); + + /** + * Apply expmap to the given values, but only for indices appearing in + * \c markedRelinMask. Values are expmapped in-place. + * \param [in][out] values The value to expmap in-place + * \param delta The linear delta with which to expmap + * \param ordering The ordering + * \param mask Mask on linear indices, only \c true entries are expmapped + * \param invalidateIfDebug If this is true, *and* NDEBUG is not defined, + * expmapped deltas will be set to an invalid value (infinity) to catch bugs + * where we might expmap something twice, or expmap it but then not + * recalculate its delta. + * @param keyFormatter Formatter for printing nonlinear keys during debugging + */ + static void ExpmapMasked(Values& values, const Permuted& delta, + const Ordering& ordering, const std::vector& mask, + boost::optional&> invalidateIfDebug = boost::optional&>(), + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + + /** + * Reorder and eliminate factors. These factors form a subset of the full + * problem, so along with the BayesTree we get a partial reordering of the + * problem that needs to be applied to the other data in ISAM2, which is the + * VariableIndex, the delta, the ordering, and the orphans (including cached + * factors). + * \param factors The factors to eliminate, representing part of the full + * problem. This is permuted during use and so is cleared when this function + * returns in order to invalidate it. + * \param keys The set of indices used in \c factors. + * \return The eliminated BayesTree and the permutation to be applied to the + * rest of the ISAM2 data. + */ + static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, + const ReorderingMode& reorderingMode); + + static size_t UpdateDelta(const boost::shared_ptr >& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold); + +}; + +} diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 0d5f190a3..89f727f8b 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -10,624 +10,136 @@ * -------------------------------------------------------------------------- */ /** - * @file ISAM2-inl.h - * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. - * @author Michael Kaess, Richard Roberts + * @file ISAM2-inl.h + * @brief + * @author Richard Roberts + * @date Mar 16, 2012 */ + #pragma once -#include -#include // for operator += -using namespace boost::assign; -#include -#include -#include -#include -#include - -#include -#include +#include +#include +#include namespace gtsam { using namespace std; -static const bool disableReordering = false; -static const double batchThreshold = 0.65; - /* ************************************************************************* */ -template -ISAM2::ISAM2(const ISAM2Params& params): - delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true), params_(params) { - // See note in gtsam/base/boost_variant_with_workaround.h - if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) - doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; -} +namespace internal { +template +void optimizeWildfire(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 -/* ************************************************************************* */ -template -ISAM2::ISAM2(): - delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true) { - // See note in gtsam/base/boost_variant_with_workaround.h - if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) - doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; -} - -/* ************************************************************************* */ -template -FastList ISAM2::getAffectedFactors(const FastList& keys) const { - static const bool debug = false; - if(debug) cout << "Getting affected factors for "; - if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } } - if(debug) cout << endl; - - FactorGraph allAffected; - FastList indices; - BOOST_FOREACH(const Index key, keys) { -// const list l = nonlinearFactors_.factors(key); -// indices.insert(indices.begin(), l.begin(), l.end()); - const VariableIndex::Factors& factors(variableIndex_[key]); - BOOST_FOREACH(size_t factor, factors) { - if(debug) cout << "Variable " << key << " affects factor " << factor << endl; - indices.push_back(factor); - } + // 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]); } - indices.sort(); - indices.unique(); - if(debug) cout << "Affected factors are: "; - if(debug) { BOOST_FOREACH(const size_t index, indices) { cout << index << " "; } } - if(debug) cout << endl; - return indices; -} +#endif -/* ************************************************************************* */ -// retrieve all factors that ONLY contain the affected variables -// (note that the remaining stuff is summarized in the cached factors) -template -FactorGraph::shared_ptr -ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { - - tic(1,"getAffectedFactors"); - FastList candidates = getAffectedFactors(affectedKeys); - toc(1,"getAffectedFactors"); - - GRAPH nonlinearAffectedFactors; - - tic(2,"affectedKeysSet"); - // for fast lookup below - FastSet affectedKeysSet; - affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); - toc(2,"affectedKeysSet"); - - tic(3,"check candidates"); - BOOST_FOREACH(size_t idx, candidates) { - bool inside = true; - BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) { - Index var = ordering_[key]; - if (affectedKeysSet.find(var) == affectedKeysSet.end()) { - inside = false; + // 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; } } - if (inside) - nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]); } - toc(3,"check candidates"); - tic(4,"linearize"); - FactorGraph::shared_ptr linearized(nonlinearAffectedFactors.linearize(theta_, ordering_)); - toc(4,"linearize"); - return linearized; + // 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)->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_) { + optimizeWildfire(child, threshold, changed, replaced, delta, count); + } + } +} } /* ************************************************************************* */ -// find intermediate (linearized) factors from cache that are passed into the affected area -template -FactorGraph::CacheFactor> -ISAM2::getCachedBoundaryFactors(Cliques& orphans) { - - static const bool debug = false; - - FactorGraph cachedBoundary; - - BOOST_FOREACH(sharedClique orphan, orphans) { - // find the last variable that was eliminated - Index key = (*orphan)->frontals().back(); -#ifndef NDEBUG -// typename BayesNet::const_iterator it = orphan->end(); -// const CONDITIONAL& lastCONDITIONAL = **(--it); -// typename CONDITIONAL::const_iterator keyit = lastCONDITIONAL.endParents(); -// const Index lastKey = *(--keyit); -// assert(key == lastKey); -#endif - // retrieve the cached factor and add to boundary - cachedBoundary.push_back(boost::dynamic_pointer_cast(orphan->cachedFactor())); - if(debug) { cout << "Cached factor for variable " << key; orphan->cachedFactor()->print(""); } - } - - return cachedBoundary; +template +int optimizeWildfire(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 + if(root) + internal::optimizeWildfire(root, threshold, changed, keys, delta, count); + return count; } -template -boost::shared_ptr > ISAM2::recalculate( - const FastSet& markedKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, - const boost::optional >& constrainKeys, ISAM2Result& result) { - - // TODO: new factors are linearized twice, the newFactors passed in are not used. - - static const bool debug = ISDEBUG("ISAM2 recalculate"); - - // Input: BayesTree(this), newFactors - -//#define PRINT_STATS // figures for paper, disable for timing -#ifdef PRINT_STATS - static int counter = 0; - int maxClique = 0; - double avgClique = 0; - int numCliques = 0; - int nnzR = 0; - if (counter>0) { // cannot call on empty tree - GaussianISAM2_P::CliqueData cdata = this->getCliqueData(); - GaussianISAM2_P::CliqueStats cstats = cdata.getStats(); - maxClique = cstats.maxCONDITIONALSize; - avgClique = cstats.avgCONDITIONALSize; - numCliques = cdata.conditionalSizes.size(); - nnzR = calculate_nnz(this->root()); - } - counter++; -#endif - - if(debug) { - cout << "markedKeys: "; - BOOST_FOREACH(const Index key, markedKeys) { cout << key << " "; } - cout << endl; - cout << "newKeys: "; - BOOST_FOREACH(const Index key, newKeys) { cout << key << " "; } - cout << endl; - } - - // 1. Remove top of Bayes tree and convert to a factor graph: - // (a) For each affected variable, remove the corresponding clique and all parents up to the root. - // (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques. - tic(1, "removetop"); - Cliques orphans; - BayesNet affectedBayesNet; - this->removeTop(markedKeys, affectedBayesNet, orphans); - toc(1, "removetop"); - - if(debug) affectedBayesNet.print("Removed top: "); - if(debug) orphans.print("Orphans: "); - - // FactorGraph factors(affectedBayesNet); - // bug was here: we cannot reuse the original factors, because then the cached factors get messed up - // [all the necessary data is actually contained in the affectedBayesNet, including what was passed in from the boundaries, - // so this would be correct; however, in the process we also generate new cached_ entries that will be wrong (ie. they don't - // contain what would be passed up at a certain point if batch elimination was done, but that's what we need); we could choose - // not to update cached_ from here, but then the new information (and potentially different variable ordering) is not reflected - // in the cached_ values which again will be wrong] - // so instead we have to retrieve the original linearized factors AND add the cached factors from the boundary - - // BEGIN OF COPIED CODE - - // ordering provides all keys in conditionals, there cannot be others because path to root included - tic(2,"affectedKeys"); - FastList affectedKeys = affectedBayesNet.ordering(); - toc(2,"affectedKeys"); - - if(affectedKeys.size() >= theta_.size() * batchThreshold) { - - tic(3,"batch"); - - tic(0,"add keys"); - boost::shared_ptr > affectedKeysSet(new FastSet()); - BOOST_FOREACH(const Ordering::value_type& key_index, ordering_) { affectedKeysSet->insert(key_index.second); } - toc(0,"add keys"); - - tic(1,"reorder"); - tic(1,"CCOLAMD"); - // Do a batch step - reorder and relinearize all variables - vector cmember(theta_.size(), 0); - FastSet constrainedKeysSet; - if(constrainKeys) - constrainedKeysSet = *constrainKeys; - else - constrainedKeysSet.insert(newKeys.begin(), newKeys.end()); - if(theta_.size() > constrainedKeysSet.size()) { - BOOST_FOREACH(Index var, constrainedKeysSet) { cmember[var] = 1; } - } - Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember)); - Permutation::shared_ptr colamdInverse(colamd->inverse()); - toc(1,"CCOLAMD"); - - // Reorder - tic(2,"permute global variable index"); - variableIndex_.permute(*colamd); - toc(2,"permute global variable index"); - tic(3,"permute delta"); - delta_.permute(*colamd); - toc(3,"permute delta"); - tic(4,"permute ordering"); - ordering_.permuteWithInverse(*colamdInverse); - toc(4,"permute ordering"); - toc(1,"reorder"); - - tic(2,"linearize"); - GaussianFactorGraph factors(*nonlinearFactors_.linearize(theta_, ordering_)); - toc(2,"linearize"); - - tic(5,"eliminate"); - JunctionTree jt(factors, variableIndex_); - sharedClique newRoot = jt.eliminate(EliminatePreferLDL); - if(debug) newRoot->print("Eliminated: "); - toc(5,"eliminate"); - - tic(6,"insert"); - this->clear(); - this->insert(newRoot); - toc(6,"insert"); - - toc(3,"batch"); - - result.variablesReeliminated = affectedKeysSet->size(); - - lastAffectedMarkedCount = markedKeys.size(); - lastAffectedVariableCount = affectedKeysSet->size(); - lastAffectedFactorCount = factors.size(); - - return affectedKeysSet; - - } else { - - tic(4,"incremental"); - - // 2. Add the new factors \Factors' into the resulting factor graph - FastList affectedAndNewKeys; - affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end()); - affectedAndNewKeys.insert(affectedAndNewKeys.end(), newKeys.begin(), newKeys.end()); - tic(1,"relinearizeAffected"); - GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys)); - if(debug) factors.print("Relinearized factors: "); - toc(1,"relinearizeAffected"); - - if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; } - - result.variablesReeliminated = affectedAndNewKeys.size(); - lastAffectedMarkedCount = markedKeys.size(); - lastAffectedVariableCount = affectedKeys.size(); - lastAffectedFactorCount = factors.size(); - -#ifdef PRINT_STATS - // output for generating figures - cout << "linear: #markedKeys: " << markedKeys.size() << " #affectedVariables: " << affectedKeys.size() - << " #affectedFactors: " << factors.size() << " maxCliqueSize: " << maxClique - << " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl; -#endif - - tic(2,"cached"); - // add the cached intermediate results from the boundary of the orphans ... - FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); - if(debug) cachedBoundary.print("Boundary factors: "); - factors.reserve(factors.size() + cachedBoundary.size()); - // Copy so that we can later permute factors - BOOST_FOREACH(const CacheFactor::shared_ptr& cached, cachedBoundary) { - factors.push_back(GaussianFactor::shared_ptr(new CacheFactor(*cached))); - } - toc(2,"cached"); - - // END OF COPIED CODE - - // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm [alg:BayesTree]) - - tic(4,"reorder and eliminate"); - - tic(1,"list to set"); - // create a partial reordering for the new and contaminated factors - // markedKeys are passed in: those variables will be forced to the end in the ordering - boost::shared_ptr > affectedKeysSet(new FastSet(markedKeys)); - affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); - toc(1,"list to set"); - - tic(2,"PartialSolve"); - typename Impl::ReorderingMode reorderingMode; - reorderingMode.nFullSystemVars = ordering_.nVars(); - reorderingMode.algorithm = Impl::ReorderingMode::COLAMD; - reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST; - if(constrainKeys) - reorderingMode.constrainedKeys = *constrainKeys; - else - reorderingMode.constrainedKeys = FastSet(newKeys.begin(), newKeys.end()); - typename Impl::PartialSolveResult partialSolveResult = - Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode); - toc(2,"PartialSolve"); - - // We now need to permute everything according this partial reordering: the - // delta vector, the global ordering, and the factors we're about to - // re-eliminate. The reordered variables are also mentioned in the - // orphans and the leftover cached factors. - tic(3,"permute global variable index"); - variableIndex_.permute(partialSolveResult.fullReordering); - toc(3,"permute global variable index"); - tic(4,"permute delta"); - delta_.permute(partialSolveResult.fullReordering); - toc(4,"permute delta"); - tic(5,"permute ordering"); - ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse); - toc(5,"permute ordering"); - - toc(4,"reorder and eliminate"); - - tic(6,"re-assemble"); - if(partialSolveResult.bayesTree) { - assert(!this->root_); - this->insert(partialSolveResult.bayesTree); - } - toc(6,"re-assemble"); - - // 4. Insert the orphans back into the new Bayes tree. - tic(7,"orphans"); - tic(1,"permute"); - BOOST_FOREACH(sharedClique orphan, orphans) { - (void)orphan->permuteSeparatorWithInverse(partialSolveResult.fullReorderingInverse); - } - toc(1,"permute"); - tic(2,"insert"); - // add orphans to the bottom of the new tree - BOOST_FOREACH(sharedClique orphan, orphans) { - // Because the affectedKeysSelector is sorted, the orphan separator keys - // will be sorted correctly according to the new elimination order after - // applying the permutation, so findParentClique, which looks for the - // lowest-ordered parent, will still work. - Index parentRepresentative = Base::findParentClique((*orphan)->parents()); - sharedClique parent = (*this)[parentRepresentative]; - parent->children_ += orphan; - orphan->parent_ = parent; // set new parent! - } - toc(2,"insert"); - toc(7,"orphans"); - - toc(4,"incremental"); - - return affectedKeysSet; +/* ************************************************************************* */ +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 -ISAM2Result ISAM2::update( - const GRAPH& newFactors, const Values& newTheta, const FastVector& removeFactorIndices, - const boost::optional >& constrainedKeys, bool force_relinearize) { - - static const bool debug = ISDEBUG("ISAM2 update"); - static const bool verbose = ISDEBUG("ISAM2 update verbose"); - - static int count = 0; - count++; - - lastAffectedVariableCount = 0; - lastAffectedFactorCount = 0; - lastAffectedCliqueCount = 0; - lastAffectedMarkedCount = 0; - lastBacksubVariableCount = 0; - lastNnzTop = 0; - ISAM2Result result; - const bool relinearizeThisStep = force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0); - - if(verbose) { - cout << "ISAM2::update\n"; - this->print("ISAM2: "); - } - - // Update delta if we need it to check relinearization later - if(relinearizeThisStep) { - tic(0, "updateDelta"); - updateDelta(disableReordering); - toc(0, "updateDelta"); - } - - tic(1,"push_back factors"); - // Add the new factor indices to the result struct - result.newFactorsIndices.resize(newFactors.size()); - for(size_t i=0; i markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors - // Also mark keys involved in removed factors - { - FastSet markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors - markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys - } - // NOTE: we use assign instead of the iterator constructor here because this - // is a vector of size_t, so the constructor unintentionally resolves to - // vector(size_t count, Index value) instead of the iterator constructor. - FastVector newKeys; newKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them - toc(4,"gather involved keys"); - - // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold - if (relinearizeThisStep) { - tic(5,"gather relinearize keys"); - vector markedRelinMask(ordering_.nVars(), false); - // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. - FastSet relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold); - if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, ordering_, 0.0); // This is used for debugging - - // Add the variables being relinearized to the marked keys - BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; } - markedKeys.insert(relinKeys.begin(), relinKeys.end()); - toc(5,"gather relinearize keys"); - - tic(6,"fluid find_all"); - // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. - if (!relinKeys.empty() && this->root()) - Impl::FindAll(this->root(), markedKeys, markedRelinMask); // add other cliques that have the marked ones in the separator - toc(6,"fluid find_all"); - - tic(7,"expmap"); - // 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. - if (!relinKeys.empty()) - Impl::ExpmapMasked(theta_, delta_, ordering_, markedRelinMask, delta_); - toc(7,"expmap"); - - result.variablesRelinearized = markedKeys.size(); - -#ifndef NDEBUG - lastRelinVariables_ = markedRelinMask; -#endif - } else { - result.variablesRelinearized = 0; -#ifndef NDEBUG - lastRelinVariables_ = vector(ordering_.nVars(), false); -#endif - } - - tic(8,"linearize new"); - tic(1,"linearize"); - // 7. Linearize new factors - FactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_); - toc(1,"linearize"); - - tic(2,"augment VI"); - // Augment the variable index with the new factors - variableIndex_.augment(*linearFactors); - toc(2,"augment VI"); - toc(8,"linearize new"); - - tic(9,"recalculate"); - // 8. Redo top of Bayes tree - // Convert constrained symbols to indices - boost::optional > constrainedIndices; - if(constrainedKeys) { - constrainedIndices.reset(FastSet()); - BOOST_FOREACH(Key key, *constrainedKeys) { - constrainedIndices->insert(ordering_[key]); - } - } - boost::shared_ptr > replacedKeys; - if(!markedKeys.empty() || !newKeys.empty()) - replacedKeys = recalculate(markedKeys, newKeys, linearFactors, constrainedIndices, result); - - // Update replaced keys mask (accumulates until back-substitution takes place) - if(replacedKeys) { - BOOST_FOREACH(const Index var, *replacedKeys) { - deltaReplacedMask_[var] = true; } } - toc(9,"recalculate"); - - //tic(9,"solve"); - // 9. Solve - if(debug) delta_.print("delta_: "); - //toc(9,"solve"); - - tic(10,"evaluate error after"); - if(params_.evaluateNonlinearError) - result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate())); - toc(10,"evaluate error after"); - - result.cliques = this->nodes().size(); - deltaUptodate_ = false; - +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; } -/* ************************************************************************* */ -template -void ISAM2::updateDelta(bool forceFullSolve) const { - - if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { - // If using Gauss-Newton, update with wildfireThreshold - const ISAM2GaussNewtonParams& gaussNewtonParams = - boost::get(params_.optimizationParams); - const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; - tic(0, "Wildfire update"); - lastBacksubVariableCount = Impl::UpdateDelta(this->root(), deltaReplacedMask_, delta_, effectiveWildfireThreshold); - toc(0, "Wildfire update"); - - } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { - // If using Dogleg, do a Dogleg step - const ISAM2DoglegParams& doglegParams = - boost::get(params_.optimizationParams); - - // Do one Dogleg iteration - tic(1, "Dogleg Iterate"); - DoglegOptimizerImpl::IterationResult doglegResult = DoglegOptimizerImpl::Iterate( - *doglegDelta_, doglegParams.adaptationMode, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose); - toc(1, "Dogleg Iterate"); - - // Update Delta and linear step - doglegDelta_ = doglegResult.Delta; - delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation - delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution - - // Clear replaced mask - deltaReplacedMask_.assign(deltaReplacedMask_.size(), false); - } - - deltaUptodate_ = true; } -/* ************************************************************************* */ -template -Values ISAM2::calculateEstimate() const { - // We use ExpmapMasked here instead of regular expmap because the former - // handles Permuted - Values ret(theta_); - vector mask(ordering_.nVars(), true); - Impl::ExpmapMasked(ret, getDelta(), ordering_, mask); - return ret; -} -/* ************************************************************************* */ -template -template -VALUE ISAM2::calculateEstimate(Key key) const { - const Index index = getOrdering()[key]; - const SubVector delta = getDelta()[index]; - return theta_.at(key).retract(delta); -} - -/* ************************************************************************* */ -template -Values ISAM2::calculateBestEstimate() const { - VectorValues delta(theta_.dims(ordering_)); - optimize2(this->root(), delta); - return theta_.retract(delta, ordering_); -} - -/* ************************************************************************* */ -template -const Permuted& ISAM2::getDelta() const { - if(!deltaUptodate_) - updateDelta(); - return delta_; -} - -} -/// namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp new file mode 100644 index 000000000..01d6bb2cf --- /dev/null +++ b/gtsam/nonlinear/ISAM2.cpp @@ -0,0 +1,687 @@ +/* ---------------------------------------------------------------------------- + + * 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 ISAM2-inl.h + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. + * @author Michael Kaess, Richard Roberts + */ + +#include +#include // for operator += +using namespace boost::assign; + +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace gtsam { + +using namespace std; + +static const bool disableReordering = false; +static const double batchThreshold = 0.65; + +/* ************************************************************************* */ +ISAM2::ISAM2(const ISAM2Params& params): + delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true), params_(params) { + // See note in gtsam/base/boost_variant_with_workaround.h + if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) + doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; +} + +/* ************************************************************************* */ +ISAM2::ISAM2(): + delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true) { + // See note in gtsam/base/boost_variant_with_workaround.h + if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) + doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; +} + +/* ************************************************************************* */ +FastList ISAM2::getAffectedFactors(const FastList& keys) const { + static const bool debug = false; + if(debug) cout << "Getting affected factors for "; + if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } } + if(debug) cout << endl; + + FactorGraph allAffected; + FastList indices; + BOOST_FOREACH(const Index key, keys) { +// const list l = nonlinearFactors_.factors(key); +// indices.insert(indices.begin(), l.begin(), l.end()); + const VariableIndex::Factors& factors(variableIndex_[key]); + BOOST_FOREACH(size_t factor, factors) { + if(debug) cout << "Variable " << key << " affects factor " << factor << endl; + indices.push_back(factor); + } + } + indices.sort(); + indices.unique(); + if(debug) cout << "Affected factors are: "; + if(debug) { BOOST_FOREACH(const size_t index, indices) { cout << index << " "; } } + if(debug) cout << endl; + return indices; +} + +/* ************************************************************************* */ +// retrieve all factors that ONLY contain the affected variables +// (note that the remaining stuff is summarized in the cached factors) +FactorGraph::shared_ptr +ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { + + tic(1,"getAffectedFactors"); + FastList candidates = getAffectedFactors(affectedKeys); + toc(1,"getAffectedFactors"); + + NonlinearFactorGraph nonlinearAffectedFactors; + + tic(2,"affectedKeysSet"); + // for fast lookup below + FastSet affectedKeysSet; + affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); + toc(2,"affectedKeysSet"); + + tic(3,"check candidates"); + BOOST_FOREACH(size_t idx, candidates) { + bool inside = true; + BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) { + Index var = ordering_[key]; + if (affectedKeysSet.find(var) == affectedKeysSet.end()) { + inside = false; + break; + } + } + if (inside) + nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]); + } + toc(3,"check candidates"); + + tic(4,"linearize"); + FactorGraph::shared_ptr linearized(nonlinearAffectedFactors.linearize(theta_, ordering_)); + toc(4,"linearize"); + return linearized; +} + +/* ************************************************************************* */ +// find intermediate (linearized) factors from cache that are passed into the affected area +FactorGraph +ISAM2::getCachedBoundaryFactors(Cliques& orphans) { + + static const bool debug = false; + + FactorGraph cachedBoundary; + + BOOST_FOREACH(sharedClique orphan, orphans) { + // find the last variable that was eliminated + Index key = (*orphan)->frontals().back(); +#ifndef NDEBUG +// typename BayesNet::const_iterator it = orphan->end(); +// const CONDITIONAL& lastCONDITIONAL = **(--it); +// typename CONDITIONAL::const_iterator keyit = lastCONDITIONAL.endParents(); +// const Index lastKey = *(--keyit); +// assert(key == lastKey); +#endif + // retrieve the cached factor and add to boundary + cachedBoundary.push_back(boost::dynamic_pointer_cast(orphan->cachedFactor())); + if(debug) { cout << "Cached factor for variable " << key; orphan->cachedFactor()->print(""); } + } + + return cachedBoundary; +} + +boost::shared_ptr > ISAM2::recalculate( + const FastSet& markedKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, + const boost::optional >& constrainKeys, ISAM2Result& result) { + + // TODO: new factors are linearized twice, the newFactors passed in are not used. + + static const bool debug = ISDEBUG("ISAM2 recalculate"); + + // Input: BayesTree(this), newFactors + +//#define PRINT_STATS // figures for paper, disable for timing +#ifdef PRINT_STATS + static int counter = 0; + int maxClique = 0; + double avgClique = 0; + int numCliques = 0; + int nnzR = 0; + if (counter>0) { // cannot call on empty tree + GaussianISAM2_P::CliqueData cdata = this->getCliqueData(); + GaussianISAM2_P::CliqueStats cstats = cdata.getStats(); + maxClique = cstats.maxCONDITIONALSize; + avgClique = cstats.avgCONDITIONALSize; + numCliques = cdata.conditionalSizes.size(); + nnzR = calculate_nnz(this->root()); + } + counter++; +#endif + + if(debug) { + cout << "markedKeys: "; + BOOST_FOREACH(const Index key, markedKeys) { cout << key << " "; } + cout << endl; + cout << "newKeys: "; + BOOST_FOREACH(const Index key, newKeys) { cout << key << " "; } + cout << endl; + } + + // 1. Remove top of Bayes tree and convert to a factor graph: + // (a) For each affected variable, remove the corresponding clique and all parents up to the root. + // (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques. + tic(1, "removetop"); + Cliques orphans; + BayesNet affectedBayesNet; + this->removeTop(markedKeys, affectedBayesNet, orphans); + toc(1, "removetop"); + + if(debug) affectedBayesNet.print("Removed top: "); + if(debug) orphans.print("Orphans: "); + + // FactorGraph factors(affectedBayesNet); + // bug was here: we cannot reuse the original factors, because then the cached factors get messed up + // [all the necessary data is actually contained in the affectedBayesNet, including what was passed in from the boundaries, + // so this would be correct; however, in the process we also generate new cached_ entries that will be wrong (ie. they don't + // contain what would be passed up at a certain point if batch elimination was done, but that's what we need); we could choose + // not to update cached_ from here, but then the new information (and potentially different variable ordering) is not reflected + // in the cached_ values which again will be wrong] + // so instead we have to retrieve the original linearized factors AND add the cached factors from the boundary + + // BEGIN OF COPIED CODE + + // ordering provides all keys in conditionals, there cannot be others because path to root included + tic(2,"affectedKeys"); + FastList affectedKeys = affectedBayesNet.ordering(); + toc(2,"affectedKeys"); + + if(affectedKeys.size() >= theta_.size() * batchThreshold) { + + tic(3,"batch"); + + tic(0,"add keys"); + boost::shared_ptr > affectedKeysSet(new FastSet()); + BOOST_FOREACH(const Ordering::value_type& key_index, ordering_) { affectedKeysSet->insert(key_index.second); } + toc(0,"add keys"); + + tic(1,"reorder"); + tic(1,"CCOLAMD"); + // Do a batch step - reorder and relinearize all variables + vector cmember(theta_.size(), 0); + FastSet constrainedKeysSet; + if(constrainKeys) + constrainedKeysSet = *constrainKeys; + else + constrainedKeysSet.insert(newKeys.begin(), newKeys.end()); + if(theta_.size() > constrainedKeysSet.size()) { + BOOST_FOREACH(Index var, constrainedKeysSet) { cmember[var] = 1; } + } + Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember)); + Permutation::shared_ptr colamdInverse(colamd->inverse()); + toc(1,"CCOLAMD"); + + // Reorder + tic(2,"permute global variable index"); + variableIndex_.permute(*colamd); + toc(2,"permute global variable index"); + tic(3,"permute delta"); + delta_.permute(*colamd); + toc(3,"permute delta"); + tic(4,"permute ordering"); + ordering_.permuteWithInverse(*colamdInverse); + toc(4,"permute ordering"); + toc(1,"reorder"); + + tic(2,"linearize"); + GaussianFactorGraph factors(*nonlinearFactors_.linearize(theta_, ordering_)); + toc(2,"linearize"); + + tic(5,"eliminate"); + JunctionTree jt(factors, variableIndex_); + sharedClique newRoot = jt.eliminate(EliminatePreferLDL); + if(debug) newRoot->print("Eliminated: "); + toc(5,"eliminate"); + + tic(6,"insert"); + this->clear(); + this->insert(newRoot); + toc(6,"insert"); + + toc(3,"batch"); + + result.variablesReeliminated = affectedKeysSet->size(); + + lastAffectedMarkedCount = markedKeys.size(); + lastAffectedVariableCount = affectedKeysSet->size(); + lastAffectedFactorCount = factors.size(); + + return affectedKeysSet; + + } else { + + tic(4,"incremental"); + + // 2. Add the new factors \Factors' into the resulting factor graph + FastList affectedAndNewKeys; + affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end()); + affectedAndNewKeys.insert(affectedAndNewKeys.end(), newKeys.begin(), newKeys.end()); + tic(1,"relinearizeAffected"); + GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys)); + if(debug) factors.print("Relinearized factors: "); + toc(1,"relinearizeAffected"); + + if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; } + + result.variablesReeliminated = affectedAndNewKeys.size(); + lastAffectedMarkedCount = markedKeys.size(); + lastAffectedVariableCount = affectedKeys.size(); + lastAffectedFactorCount = factors.size(); + +#ifdef PRINT_STATS + // output for generating figures + cout << "linear: #markedKeys: " << markedKeys.size() << " #affectedVariables: " << affectedKeys.size() + << " #affectedFactors: " << factors.size() << " maxCliqueSize: " << maxClique + << " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl; +#endif + + tic(2,"cached"); + // add the cached intermediate results from the boundary of the orphans ... + FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); + if(debug) cachedBoundary.print("Boundary factors: "); + factors.reserve(factors.size() + cachedBoundary.size()); + // Copy so that we can later permute factors + BOOST_FOREACH(const CacheFactor::shared_ptr& cached, cachedBoundary) { + factors.push_back(GaussianFactor::shared_ptr(new CacheFactor(*cached))); + } + toc(2,"cached"); + + // END OF COPIED CODE + + // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm [alg:BayesTree]) + + tic(4,"reorder and eliminate"); + + tic(1,"list to set"); + // create a partial reordering for the new and contaminated factors + // markedKeys are passed in: those variables will be forced to the end in the ordering + boost::shared_ptr > affectedKeysSet(new FastSet(markedKeys)); + affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); + toc(1,"list to set"); + + tic(2,"PartialSolve"); + typename Impl::ReorderingMode reorderingMode; + reorderingMode.nFullSystemVars = ordering_.nVars(); + reorderingMode.algorithm = Impl::ReorderingMode::COLAMD; + reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST; + if(constrainKeys) + reorderingMode.constrainedKeys = *constrainKeys; + else + reorderingMode.constrainedKeys = FastSet(newKeys.begin(), newKeys.end()); + typename Impl::PartialSolveResult partialSolveResult = + Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode); + toc(2,"PartialSolve"); + + // We now need to permute everything according this partial reordering: the + // delta vector, the global ordering, and the factors we're about to + // re-eliminate. The reordered variables are also mentioned in the + // orphans and the leftover cached factors. + tic(3,"permute global variable index"); + variableIndex_.permute(partialSolveResult.fullReordering); + toc(3,"permute global variable index"); + tic(4,"permute delta"); + delta_.permute(partialSolveResult.fullReordering); + toc(4,"permute delta"); + tic(5,"permute ordering"); + ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse); + toc(5,"permute ordering"); + + toc(4,"reorder and eliminate"); + + tic(6,"re-assemble"); + if(partialSolveResult.bayesTree) { + assert(!this->root_); + this->insert(partialSolveResult.bayesTree); + } + toc(6,"re-assemble"); + + // 4. Insert the orphans back into the new Bayes tree. + tic(7,"orphans"); + tic(1,"permute"); + BOOST_FOREACH(sharedClique orphan, orphans) { + (void)orphan->permuteSeparatorWithInverse(partialSolveResult.fullReorderingInverse); + } + toc(1,"permute"); + tic(2,"insert"); + // add orphans to the bottom of the new tree + BOOST_FOREACH(sharedClique orphan, orphans) { + // Because the affectedKeysSelector is sorted, the orphan separator keys + // will be sorted correctly according to the new elimination order after + // applying the permutation, so findParentClique, which looks for the + // lowest-ordered parent, will still work. + Index parentRepresentative = Base::findParentClique((*orphan)->parents()); + sharedClique parent = (*this)[parentRepresentative]; + parent->children_ += orphan; + orphan->parent_ = parent; // set new parent! + } + toc(2,"insert"); + toc(7,"orphans"); + + toc(4,"incremental"); + + return affectedKeysSet; + } +} + +/* ************************************************************************* */ +ISAM2Result ISAM2::update( + const NonlinearFactorGraph& newFactors, const Values& newTheta, const FastVector& removeFactorIndices, + const boost::optional >& constrainedKeys, bool force_relinearize) { + + static const bool debug = ISDEBUG("ISAM2 update"); + static const bool verbose = ISDEBUG("ISAM2 update verbose"); + + static int count = 0; + count++; + + lastAffectedVariableCount = 0; + lastAffectedFactorCount = 0; + lastAffectedCliqueCount = 0; + lastAffectedMarkedCount = 0; + lastBacksubVariableCount = 0; + lastNnzTop = 0; + ISAM2Result result; + const bool relinearizeThisStep = force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0); + + if(verbose) { + cout << "ISAM2::update\n"; + this->print("ISAM2: "); + } + + // Update delta if we need it to check relinearization later + if(relinearizeThisStep) { + tic(0, "updateDelta"); + updateDelta(disableReordering); + toc(0, "updateDelta"); + } + + tic(1,"push_back factors"); + // Add the new factor indices to the result struct + result.newFactorsIndices.resize(newFactors.size()); + for(size_t i=0; i markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors + // Also mark keys involved in removed factors + { + FastSet markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors + markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys + } + // NOTE: we use assign instead of the iterator constructor here because this + // is a vector of size_t, so the constructor unintentionally resolves to + // vector(size_t count, Index value) instead of the iterator constructor. + FastVector newKeys; newKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them + toc(4,"gather involved keys"); + + // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold + if (relinearizeThisStep) { + tic(5,"gather relinearize keys"); + vector markedRelinMask(ordering_.nVars(), false); + // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. + FastSet relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold); + if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, ordering_, 0.0); // This is used for debugging + + // Add the variables being relinearized to the marked keys + BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; } + markedKeys.insert(relinKeys.begin(), relinKeys.end()); + toc(5,"gather relinearize keys"); + + tic(6,"fluid find_all"); + // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. + if (!relinKeys.empty() && this->root()) + Impl::FindAll(this->root(), markedKeys, markedRelinMask); // add other cliques that have the marked ones in the separator + toc(6,"fluid find_all"); + + tic(7,"expmap"); + // 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. + if (!relinKeys.empty()) + Impl::ExpmapMasked(theta_, delta_, ordering_, markedRelinMask, delta_); + toc(7,"expmap"); + + result.variablesRelinearized = markedKeys.size(); + +#ifndef NDEBUG + lastRelinVariables_ = markedRelinMask; +#endif + } else { + result.variablesRelinearized = 0; +#ifndef NDEBUG + lastRelinVariables_ = vector(ordering_.nVars(), false); +#endif + } + + tic(8,"linearize new"); + tic(1,"linearize"); + // 7. Linearize new factors + FactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_); + toc(1,"linearize"); + + tic(2,"augment VI"); + // Augment the variable index with the new factors + variableIndex_.augment(*linearFactors); + toc(2,"augment VI"); + toc(8,"linearize new"); + + tic(9,"recalculate"); + // 8. Redo top of Bayes tree + // Convert constrained symbols to indices + boost::optional > constrainedIndices; + if(constrainedKeys) { + constrainedIndices.reset(FastSet()); + BOOST_FOREACH(Key key, *constrainedKeys) { + constrainedIndices->insert(ordering_[key]); + } + } + boost::shared_ptr > replacedKeys; + if(!markedKeys.empty() || !newKeys.empty()) + replacedKeys = recalculate(markedKeys, newKeys, linearFactors, constrainedIndices, result); + + // Update replaced keys mask (accumulates until back-substitution takes place) + if(replacedKeys) { + BOOST_FOREACH(const Index var, *replacedKeys) { + deltaReplacedMask_[var] = true; } } + toc(9,"recalculate"); + + //tic(9,"solve"); + // 9. Solve + if(debug) delta_.print("delta_: "); + //toc(9,"solve"); + + tic(10,"evaluate error after"); + if(params_.evaluateNonlinearError) + result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate())); + toc(10,"evaluate error after"); + + result.cliques = this->nodes().size(); + deltaUptodate_ = false; + + return result; +} + +/* ************************************************************************* */ +void ISAM2::updateDelta(bool forceFullSolve) const { + + if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { + // If using Gauss-Newton, update with wildfireThreshold + const ISAM2GaussNewtonParams& gaussNewtonParams = + boost::get(params_.optimizationParams); + const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; + tic(0, "Wildfire update"); + lastBacksubVariableCount = Impl::UpdateDelta(this->root(), deltaReplacedMask_, delta_, effectiveWildfireThreshold); + toc(0, "Wildfire update"); + + } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { + // If using Dogleg, do a Dogleg step + const ISAM2DoglegParams& doglegParams = + boost::get(params_.optimizationParams); + + // Do one Dogleg iteration + tic(1, "Dogleg Iterate"); + DoglegOptimizerImpl::IterationResult doglegResult = DoglegOptimizerImpl::Iterate( + *doglegDelta_, doglegParams.adaptationMode, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose); + toc(1, "Dogleg Iterate"); + + // Update Delta and linear step + doglegDelta_ = doglegResult.Delta; + delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation + delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution + + // Clear replaced mask + deltaReplacedMask_.assign(deltaReplacedMask_.size(), false); + } + + deltaUptodate_ = true; +} + +/* ************************************************************************* */ +Values ISAM2::calculateEstimate() const { + // We use ExpmapMasked here instead of regular expmap because the former + // handles Permuted + Values ret(theta_); + vector mask(ordering_.nVars(), true); + Impl::ExpmapMasked(ret, getDelta(), ordering_, mask); + return ret; +} + +/* ************************************************************************* */ +template +VALUE ISAM2::calculateEstimate(Key key) const { + const Index index = getOrdering()[key]; + const SubVector delta = getDelta()[index]; + return theta_.at(key).retract(delta); +} + +/* ************************************************************************* */ +Values ISAM2::calculateBestEstimate() const { + VectorValues delta(theta_.dims(ordering_)); + optimize2(this->root(), delta); + return theta_.retract(delta, ordering_); +} + +/* ************************************************************************* */ +const Permuted& ISAM2::getDelta() const { + if(!deltaUptodate_) + updateDelta(); + return delta_; +} + +/* ************************************************************************* */ +VectorValues optimizeGradientSearch(const ISAM2& isam) { + tic(0, "Allocate VectorValues"); + VectorValues grad = *allocateVectorValues(isam); + toc(0, "Allocate VectorValues"); + + optimizeGradientSearchInPlace(isam, grad); + + return grad; +} + +/* ************************************************************************* */ +void optimizeGradientSearchInPlace(const ISAM2& Rd, VectorValues& grad) { + tic(1, "Compute Gradient"); + // Compute gradient (call gradientAtZero function, which is defined for various linear systems) + gradientAtZero(Rd, grad); + double gradientSqNorm = grad.dot(grad); + toc(1, "Compute Gradient"); + + tic(2, "Compute R*g"); + // Compute R * g + FactorGraph Rd_jfg(Rd); + Errors Rg = Rd_jfg * grad; + toc(2, "Compute R*g"); + + tic(3, "Compute minimizing step size"); + // Compute minimizing step size + double step = -gradientSqNorm / dot(Rg, Rg); + toc(3, "Compute minimizing step size"); + + tic(4, "Compute point"); + // Compute steepest descent point + scal(step, grad); + toc(4, "Compute point"); +} + +/* ************************************************************************* */ +VectorValues gradient(const BayesTree& bayesTree, const VectorValues& x0) { + return gradient(FactorGraph(bayesTree), x0); +} + +/* ************************************************************************* */ +static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, VectorValues& g) { + // Loop through variables in each clique, adding contributions + int variablePosition = 0; + for(GaussianConditional::const_iterator jit = root->conditional()->begin(); jit != root->conditional()->end(); ++jit) { + const int dim = root->conditional()->dim(jit); + g[*jit] += root->gradientContribution().segment(variablePosition, dim); + variablePosition += dim; + } + + // Recursively add contributions from children + typedef boost::shared_ptr sharedClique; + BOOST_FOREACH(const sharedClique& child, root->children()) { + gradientAtZeroTreeAdder(child, g); + } +} + +/* ************************************************************************* */ +void gradientAtZero(const BayesTree& bayesTree, VectorValues& g) { + // Zero-out gradient + g.setZero(); + + // Sum up contributions for each clique + gradientAtZeroTreeAdder(bayesTree.root(), g); +} + +} +/// namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 208f0748f..d5a1cac52 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -12,7 +12,7 @@ /** * @file ISAM2.h * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. - * @author Michael Kaess + * @author Michael Kaess, Richard Roberts */ // \callgraph @@ -23,8 +23,6 @@ #include #include -// Workaround for boost < 1.47, see note in file -//#include #include namespace gtsam { @@ -181,14 +179,13 @@ struct ISAM2Result { FastVector newFactorsIndices; }; -template -struct ISAM2Clique : public BayesTreeCliqueBase, CONDITIONAL> { +struct ISAM2Clique : public BayesTreeCliqueBase { - typedef ISAM2Clique This; - typedef BayesTreeCliqueBase Base; + typedef ISAM2Clique This; + typedef BayesTreeCliqueBase Base; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; - typedef CONDITIONAL ConditionalType; + typedef GaussianConditional ConditionalType; typedef typename ConditionalType::shared_ptr sharedConditional; typename Base::FactorType::shared_ptr cachedFactor_; @@ -269,8 +266,7 @@ private: * estimate of all variables. * */ -template -class ISAM2: public BayesTree > { +class ISAM2: public BayesTree { protected: @@ -316,7 +312,7 @@ protected: mutable std::vector deltaReplacedMask_; /** All original nonlinear factors are stored here to use during relinearization */ - GRAPH nonlinearFactors_; + NonlinearFactorGraph nonlinearFactors_; /** The current elimination ordering Symbols to Index (integer) keys. * @@ -339,9 +335,7 @@ private: public: - typedef BayesTree > Base; ///< The BayesTree base class - typedef ISAM2 This; ///< This class - typedef GRAPH Graph; + typedef BayesTree Base; ///< The BayesTree base class /** Create an empty ISAM2 instance */ ISAM2(const ISAM2Params& params); @@ -353,7 +347,7 @@ public: typedef typename Base::sharedClique sharedClique; ///< Shared pointer to a clique typedef typename Base::Cliques Cliques; ///< List of Clique typedef from base class - void cloneTo(boost::shared_ptr& newISAM2) const { + void cloneTo(boost::shared_ptr& newISAM2) const { boost::shared_ptr bayesTree = boost::static_pointer_cast(newISAM2); Base::cloneTo(bayesTree); newISAM2->theta_ = theta_; @@ -395,7 +389,7 @@ public: * (Params::relinearizeSkip). * @return An ISAM2Result struct containing information about the update */ - ISAM2Result update(const GRAPH& newFactors = GRAPH(), const Values& newTheta = Values(), + ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const FastVector& removeFactorIndices = FastVector(), const boost::optional >& constrainedKeys = boost::none, bool force_relinearize = false); @@ -432,7 +426,7 @@ public: const Permuted& getDelta() const; /** Access the set of nonlinear factors */ - const GRAPH& getFactorsUnsafe() const { return nonlinearFactors_; } + const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } /** Access the current ordering */ const Ordering& getOrdering() const { return ordering_; } @@ -462,6 +456,88 @@ private: }; // ISAM2 +/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ +VectorValues optimize(const ISAM2& isam) { + VectorValues delta = *allocateVectorValues(isam); + internal::optimizeInPlace(isam.root(), delta); + return delta; +} + +/// Optimize the BayesTree, starting from the root. +/// @param replaced Needs to contain +/// all variables that are contained in the top of the Bayes tree that has been +/// redone. +/// @param delta The current solution, an offset from the linearization +/// point. +/// @param threshold 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. +/// @return The number of variables that were solved for +template +int optimizeWildfire(const boost::shared_ptr& root, + double threshold, const std::vector& replaced, Permuted& delta); + +/** + * Optimize along the gradient direction, with a closed-form computation to + * perform the line search. The gradient is computed about \f$ \delta x=0 \f$. + * + * This function returns \f$ \delta x \f$ that minimizes a reparametrized + * problem. The error function of a GaussianBayesNet is + * + * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f] + * + * with gradient and Hessian + * + * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] + * + * This function performs the line search in the direction of the + * gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size + * \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$: + * + * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] + * + * Optimizing by setting the derivative to zero yields + * \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function + * evaluates the denominator without computing the Hessian \f$ G \f$, returning + * + * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] + */ +VectorValues optimizeGradientSearch(const ISAM2& isam); + +/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */ +void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad); + +/// 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$. + * 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 + */ +void gradientAtZero(const BayesTree& bayesTree, VectorValues& g); + } /// namespace gtsam #include From 6a957d059b0d649e985f1b012e9220627a13b635 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sat, 17 Mar 2012 23:57:39 +0000 Subject: [PATCH 04/16] Fixed warnings about mixing class and struct --- gtsam/inference/tests/testEliminationTree.cpp | 3 ++- gtsam/linear/GaussianFactorGraph.h | 2 +- gtsam/linear/HessianFactor.h | 2 +- gtsam/linear/NoiseModel.h | 2 +- gtsam/linear/SharedDiagonal.h | 3 ++- gtsam/linear/SharedGaussian.h | 3 ++- 6 files changed, 9 insertions(+), 6 deletions(-) diff --git a/gtsam/inference/tests/testEliminationTree.cpp b/gtsam/inference/tests/testEliminationTree.cpp index b1efc26c8..0ec8b2266 100644 --- a/gtsam/inference/tests/testEliminationTree.cpp +++ b/gtsam/inference/tests/testEliminationTree.cpp @@ -25,7 +25,8 @@ using namespace gtsam; using namespace std; -struct EliminationTreeTester { +class EliminationTreeTester { +public: // build hardcoded tree static SymbolicEliminationTree::shared_ptr buildHardcodedTree(const SymbolicFactorGraph& fg) { diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index f48f7f13f..822c8f62a 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -31,7 +31,7 @@ namespace gtsam { - struct SharedDiagonal; + class SharedDiagonal; /** return A*x-b * \todo Make this a member function - affects SubgraphPreconditioner */ diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index c1e5f016a..7cba0c744 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -33,7 +33,7 @@ namespace gtsam { // Forward declarations class JacobianFactor; - struct SharedDiagonal; + class SharedDiagonal; class GaussianConditional; template class BayesNet; diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index ee5d9e00c..b6cf4deac 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -23,7 +23,7 @@ namespace gtsam { - struct SharedDiagonal; // forward declare + class SharedDiagonal; // forward declare /// All noise models live in the noiseModel namespace namespace noiseModel { diff --git a/gtsam/linear/SharedDiagonal.h b/gtsam/linear/SharedDiagonal.h index b839cca1a..8a7e101a2 100644 --- a/gtsam/linear/SharedDiagonal.h +++ b/gtsam/linear/SharedDiagonal.h @@ -25,7 +25,8 @@ namespace gtsam { // note, deliberately not in noiseModel namespace // A useful convenience class to refer to a shared Diagonal model // There are (somewhat dangerous) constructors from Vector and pair // that call Sigmas and Sigma, respectively. - struct SharedDiagonal: public noiseModel::Diagonal::shared_ptr { + class SharedDiagonal: public noiseModel::Diagonal::shared_ptr { + public: SharedDiagonal() { } SharedDiagonal(const noiseModel::Diagonal::shared_ptr& p) : diff --git a/gtsam/linear/SharedGaussian.h b/gtsam/linear/SharedGaussian.h index 4748efd04..a3154bac1 100644 --- a/gtsam/linear/SharedGaussian.h +++ b/gtsam/linear/SharedGaussian.h @@ -27,7 +27,8 @@ namespace gtsam { // note, deliberately not in noiseModel namespace * A useful convenience class to refer to a shared Gaussian model * Also needed to make noise models in matlab */ - struct SharedGaussian: public SharedNoiseModel { + class SharedGaussian: public SharedNoiseModel { + public: typedef SharedNoiseModel Base; From 047dda05d70caf18d3759c75987efeee812d73a4 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sat, 17 Mar 2012 23:57:42 +0000 Subject: [PATCH 05/16] Removed ISAM2 templating --- .cproject | 2 +- .project | 2 +- gtsam/linear/GaussianBayesTree-inl.h | 13 ++++++ gtsam/linear/GaussianBayesTree.cpp | 24 +++-------- gtsam/linear/GaussianBayesTree.h | 3 +- gtsam/linear/GaussianJunctionTree.cpp | 2 +- gtsam/nonlinear/ISAM2-impl.cpp | 13 +++--- gtsam/nonlinear/ISAM2-impl.h | 13 +++--- gtsam/nonlinear/ISAM2.cpp | 18 +++++--- gtsam/nonlinear/ISAM2.h | 25 +++++------- tests/testGaussianISAM2.cpp | 59 +++++++++++++-------------- 11 files changed, 91 insertions(+), 83 deletions(-) diff --git a/.cproject b/.cproject index c6c10d6db..6a32e5c08 100644 --- a/.cproject +++ b/.cproject @@ -21,7 +21,7 @@ - + diff --git a/.project b/.project index c4d531b04..e52e979df 100644 --- a/.project +++ b/.project @@ -23,7 +23,7 @@ org.eclipse.cdt.make.core.buildArguments - + -j8 org.eclipse.cdt.make.core.buildCommand diff --git a/gtsam/linear/GaussianBayesTree-inl.h b/gtsam/linear/GaussianBayesTree-inl.h index 9cb327bec..4972b13bf 100644 --- a/gtsam/linear/GaussianBayesTree-inl.h +++ b/gtsam/linear/GaussianBayesTree-inl.h @@ -25,4 +25,17 @@ namespace gtsam { +/* ************************************************************************* */ +namespace internal { +template +void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result) { + // parents are assumed to already be solved and available in result + clique->conditional()->solveInPlace(result); + + // starting from the root, call optimize on each conditional + BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_) + optimizeInPlace(child, result); +} +} + } diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index 4243657e5..ae76d958e 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -23,22 +23,15 @@ namespace gtsam { /* ************************************************************************* */ -namespace internal { -void optimizeInPlace(const boost::shared_ptr >& clique, VectorValues& result) { - // parents are assumed to already be solved and available in result - clique->conditional()->solveInPlace(result); - - // starting from the root, call optimize on each conditional - BOOST_FOREACH(const boost::shared_ptr >& child, clique->children_) - optimizeInPlace(child, result); -} +VectorValues optimize(const GaussianBayesTree& bayesTree) { + VectorValues result = *allocateVectorValues(bayesTree); + optimizeInPlace(bayesTree, result); + return result; } /* ************************************************************************* */ -VectorValues optimize(const GaussianBayesTree& bayesTree) { - VectorValues result = *allocateVectorValues(bayesTree); - internal::optimizeInPlace(bayesTree.root(), result); - return result; +void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result) { + internal::optimizeInPlace(bayesTree.root(), result); } /* ************************************************************************* */ @@ -77,11 +70,6 @@ void optimizeGradientSearchInPlace(const GaussianBayesTree& Rd, VectorValues& gr toc(4, "Compute point"); } -/* ************************************************************************* */ -void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result) { - internal::optimizeInPlace(bayesTree.root(), result); -} - /* ************************************************************************* */ VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0) { return gradient(FactorGraph(bayesTree), x0); diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h index bb15eb063..8b581351c 100644 --- a/gtsam/linear/GaussianBayesTree.h +++ b/gtsam/linear/GaussianBayesTree.h @@ -34,7 +34,8 @@ VectorValues optimize(const GaussianBayesTree& bayesTree); void optimizeInPlace(const GaussianBayesTree& clique, VectorValues& result); namespace internal { -void optimizeInPlace(const boost::shared_ptr >& clique, VectorValues& result); +template +void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result); } /** diff --git a/gtsam/linear/GaussianJunctionTree.cpp b/gtsam/linear/GaussianJunctionTree.cpp index 2c2a44dbb..87063bb48 100644 --- a/gtsam/linear/GaussianJunctionTree.cpp +++ b/gtsam/linear/GaussianJunctionTree.cpp @@ -50,7 +50,7 @@ namespace gtsam { // back-substitution tic(3, "back-substitute"); - internal::optimizeInPlace(rootClique, result); + internal::optimizeInPlace(rootClique, result); toc(3, "back-substitute"); return result; } diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 8dce6934a..b6d018796 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -16,13 +16,14 @@ */ #include +#include namespace gtsam { /* ************************************************************************* */ void ISAM2::Impl::AddVariables( const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, - Ordering& ordering,typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) { + Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); theta.insert(newTheta); @@ -54,9 +55,9 @@ void ISAM2::Impl::AddVariables( } /* ************************************************************************* */ -FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) { +FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors) { FastSet indices; - BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) { + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) { BOOST_FOREACH(Key key, factor->keys()) { indices.insert(ordering[key]); } @@ -93,7 +94,7 @@ FastSet ISAM2::Impl::CheckRelinearization(const Permuted& d } /* ************************************************************************* */ -void ISAM2::Impl::FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask) { +void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask) { static const bool debug = false; // does the separator contain any of the variables? bool found = false; @@ -107,7 +108,7 @@ void ISAM2::Impl::FindAll(typename ISAM2Clique::shared_ptr clique, FastSetprint("Key(s) marked in clique "); if(debug) cout << "so marking key " << (*clique)->keys().front() << endl; } - BOOST_FOREACH(const typename ISAM2Clique::shared_ptr& child, clique->children_) { + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children_) { FindAll(child, keys, markedMask); } } @@ -226,7 +227,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, // eliminate into a Bayes net tic(7,"eliminate"); - JunctionTree jt(factors, affectedFactorsIndex); + JunctionTree jt(factors, affectedFactorsIndex); result.bayesTree = jt.eliminate(EliminatePreferLDL); toc(7,"eliminate"); diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 3dcc15aa8..f49a385bc 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -15,7 +15,10 @@ * @author Michael Kaess, Richard Roberts */ +#pragma once + #include +#include namespace gtsam { @@ -24,7 +27,7 @@ using namespace std; struct ISAM2::Impl { struct PartialSolveResult { - typename ISAM2::sharedClique bayesTree; + ISAM2::sharedClique bayesTree; Permutation fullReordering; Permutation fullReorderingInverse; }; @@ -46,7 +49,7 @@ struct ISAM2::Impl { * @param keyFormatter Formatter for printing nonlinear keys during debugging */ static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, - Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol @@ -55,7 +58,7 @@ struct ISAM2::Impl { * @param factors The factors from which to extract the variables * @return The set of variables indices from the factors */ - static FastSet IndicesFromFactors(const Ordering& ordering, const GRAPH& factors); + static FastSet IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors); /** * Find the set of variables to be relinearized according to relinearizeThreshold. @@ -84,7 +87,7 @@ struct ISAM2::Impl { * * Alternatively could we trace up towards the root for each variable here? */ - static void FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask); + static void FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask); /** * Apply expmap to the given values, but only for indices appearing in @@ -120,7 +123,7 @@ struct ISAM2::Impl { static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, const ReorderingMode& reorderingMode); - static size_t UpdateDelta(const boost::shared_ptr >& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold); + static size_t UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold); }; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 01d6bb2cf..58cc4b23c 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -26,7 +26,6 @@ using namespace boost::assign; #include #include -#include #include @@ -120,7 +119,7 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { /* ************************************************************************* */ // find intermediate (linearized) factors from cache that are passed into the affected area -FactorGraph +FactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { static const bool debug = false; @@ -252,7 +251,7 @@ boost::shared_ptr > ISAM2::recalculate( toc(2,"linearize"); tic(5,"eliminate"); - JunctionTree jt(factors, variableIndex_); + JunctionTree jt(factors, variableIndex_); sharedClique newRoot = jt.eliminate(EliminatePreferLDL); if(debug) newRoot->print("Eliminated: "); toc(5,"eliminate"); @@ -324,7 +323,7 @@ boost::shared_ptr > ISAM2::recalculate( toc(1,"list to set"); tic(2,"PartialSolve"); - typename Impl::ReorderingMode reorderingMode; + Impl::ReorderingMode reorderingMode; reorderingMode.nFullSystemVars = ordering_.nVars(); reorderingMode.algorithm = Impl::ReorderingMode::COLAMD; reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST; @@ -332,7 +331,7 @@ boost::shared_ptr > ISAM2::recalculate( reorderingMode.constrainedKeys = *constrainKeys; else reorderingMode.constrainedKeys = FastSet(newKeys.begin(), newKeys.end()); - typename Impl::PartialSolveResult partialSolveResult = + Impl::PartialSolveResult partialSolveResult = Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode); toc(2,"PartialSolve"); @@ -605,7 +604,7 @@ VALUE ISAM2::calculateEstimate(Key key) const { /* ************************************************************************* */ Values ISAM2::calculateBestEstimate() const { VectorValues delta(theta_.dims(ordering_)); - optimize2(this->root(), delta); + internal::optimizeInPlace(this->root(), delta); return theta_.retract(delta, ordering_); } @@ -616,6 +615,13 @@ const Permuted& ISAM2::getDelta() const { return delta_; } +/* ************************************************************************* */ +VectorValues optimize(const ISAM2& isam) { + VectorValues delta = *allocateVectorValues(isam); + internal::optimizeInPlace(isam.root(), delta); + return delta; +} + /* ************************************************************************* */ VectorValues optimizeGradientSearch(const ISAM2& isam) { tic(0, "Allocate VectorValues"); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index d5a1cac52..2d5b16d1b 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -21,7 +21,7 @@ #include #include -#include +#include #include @@ -186,9 +186,9 @@ struct ISAM2Clique : public BayesTreeCliqueBase shared_ptr; typedef boost::weak_ptr weak_ptr; typedef GaussianConditional ConditionalType; - typedef typename ConditionalType::shared_ptr sharedConditional; + typedef ConditionalType::shared_ptr sharedConditional; - typename Base::FactorType::shared_ptr cachedFactor_; + Base::FactorType::shared_ptr cachedFactor_; Vector gradientContribution_; /** Construct from a conditional */ @@ -196,7 +196,7 @@ struct ISAM2Clique : public BayesTreeCliqueBase >& result) : + ISAM2Clique(const std::pair >& result) : Base(result.first), cachedFactor_(result.second), gradientContribution_(result.first->get_R().cols() + result.first->get_S().cols()) { // Compute gradient contribution const ConditionalType& conditional(*result.first); @@ -208,13 +208,13 @@ struct ISAM2Clique : public BayesTreeCliqueBaseclone() : typename Base::FactorType::shared_ptr()))); + cachedFactor_ ? cachedFactor_->clone() : Base::FactorType::shared_ptr()))); copy->gradientContribution_ = gradientContribution_; return copy; } /** Access the cached factor */ - typename Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } + Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } /** Access the gradient contribution */ const Vector& gradientContribution() const { return gradientContribution_; } @@ -343,9 +343,9 @@ public: /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ ISAM2(); - typedef typename Base::Clique Clique; ///< A clique - typedef typename Base::sharedClique sharedClique; ///< Shared pointer to a clique - typedef typename Base::Cliques Cliques; ///< List of Clique typedef from base class + typedef Base::Clique Clique; ///< A clique + typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique + typedef Base::Cliques Cliques; ///< List of Clique typedef from base class void cloneTo(boost::shared_ptr& newISAM2) const { boost::shared_ptr bayesTree = boost::static_pointer_cast(newISAM2); @@ -457,11 +457,7 @@ private: }; // ISAM2 /** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ -VectorValues optimize(const ISAM2& isam) { - VectorValues delta = *allocateVectorValues(isam); - internal::optimizeInPlace(isam.root(), delta); - return delta; -} +VectorValues optimize(const ISAM2& isam); /// Optimize the BayesTree, starting from the root. /// @param replaced Needs to contain @@ -541,3 +537,4 @@ void gradientAtZero(const BayesTree& bayesTree } /// namespace gtsam #include +#include diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 21f2e62e9..831d17478 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -17,7 +17,7 @@ using namespace boost::assign; #include #include #include -#include +#include #include #include @@ -52,7 +52,7 @@ TEST(ISAM2, AddVariables) { Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0); - GaussianISAM2<>::Nodes nodes(2); + ISAM2::Nodes nodes(2); // Verify initial state LONGS_EQUAL(0, ordering[planarSLAM::PointKey(0)]); @@ -82,11 +82,11 @@ TEST(ISAM2, AddVariables) { Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1); - GaussianISAM2<>::Nodes nodesExpected( - 3, GaussianISAM2<>::sharedClique()); + ISAM2::Nodes nodesExpected( + 3, ISAM2::sharedClique()); // Expand initial state - GaussianISAM2<>::Impl::AddVariables(newTheta, theta, delta, replacedKeys, ordering, nodes); + ISAM2::Impl::AddVariables(newTheta, theta, delta, replacedKeys, ordering, nodes); EXPECT(assert_equal(thetaExpected, theta)); EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted)); @@ -171,10 +171,10 @@ TEST(ISAM2, optimize2) { conditional->solveInPlace(expected); // Clique - GaussianISAM2<>::sharedClique clique( - GaussianISAM2<>::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr()))); + ISAM2::sharedClique clique( + ISAM2::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr()))); VectorValues actual(theta.dims(ordering)); - internal::optimizeInPlace(clique, actual); + internal::optimizeInPlace(clique, actual); // expected.print("expected: "); // actual.print("actual: "); @@ -182,7 +182,7 @@ TEST(ISAM2, optimize2) { } /* ************************************************************************* */ -bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const GaussianISAM2<>& isam) { +bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const ISAM2& isam) { Values actual = isam.calculateEstimate(); Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first; GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering); @@ -212,7 +212,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); Values fullinit; planarSLAM::Graph fullgraph; @@ -300,7 +300,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) CHECK(isam_check(fullgraph, fullinit, isam)); // Check gradient at each node - typedef GaussianISAM2<>::sharedClique sharedClique; + typedef ISAM2::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph jfg; @@ -345,7 +345,7 @@ TEST(ISAM2, slamlike_solution_dogleg) SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2<> isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); + ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); Values fullinit; planarSLAM::Graph fullgraph; @@ -433,7 +433,7 @@ TEST(ISAM2, slamlike_solution_dogleg) CHECK(isam_check(fullgraph, fullinit, isam)); // Check gradient at each node - typedef GaussianISAM2<>::sharedClique sharedClique; + typedef ISAM2::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph jfg; @@ -473,7 +473,7 @@ TEST(ISAM2, clone) { SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)); + ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)); Values fullinit; planarSLAM::Graph fullgraph; @@ -558,8 +558,8 @@ TEST(ISAM2, clone) { } // CLONING... - boost::shared_ptr > isam2 - = boost::shared_ptr >(new GaussianISAM2<>()); + boost::shared_ptr isam2 + = boost::shared_ptr(new ISAM2()); isam.cloneTo(isam2); CHECK(assert_equal(isam, *isam2)); @@ -567,24 +567,23 @@ TEST(ISAM2, clone) { /* ************************************************************************* */ TEST(ISAM2, permute_cached) { - typedef ISAM2Clique Clique; - typedef boost::shared_ptr > sharedClique; + typedef boost::shared_ptr sharedISAM2Clique; // Construct expected permuted BayesTree (variable 2 has been changed to 1) - BayesTree expected; - expected.insert(sharedClique(new Clique(make_pair( + BayesTree expected; + expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( boost::make_shared(pair_list_of (3, Matrix_(1,1,1.0)) (4, Matrix_(1,1,2.0)), 2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4) HessianFactor::shared_ptr())))); // Cached: empty - expected.insert(sharedClique(new Clique(make_pair( + expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( boost::make_shared(pair_list_of (2, Matrix_(1,1,1.0)) (3, Matrix_(1,1,2.0)), 1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3) boost::make_shared(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3) - expected.insert(sharedClique(new Clique(make_pair( + expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( boost::make_shared(pair_list_of (0, Matrix_(1,1,1.0)) (2, Matrix_(1,1,2.0)), @@ -595,20 +594,20 @@ TEST(ISAM2, permute_cached) { expected.root()->children().front()->children().front()->conditional()->keys()[1] = 1; // Construct unpermuted BayesTree - BayesTree actual; - actual.insert(sharedClique(new Clique(make_pair( + BayesTree actual; + actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( boost::make_shared(pair_list_of (3, Matrix_(1,1,1.0)) (4, Matrix_(1,1,2.0)), 2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4) HessianFactor::shared_ptr())))); // Cached: empty - actual.insert(sharedClique(new Clique(make_pair( + actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( boost::make_shared(pair_list_of (2, Matrix_(1,1,1.0)) (3, Matrix_(1,1,2.0)), 1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3) boost::make_shared(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3) - actual.insert(sharedClique(new Clique(make_pair( + actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( boost::make_shared(pair_list_of (0, Matrix_(1,1,1.0)) (2, Matrix_(1,1,2.0)), @@ -646,7 +645,7 @@ TEST(ISAM2, removeFactors) SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); Values fullinit; planarSLAM::Graph fullgraph; @@ -740,7 +739,7 @@ TEST(ISAM2, removeFactors) CHECK(isam_check(fullgraph, fullinit, isam)); // Check gradient at each node - typedef GaussianISAM2<>::sharedClique sharedClique; + typedef ISAM2::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph jfg; @@ -785,7 +784,7 @@ TEST(ISAM2, constrained_ordering) SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); Values fullinit; planarSLAM::Graph fullgraph; @@ -883,7 +882,7 @@ TEST(ISAM2, constrained_ordering) (isam.getOrdering()[planarSLAM::PoseKey(3)] == 13 && isam.getOrdering()[planarSLAM::PoseKey(4)] == 12)); // Check gradient at each node - typedef GaussianISAM2<>::sharedClique sharedClique; + typedef ISAM2::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph jfg; From 4c2581f40e02436305855fa86150d1986d70d5a6 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sat, 17 Mar 2012 23:57:44 +0000 Subject: [PATCH 06/16] In progress --- .cproject | 2 +- .project | 2 +- gtsam/nonlinear/DoglegOptimizerImpl.h | 11 ++++++++--- gtsam/nonlinear/ISAM2-impl.cpp | 7 +++++++ gtsam/nonlinear/ISAM2-impl.h | 3 ++- gtsam/nonlinear/ISAM2-inl.h | 8 ++++++++ gtsam/nonlinear/ISAM2.cpp | 19 ++++++++++--------- gtsam/nonlinear/ISAM2.h | 8 ++++++++ gtsam/nonlinear/Values.h | 4 ++++ 9 files changed, 49 insertions(+), 15 deletions(-) diff --git a/.cproject b/.cproject index 6a32e5c08..d1d201f0c 100644 --- a/.cproject +++ b/.cproject @@ -72,7 +72,7 @@ - + diff --git a/.project b/.project index e52e979df..79f74bfbc 100644 --- a/.project +++ b/.project @@ -31,7 +31,7 @@ org.eclipse.cdt.make.core.buildLocation - ${ProjDirPath}/build + ${workspace_loc:/gtsam/build-timing} org.eclipse.cdt.make.core.cleanBuildTarget diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index 5026a155f..999147156 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -135,9 +135,14 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( const F& f, const VALUES& x0, const Ordering& ordering, const double f_error, const bool verbose) { // Compute steepest descent and Newton's method points - tic(0, "Steepest Descent"); - VectorValues dx_u = optimizeGradientSearch(Rd); - toc(0, "Steepest Descent"); + tic(0, "optimizeGradientSearch"); + tic(0, "allocateVectorValues"); + VectorValues dx_u = *allocateVectorValues(Rd); + toc(0, "allocateVectorValues"); + tic(1, "optimizeGradientSearchInPlace"); + optimizeGradientSearchInPlace(Rd, dx_u); + toc(1, "optimizeGradientSearchInPlace"); + toc(0, "optimizeGradientSearch"); tic(1, "optimize"); VectorValues dx_n = optimize(Rd); toc(1, "optimize"); diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index b6d018796..807111753 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -23,6 +23,7 @@ namespace gtsam { /* ************************************************************************* */ void ISAM2::Impl::AddVariables( const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, + Permuted& deltaNewton, Permuted& deltaGradSearch, Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); @@ -37,6 +38,12 @@ void ISAM2::Impl::AddVariables( delta.container().append(dims); delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); delta.permutation().resize(originalnVars + newTheta.size()); + deltaNewton.container().append(dims); + deltaNewton.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); + deltaNewton.permutation().resize(originalnVars + newTheta.size()); + deltaGradSearch.container().append(dims); + deltaGradSearch.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); + deltaGradSearch.permutation().resize(originalnVars + newTheta.size()); { Index nextVar = originalnVars; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index f49a385bc..be3f010e3 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -48,7 +48,8 @@ struct ISAM2::Impl { * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables * @param keyFormatter Formatter for printing nonlinear keys during debugging */ - static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, + static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, + Permuted& deltaNewton, Permuted& deltaGradSearch, vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 89f727f8b..04395069c 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -29,6 +29,14 @@ namespace gtsam { using namespace std; +/* ************************************************************************* */ +template +VALUE ISAM2::calculateEstimate(Key key) const { + const Index index = getOrdering()[key]; + const SubVector delta = getDelta()[index]; + return theta_.at(key).retract(delta); +} + /* ************************************************************************* */ namespace internal { template diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 58cc4b23c..f302e174e 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -593,14 +593,6 @@ Values ISAM2::calculateEstimate() const { return ret; } -/* ************************************************************************* */ -template -VALUE ISAM2::calculateEstimate(Key key) const { - const Index index = getOrdering()[key]; - const SubVector delta = getDelta()[index]; - return theta_.at(key).retract(delta); -} - /* ************************************************************************* */ Values ISAM2::calculateBestEstimate() const { VectorValues delta(theta_.dims(ordering_)); @@ -617,11 +609,20 @@ const Permuted& ISAM2::getDelta() const { /* ************************************************************************* */ VectorValues optimize(const ISAM2& isam) { + tic(0, "allocateVectorValues"); VectorValues delta = *allocateVectorValues(isam); - internal::optimizeInPlace(isam.root(), delta); + toc(0, "allocateVectorValues"); + optimizeInPlace(isam, delta); return delta; } +/* ************************************************************************* */ +void optimizeInPlace(const ISAM2& isam, VectorValues& delta) { + tic(1, "optimizeInPlace"); + internal::optimizeInPlace(isam.root(), delta); + toc(1, "optimizeInPlace"); +} + /* ************************************************************************* */ VectorValues optimizeGradientSearch(const ISAM2& isam) { tic(0, "Allocate VectorValues"); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 2d5b16d1b..535f21c75 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -292,6 +292,11 @@ protected: */ mutable Permuted delta_; + VectorValues deltaNewtonUnpermuted_; + mutable Permuted deltaNewtonUnpermuted_; + VectorValues deltaGradSearchUnpermuted_; + mutable Permuted deltaGradSearchUnpermuted_; + /** Indicates whether the current delta is up-to-date, only used * internally - delta will always be updated if necessary when it is * requested with getDelta() or calculateEstimate(). @@ -459,6 +464,9 @@ private: /** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ VectorValues optimize(const ISAM2& isam); +/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ +void optimizeInPlace(const ISAM2& isam, VectorValues& delta); + /// Optimize the BayesTree, starting from the root. /// @param replaced Needs to contain /// all variables that are contained in the top of the Bayes tree that has been diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index bdd1739da..5f6dad022 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -117,6 +117,8 @@ namespace gtsam { typedef boost::transform_iterator< boost::function1, KeyValueMap::const_reverse_iterator> const_reverse_iterator; + typedef KeyValuePair value_type; + private: template struct _KeyValuePair { @@ -143,6 +145,7 @@ namespace gtsam { /** A key-value pair, with the value a specific derived Value type. */ typedef _KeyValuePair KeyValuePair; typedef _ConstKeyValuePair ConstKeyValuePair; + typedef KeyValuePair value_type; typedef boost::transform_iterator< @@ -208,6 +211,7 @@ namespace gtsam { public: /** A const key-value pair, with the value a specific derived Value type. */ typedef _ConstKeyValuePair KeyValuePair; + typedef KeyValuePair value_type; typedef typename Filtered::const_const_iterator iterator; typedef typename Filtered::const_const_iterator const_iterator; From c695b23e36e5eca277de97f89930cd6d5afcfecc Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 18 Mar 2012 05:13:40 +0000 Subject: [PATCH 07/16] 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 From 670117cfe7a2e27134e793ce698fd54c77184f78 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 19 Mar 2012 14:32:37 +0000 Subject: [PATCH 08/16] Compiles and passes unit tests --- .project | 2 +- gtsam/linear/GaussianConditional.cpp | 12 ++--- gtsam/linear/VectorValues.cpp | 16 +++++++ gtsam/linear/VectorValues.h | 6 +++ gtsam/nonlinear/ISAM2-impl.cpp | 68 +++++++++++++++++++++------- gtsam/nonlinear/ISAM2-impl.h | 2 +- gtsam/nonlinear/ISAM2.cpp | 55 +++++++++++++--------- gtsam/nonlinear/ISAM2.h | 9 ++++ tests/testGaussianISAM2.cpp | 52 ++++++++++++++++++++- 9 files changed, 175 insertions(+), 47 deletions(-) diff --git a/.project b/.project index 79f74bfbc..e52e979df 100644 --- a/.project +++ b/.project @@ -31,7 +31,7 @@ org.eclipse.cdt.make.core.buildLocation - ${workspace_loc:/gtsam/build-timing} + ${ProjDirPath}/build org.eclipse.cdt.make.core.cleanBuildTarget diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 0d6c8fc73..2fca36b6a 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -194,7 +194,7 @@ inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES static const bool debug = false; if(debug) conditional.print("Solving conditional in place"); - Vector xS = extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents()); + Vector xS = internal::extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents()); xS = conditional.get_d() - conditional.get_S() * xS; Vector soln = conditional.permutation().transpose() * conditional.get_R().triangularView().solve(xS); @@ -202,7 +202,7 @@ inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES gtsam::print(Matrix(conditional.get_R()), "Calling backSubstituteUpper on "); gtsam::print(soln, "full back-substitution solution: "); } - writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals()); + internal::writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals()); } /* ************************************************************************* */ @@ -217,7 +217,7 @@ void GaussianConditional::solveInPlace(Permuted& x) const { /* ************************************************************************* */ void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { - Vector frontalVec = extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); + Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); // TODO: verify permutation frontalVec = permutation_ * gtsam::backSubstituteUpper(frontalVec,Matrix(get_R())); GaussianConditional::const_iterator it; @@ -225,14 +225,14 @@ void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { const Index i = *it; transposeMultiplyAdd(-1.0,get_S(it),frontalVec,gy[i]); } - writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); + internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); } /* ************************************************************************* */ void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const { - Vector frontalVec = extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); + Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); frontalVec = emul(frontalVec, get_sigmas()); - writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); + internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); } } diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 6c3ec797e..4edca2521 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -156,3 +156,19 @@ void VectorValues::operator+=(const VectorValues& c) { assert(this->hasSameStructure(c)); this->values_ += c.values_; } + +/* ************************************************************************* */ +VectorValues& VectorValues::operator=(const Permuted& rhs) { + if(this->size() != rhs.size()) + throw std::invalid_argument("VectorValues assignment from Permuted requires pre-allocation, see documentation."); + for(size_t j=0; jsize(); ++j) { + if(exists(j)) { + SubVector& l(this->at(j)); + const SubVector& r(rhs[j]); + if(l.rows() != r.rows()) + throw std::invalid_argument("VectorValues assignment from Permuted requires pre-allocation, see documentation."); + l = r; + } + } + return *this; +} diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 479710885..869fde4bb 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -273,6 +274,11 @@ namespace gtsam { */ void operator+=(const VectorValues& c); + /** Assignment operator from Permuted, requires the dimensions + * of the assignee to already be properly pre-allocated. + */ + VectorValues& operator=(const Permuted& rhs); + /// @} private: diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 13029493e..458bbfda4 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -22,8 +22,8 @@ namespace gtsam { /* ************************************************************************* */ void ISAM2::Impl::AddVariables( - const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, - Permuted& deltaNewton, Permuted& deltaGradSearch, + const Values& newTheta, Values& theta, Permuted& delta, + Permuted& deltaNewton, Permuted& deltaGradSearch, vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); @@ -48,6 +48,8 @@ void ISAM2::Impl::AddVariables( Index nextVar = originalnVars; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { delta.permutation()[nextVar] = nextVar; + deltaNewton.permutation()[nextVar] = nextVar; + deltaGradSearch.permutation()[nextVar] = nextVar; ordering.insert(key_value.key, nextVar); if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl; ++ nextVar; @@ -301,21 +303,47 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std: /* ************************************************************************* */ namespace internal { -size_t updateDoglegDeltas(const boost::shared_ptr& clique, std::vector& replacedKeys, - const VectorValues& grad, Permuted& deltaNewton, Permuted& RgProd, vector& updated) { +void updateDoglegDeltas(const boost::shared_ptr& clique, std::vector& replacedKeys, + const VectorValues& grad, Permuted& deltaNewton, Permuted& RgProd, size_t& varsUpdated) { + // Check if any frontal or separator keys were recalculated, if so, we need + // update deltas and recurse to children, but if not, we do not need to + // recurse further because of the running separator property. + bool anyReplaced = false; + BOOST_FOREACH(Index j, *clique->conditional()) { + if(replacedKeys[j]) { + anyReplaced = true; + break; + } + } + if(anyReplaced) { + // 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()); - // 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; - // 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)->beginFrontals(), (*clique)->endFrontals()); - // Write into RgProd vector - internal::writeVectorValuesSlices(RSgProd, RgProd, (*clique)->begin(), (*clique)->end()); + // Now solve the part of the Newton's method point for this clique (back-substitution) + (*clique)->solveInPlace(deltaNewton); + + // If debugging, set recalculated keys to false so we can check them later +#ifndef NDEBUG + BOOST_FOREACH(Index j, (*clique)->frontals()) { + replacedKeys[j] = false; } +#endif + + varsUpdated += (*clique)->nrFrontals(); + + // Recurse to children + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { + updateDoglegDeltas(child, replacedKeys, grad, deltaNewton, RgProd, varsUpdated); } + } } } @@ -323,15 +351,23 @@ size_t updateDoglegDeltas(const boost::shared_ptr& clique, std::vec 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); + size_t varsUpdated = 0; + internal::updateDoglegDeltas(isam.root(), replacedKeys, grad, deltaNewton, RgProd, varsUpdated); + + // Make sure all were updated +#ifndef NDEBUG + for(size_t j=0; j& 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); + Permuted& deltaNewton, Permuted& RgProd); }; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 6b66c1ce3..246e43211 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -38,7 +38,8 @@ static const double batchThreshold = 0.65; /* ************************************************************************* */ ISAM2::ISAM2(const ISAM2Params& params): - delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true), params_(params) { + delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_), + deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) { // See note in gtsam/base/boost_variant_with_workaround.h if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; @@ -46,7 +47,8 @@ ISAM2::ISAM2(const ISAM2Params& params): /* ************************************************************************* */ ISAM2::ISAM2(): - delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true) { + delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_), + deltaDoglegUptodate_(true), deltaUptodate_(true) { // See note in gtsam/base/boost_variant_with_workaround.h if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; @@ -344,6 +346,8 @@ boost::shared_ptr > ISAM2::recalculate( toc(3,"permute global variable index"); tic(4,"permute delta"); delta_.permute(partialSolveResult.fullReordering); + deltaNewton_.permute(partialSolveResult.fullReordering); + RgProd_.permute(partialSolveResult.fullReordering); toc(4,"permute delta"); tic(5,"permute ordering"); ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse); @@ -441,7 +445,7 @@ ISAM2Result ISAM2::update( tic(2,"add new variables"); // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. - Impl::AddVariables(newTheta, theta_, delta_, deltaReplacedMask_, ordering_, Base::nodes_); + Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_, Base::nodes_); toc(2,"add new variables"); tic(3,"evaluate error before"); @@ -543,6 +547,7 @@ ISAM2Result ISAM2::update( toc(10,"evaluate error after"); result.cliques = this->nodes().size(); + deltaDoglegUptodate_ = false; deltaUptodate_ = false; return result; @@ -575,9 +580,6 @@ void ISAM2::updateDelta(bool forceFullSolve) const { doglegDelta_ = doglegResult.Delta; delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution - - // Clear replaced mask - deltaReplacedMask_.assign(deltaReplacedMask_.size(), false); } deltaUptodate_ = true; @@ -618,9 +620,17 @@ VectorValues optimize(const ISAM2& isam) { /* ************************************************************************* */ void optimizeInPlace(const ISAM2& isam, VectorValues& delta) { - tic(1, "optimizeInPlace"); - internal::optimizeInPlace(isam.root(), delta); - toc(1, "optimizeInPlace"); + // We may need to update the solution calcaulations + if(!isam.deltaDoglegUptodate_) { + tic(1, "UpdateDoglegDeltas"); + ISAM2::Impl::UpdateDoglegDeltas(isam, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_); + isam.deltaDoglegUptodate_ = true; + toc(1, "UpdateDoglegDeltas"); + } + + tic(2, "copy delta"); + delta = isam.deltaNewton_; + toc(2, "copy delta"); } /* ************************************************************************* */ @@ -635,27 +645,30 @@ VectorValues optimizeGradientSearch(const ISAM2& isam) { } /* ************************************************************************* */ -void optimizeGradientSearchInPlace(const ISAM2& Rd, VectorValues& grad) { +void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) { + // We may need to update the solution calcaulations + if(!isam.deltaDoglegUptodate_) { + tic(1, "UpdateDoglegDeltas"); + ISAM2::Impl::UpdateDoglegDeltas(isam, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_); + isam.deltaDoglegUptodate_ = true; + toc(1, "UpdateDoglegDeltas"); + } + tic(1, "Compute Gradient"); // Compute gradient (call gradientAtZero function, which is defined for various linear systems) - gradientAtZero(Rd, grad); + gradientAtZero(isam, grad); double gradientSqNorm = grad.dot(grad); toc(1, "Compute Gradient"); - tic(2, "Compute R*g"); - // Compute R * g - FactorGraph Rd_jfg(Rd); - Errors Rg = Rd_jfg * grad; - toc(2, "Compute R*g"); - - tic(3, "Compute minimizing step size"); + tic(2, "Compute minimizing step size"); // Compute minimizing step size - double step = -gradientSqNorm / dot(Rg, Rg); - toc(3, "Compute minimizing step size"); + double RgNormSq = isam.RgProd_.container().vector().squaredNorm(); + double step = -gradientSqNorm / RgNormSq; + toc(2, "Compute minimizing step size"); tic(4, "Compute point"); // Compute steepest descent point - scal(step, grad); + grad.vector() *= step; toc(4, "Compute point"); } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 8d5c49268..6a504009c 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -296,6 +296,7 @@ protected: mutable Permuted deltaNewton_; VectorValues RgProdUnpermuted_; mutable Permuted RgProd_; + mutable bool deltaDoglegUptodate_; /** Indicates whether the current delta is up-to-date, only used * internally - delta will always be updated if necessary when it is @@ -359,6 +360,11 @@ public: newISAM2->variableIndex_ = variableIndex_; newISAM2->deltaUnpermuted_ = deltaUnpermuted_; newISAM2->delta_ = delta_; + newISAM2->deltaNewtonUnpermuted_ = deltaNewtonUnpermuted_; + newISAM2->deltaNewton_ = deltaNewton_; + newISAM2->RgProdUnpermuted_ = RgProdUnpermuted_; + newISAM2->RgProd_ = RgProd_; + newISAM2->deltaDoglegUptodate_ = deltaDoglegUptodate_; newISAM2->deltaUptodate_ = deltaUptodate_; newISAM2->deltaReplacedMask_ = deltaReplacedMask_; newISAM2->nonlinearFactors_ = nonlinearFactors_; @@ -459,6 +465,9 @@ private: // void linear_update(const GaussianFactorGraph& newFactors); void updateDelta(bool forceFullSolve = false) const; + friend void optimizeInPlace(const ISAM2&, VectorValues&); + friend void optimizeGradientSearchInPlace(const ISAM2&, VectorValues&); + }; // ISAM2 /** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 831d17478..a990d2ded 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -29,7 +29,7 @@ using boost::shared_ptr; const double tol = 1e-4; /* ************************************************************************* */ -TEST(ISAM2, AddVariables) { +TEST_UNSAFE(ISAM2, AddVariables) { // Create initial state Values theta; @@ -48,6 +48,26 @@ TEST(ISAM2, AddVariables) { Permuted delta(permutation, deltaUnpermuted); + VectorValues deltaNewtonUnpermuted; + deltaNewtonUnpermuted.insert(0, Vector_(3, .1, .2, .3)); + deltaNewtonUnpermuted.insert(1, Vector_(2, .4, .5)); + + Permutation permutationNewton(2); + permutationNewton[0] = 1; + permutationNewton[1] = 0; + + Permuted deltaNewton(permutationNewton, deltaNewtonUnpermuted); + + VectorValues deltaRgUnpermuted; + deltaRgUnpermuted.insert(0, Vector_(3, .1, .2, .3)); + deltaRgUnpermuted.insert(1, Vector_(2, .4, .5)); + + Permutation permutationRg(2); + permutationRg[0] = 1; + permutationRg[1] = 0; + + Permuted deltaRg(permutationRg, deltaRgUnpermuted); + vector replacedKeys(2, false); Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0); @@ -78,6 +98,30 @@ TEST(ISAM2, AddVariables) { Permuted deltaExpected(permutationExpected, deltaUnpermutedExpected); + VectorValues deltaNewtonUnpermutedExpected; + deltaNewtonUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3)); + deltaNewtonUnpermutedExpected.insert(1, Vector_(2, .4, .5)); + deltaNewtonUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); + + Permutation permutationNewtonExpected(3); + permutationNewtonExpected[0] = 1; + permutationNewtonExpected[1] = 0; + permutationNewtonExpected[2] = 2; + + Permuted deltaNewtonExpected(permutationNewtonExpected, deltaNewtonUnpermutedExpected); + + VectorValues deltaRgUnpermutedExpected; + deltaRgUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3)); + deltaRgUnpermutedExpected.insert(1, Vector_(2, .4, .5)); + deltaRgUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); + + Permutation permutationRgExpected(3); + permutationRgExpected[0] = 1; + permutationRgExpected[1] = 0; + permutationRgExpected[2] = 2; + + Permuted deltaRgExpected(permutationRgExpected, deltaRgUnpermutedExpected); + vector replacedKeysExpected(3, false); Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1); @@ -86,11 +130,15 @@ TEST(ISAM2, AddVariables) { 3, ISAM2::sharedClique()); // Expand initial state - ISAM2::Impl::AddVariables(newTheta, theta, delta, replacedKeys, ordering, nodes); + ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering, nodes); EXPECT(assert_equal(thetaExpected, theta)); EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted)); EXPECT(assert_equal(deltaExpected.permutation(), delta.permutation())); + EXPECT(assert_equal(deltaNewtonUnpermutedExpected, deltaNewtonUnpermuted)); + EXPECT(assert_equal(deltaNewtonExpected.permutation(), deltaNewton.permutation())); + EXPECT(assert_equal(deltaRgUnpermutedExpected, deltaRgUnpermuted)); + EXPECT(assert_equal(deltaRgExpected.permutation(), deltaRg.permutation())); EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys)); EXPECT(assert_equal(orderingExpected, ordering)); } From 8a4476f63d80739b9bcc11b3c2d6d93cefa2cd30 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 19 Mar 2012 16:24:47 +0000 Subject: [PATCH 09/16] Made constructor explicit --- gtsam/linear/VectorValues.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 869fde4bb..7939b486d 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -178,7 +178,7 @@ namespace gtsam { /** Construct from a container of variable dimensions (in variable order), without initializing any values. */ template - VectorValues(const CONTAINER& dimensions) { append(dimensions); } + explicit VectorValues(const CONTAINER& dimensions) { append(dimensions); } /** Construct to hold nVars vectors of varDim dimension each. */ VectorValues(Index nVars, size_t varDim) { resize(nVars, varDim); } From 13bbaaa2f43aad84b3ed4cd4ece6b8206e021996 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 19 Mar 2012 16:24:54 +0000 Subject: [PATCH 10/16] Added extra check --- gtsam/linear/VectorValues.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 4edca2521..96bafcd16 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -168,6 +168,9 @@ VectorValues& VectorValues::operator=(const Permuted& rhs) { if(l.rows() != r.rows()) throw std::invalid_argument("VectorValues assignment from Permuted requires pre-allocation, see documentation."); l = r; + } else { + if(rhs.container().exists(rhs.permutation()[j])) + throw std::invalid_argument("VectorValues assignment from Permuted requires pre-allocation, see documentation."); } } return *this; From 6356647665bde015a001931036be4e27a02122f1 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 19 Mar 2012 16:25:03 +0000 Subject: [PATCH 11/16] Bug fixes and using wildfire with Dogleg --- .cproject | 331 +++++++++++++++++---------------- .project | 2 +- gtsam/nonlinear/ISAM2-impl.cpp | 31 +-- gtsam/nonlinear/ISAM2-impl.h | 2 +- gtsam/nonlinear/ISAM2.cpp | 20 +- gtsam/nonlinear/ISAM2.h | 11 +- 6 files changed, 211 insertions(+), 186 deletions(-) diff --git a/.cproject b/.cproject index d1d201f0c..b8e21aead 100644 --- a/.cproject +++ b/.cproject @@ -21,7 +21,7 @@ - + @@ -311,6 +311,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -337,7 +345,6 @@ make - tests/testBayesTree.run true false @@ -345,7 +352,6 @@ make - testBinaryBayesNet.run true false @@ -393,7 +399,6 @@ make - testSymbolicBayesNet.run true false @@ -401,7 +406,6 @@ make - tests/testSymbolicFactor.run true false @@ -409,7 +413,6 @@ make - testSymbolicFactorGraph.run true false @@ -425,20 +428,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -465,6 +459,7 @@ make + testGraph.run true false @@ -536,6 +531,7 @@ make + testInference.run true false @@ -543,6 +539,7 @@ make + testGaussianFactor.run true false @@ -550,6 +547,7 @@ make + testJunctionTree.run true false @@ -557,6 +555,7 @@ make + testSymbolicBayesNet.run true false @@ -564,6 +563,7 @@ make + testSymbolicFactorGraph.run true false @@ -633,22 +633,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -665,6 +649,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -689,26 +689,34 @@ true true - + make - -j2 - all + -j2 VERBOSE=1 + check.nonlinear + true + false + true + + + make + -j5 + timing.nonlinear true true true - + make - -j2 - check + -j5 + nonlinear.testValues.run true true true - + make - -j2 - clean + -j5 + nonlinear.testOrdering.run true true true @@ -761,34 +769,26 @@ true true - + make - -j2 VERBOSE=1 - check.nonlinear - true - false - true - - - make - -j5 - timing.nonlinear + -j2 + all true true true - + make - -j5 - nonlinear.testValues.run + -j2 + check true true true - + make - -j5 - nonlinear.testOrdering.run + -j2 + clean true true true @@ -1139,7 +1139,6 @@ make - testErrors.run true false @@ -1619,6 +1618,7 @@ make + testSimulated2DOriented.run true false @@ -1658,6 +1658,7 @@ make + testSimulated2D.run true false @@ -1665,6 +1666,7 @@ make + testSimulated3D.run true false @@ -1936,6 +1938,7 @@ make + tests/testGaussianISAM2 true false @@ -1957,93 +1960,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - check - true - true - true - - - make - -j5 - all - true - true - true - - - cmake - .. - true - false - true - - - make - -j5 - gtsam-shared - true - true - true - - - make - -j5 - gtsam-static - true - true - true - - - make - -j5 - timing - true - true - true - - - make - -j5 - examples - true - true - true - - - make - -j5 - VERBOSE=1 all - true - true - true - - - make - -j5 - VERBOSE=1 check - true - true - true - make -j2 @@ -2140,23 +2056,7 @@ true true - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - + make -j2 install @@ -2164,21 +2064,85 @@ true true - + make -j2 + clean + true + true + true + + + make + -j5 + check + true + true + true + + + make + -j5 all true true true - + cmake .. true false true + + make + -j5 + gtsam-shared + true + true + true + + + make + -j5 + gtsam-static + true + true + true + + + make + -j5 + timing + true + true + true + + + make + -j5 + examples + true + true + true + + + make + -j5 + VERBOSE=1 all + true + true + true + + + make + -j5 + VERBOSE=1 check + true + true + true + make -j5 @@ -2219,6 +2183,45 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + true + false + true + diff --git a/.project b/.project index e52e979df..f59d79cb7 100644 --- a/.project +++ b/.project @@ -23,7 +23,7 @@ org.eclipse.cdt.make.core.buildArguments - -j8 + -j7 org.eclipse.cdt.make.core.buildCommand diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 458bbfda4..6df7e8450 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -330,13 +330,7 @@ void updateDoglegDeltas(const boost::shared_ptr& clique, std::vecto internal::writeVectorValuesSlices(RSgProd, RgProd, (*clique)->beginFrontals(), (*clique)->endFrontals()); // Now solve the part of the Newton's method point for this clique (back-substitution) - (*clique)->solveInPlace(deltaNewton); - - // If debugging, set recalculated keys to false so we can check them later -#ifndef NDEBUG - BOOST_FOREACH(Index j, (*clique)->frontals()) { - replacedKeys[j] = false; } -#endif + //(*clique)->solveInPlace(deltaNewton); varsUpdated += (*clique)->nrFrontals(); @@ -348,7 +342,7 @@ void updateDoglegDeltas(const boost::shared_ptr& clique, std::vecto } /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, std::vector& replacedKeys, +size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector& replacedKeys, Permuted& deltaNewton, Permuted& RgProd) { // Get gradient @@ -358,15 +352,24 @@ size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, std::vector& rep // Update variables size_t varsUpdated = 0; internal::updateDoglegDeltas(isam.root(), replacedKeys, grad, deltaNewton, RgProd, varsUpdated); + optimizeWildfire(isam.root(), wildfireThreshold, replacedKeys, deltaNewton); - // Make sure all were updated -#ifndef NDEBUG - for(size_t j=0; j(isam.root(), expected); + for(size_t j = 0; j Rd_jfg(isam); + Errors Rg = Rd_jfg * grad; + double RgMagExpected = dot(Rg, Rg); + double RgMagActual = RgProd.container().vector().squaredNorm(); + cout << fabs(RgMagExpected - RgMagActual) << endl; + assert(fabs(RgMagExpected - RgMagActual) < (1e-8 * RgMagActual + 1e-4)); #endif + replacedKeys.assign(replacedKeys.size(), false); + return varsUpdated; } diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 6a956af98..351a371df 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -126,7 +126,7 @@ 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, + static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector& replacedKeys, Permuted& deltaNewton, Permuted& RgProd); }; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 246e43211..b9a95b51a 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -242,6 +242,8 @@ boost::shared_ptr > ISAM2::recalculate( toc(2,"permute global variable index"); tic(3,"permute delta"); delta_.permute(*colamd); + deltaNewton_.permute(*colamd); + RgProd_.permute(*colamd); toc(3,"permute delta"); tic(4,"permute ordering"); ordering_.permuteWithInverse(*colamdInverse); @@ -623,7 +625,14 @@ void optimizeInPlace(const ISAM2& isam, VectorValues& delta) { // We may need to update the solution calcaulations if(!isam.deltaDoglegUptodate_) { tic(1, "UpdateDoglegDeltas"); - ISAM2::Impl::UpdateDoglegDeltas(isam, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_); + double wildfireThreshold = 0.0; + if(isam.params().optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) + wildfireThreshold = boost::get(isam.params().optimizationParams).wildfireThreshold; + else if(isam.params().optimizationParams.type() == typeid(ISAM2DoglegParams)) + wildfireThreshold = boost::get(isam.params().optimizationParams).wildfireThreshold; + else + assert(false); + ISAM2::Impl::UpdateDoglegDeltas(isam, wildfireThreshold, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_); isam.deltaDoglegUptodate_ = true; toc(1, "UpdateDoglegDeltas"); } @@ -649,7 +658,14 @@ void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) { // We may need to update the solution calcaulations if(!isam.deltaDoglegUptodate_) { tic(1, "UpdateDoglegDeltas"); - ISAM2::Impl::UpdateDoglegDeltas(isam, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_); + double wildfireThreshold = 0.0; + if(isam.params().optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) + wildfireThreshold = boost::get(isam.params().optimizationParams).wildfireThreshold; + else if(isam.params().optimizationParams.type() == typeid(ISAM2DoglegParams)) + wildfireThreshold = boost::get(isam.params().optimizationParams).wildfireThreshold; + else + assert(false); + ISAM2::Impl::UpdateDoglegDeltas(isam, wildfireThreshold, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_); isam.deltaDoglegUptodate_ = true; toc(1, "UpdateDoglegDeltas"); } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 6a504009c..ca688ecc0 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -54,15 +54,18 @@ struct ISAM2GaussNewtonParams { */ struct ISAM2DoglegParams { double initialDelta; ///< The initial trust region radius for Dogleg + double wildfireThreshold; ///< Continue updating the linear delta only when changes are above this threshold (default: 0.001) DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationMode; ///< See description in DoglegOptimizerImpl::TrustRegionAdaptationMode bool verbose; ///< Whether Dogleg prints iteration and convergence information /** Specify parameters as constructor arguments */ ISAM2DoglegParams( - double _initialDelta = 1.0, ///< see ISAM2DoglegParams public variables, ISAM2DoglegParams::initialDelta - DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = DoglegOptimizerImpl::SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams public variables, ISAM2DoglegParams::adaptationMode - bool _verbose = false ///< see ISAM2DoglegParams public variables, ISAM2DoglegParams::verbose - ) : initialDelta(_initialDelta), adaptationMode(_adaptationMode), verbose(_verbose) {} + double _initialDelta = 1.0, ///< see ISAM2DoglegParams::initialDelta + double _wildfireThreshold = 0.0, ///< see ISAM2DoglegParams::wildfireThreshold + DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = DoglegOptimizerImpl::SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams::adaptationMode + bool _verbose = false ///< see ISAM2DoglegParams::verbose + ) : initialDelta(_initialDelta), wildfireThreshold(_wildfireThreshold), + adaptationMode(_adaptationMode), verbose(_verbose) {} }; /** From a558ad042e0ddab5606d40a66aa2e1c3dbb38165 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 19 Mar 2012 16:55:45 +0000 Subject: [PATCH 12/16] Changed default wildfireThreshold for Dogleg from 0.0 to 1e-5 --- gtsam/nonlinear/ISAM2.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index ca688ecc0..b967eadc4 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -61,7 +61,7 @@ struct ISAM2DoglegParams { /** Specify parameters as constructor arguments */ ISAM2DoglegParams( double _initialDelta = 1.0, ///< see ISAM2DoglegParams::initialDelta - double _wildfireThreshold = 0.0, ///< see ISAM2DoglegParams::wildfireThreshold + double _wildfireThreshold = 1e-5, ///< see ISAM2DoglegParams::wildfireThreshold DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = DoglegOptimizerImpl::SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams::adaptationMode bool _verbose = false ///< see ISAM2DoglegParams::verbose ) : initialDelta(_initialDelta), wildfireThreshold(_wildfireThreshold), From b1d455278163b5178783deebbf17ec302772831f Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 19 Mar 2012 16:55:52 +0000 Subject: [PATCH 13/16] Timing statements --- gtsam/nonlinear/DoglegOptimizerImpl.h | 9 +-------- gtsam/nonlinear/ISAM2.cpp | 9 ++++++++- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index 999147156..ed74066d2 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -191,7 +191,6 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( if(verbose) cout << "rho = " << rho << endl; if(rho >= 0.75) { - tic(7, "Rho >= 0.75"); // M agrees very well with f, so try to increase lambda const double dx_d_norm = result.dx_d.vector().norm(); const double newDelta = std::max(Delta, 3.0 * dx_d_norm); // Compute new Delta @@ -209,14 +208,12 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( assert(false); } Delta = newDelta; // Update Delta from new Delta - toc(7, "Rho >= 0.75"); } else if(0.75 > rho && rho >= 0.25) { // M agrees so-so with f, keep the same Delta stay = false; } else if(0.25 > rho && rho >= 0.0) { - tic(8, "0.25 > Rho >= 0.75"); // M does not agree well with f, decrease Delta until it does double newDelta; if(Delta > 1e-5) @@ -232,11 +229,8 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( assert(false); } Delta = newDelta; // Update Delta from new Delta - toc(8, "0.25 > Rho >= 0.75"); - } - else { - tic(9, "Rho < 0"); + } else { // f actually increased, so keep decreasing Delta until f does not decrease assert(0.0 > rho); if(Delta > 1e-5) { @@ -247,7 +241,6 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( if(verbose) cout << "Warning: Dog leg stopping because cannot decrease error with minimum Delta" << endl; stay = false; } - toc(9, "Rho < 0"); } } diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index b9a95b51a..eb8ee9898 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -591,9 +591,16 @@ void ISAM2::updateDelta(bool forceFullSolve) const { Values ISAM2::calculateEstimate() const { // We use ExpmapMasked here instead of regular expmap because the former // handles Permuted + tic(1, "Copy Values"); Values ret(theta_); + toc(1, "Copy Values"); + tic(2, "getDelta"); + const Permuted& delta(getDelta()); + toc(2, "getDelta"); + tic(3, "Expmap"); vector mask(ordering_.nVars(), true); - Impl::ExpmapMasked(ret, getDelta(), ordering_, mask); + Impl::ExpmapMasked(ret, delta, ordering_, mask); + toc(3, "Expmap"); return ret; } From 5862943a8a479806ed505cfd55766e3333e405a3 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 22 Mar 2012 06:18:38 +0000 Subject: [PATCH 14/16] Timing statements and avoiding recalculating dimensions --- gtsam/nonlinear/DoglegOptimizerImpl.h | 9 ++++++--- gtsam/nonlinear/ISAM2.cpp | 14 ++++++++------ 2 files changed, 14 insertions(+), 9 deletions(-) diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index ed74066d2..81db26384 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -143,9 +143,10 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( optimizeGradientSearchInPlace(Rd, dx_u); toc(1, "optimizeGradientSearchInPlace"); toc(0, "optimizeGradientSearch"); - tic(1, "optimize"); - VectorValues dx_n = optimize(Rd); - toc(1, "optimize"); + tic(1, "optimizeInPlace"); + VectorValues dx_n(VectorValues::SameStructure(dx_u); + optimizeInPlace(Rd, *dx_n); + toc(1, "optimizeInPlace"); tic(2, "jfg error"); const GaussianFactorGraph jfg(Rd); const double M_error = jfg.error(VectorValues::Zero(dx_u)); @@ -182,6 +183,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( if(verbose) cout << "f error: " << f_error << " -> " << result.f_error << endl; if(verbose) cout << "M error: " << M_error << " -> " << new_M_error << endl; + tic(7, "adjust Delta"); // Compute gain ratio. Here we take advantage of the invariant that the // Bayes' net error at zero is equal to the nonlinear error const double rho = fabs(M_error - new_M_error) < 1e-15 ? @@ -242,6 +244,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( stay = false; } } + toc(7, "adjust Delta"); } // dx_d and f_error have already been filled in during the loop diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index eb8ee9898..58af039dd 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -574,14 +574,16 @@ void ISAM2::updateDelta(bool forceFullSolve) const { // Do one Dogleg iteration tic(1, "Dogleg Iterate"); - DoglegOptimizerImpl::IterationResult doglegResult = DoglegOptimizerImpl::Iterate( - *doglegDelta_, doglegParams.adaptationMode, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose); + DoglegOptimizerImpl::IterationResult doglegResult(DoglegOptimizerImpl::Iterate( + *doglegDelta_, doglegParams.adaptationMode, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose)); toc(1, "Dogleg Iterate"); + tic(2, "Copy dx_d"); // Update Delta and linear step doglegDelta_ = doglegResult.Delta; delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution + toc(2, "Copy dx_d"); } deltaUptodate_ = true; @@ -677,17 +679,17 @@ void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) { toc(1, "UpdateDoglegDeltas"); } - tic(1, "Compute Gradient"); + tic(2, "Compute Gradient"); // Compute gradient (call gradientAtZero function, which is defined for various linear systems) gradientAtZero(isam, grad); double gradientSqNorm = grad.dot(grad); - toc(1, "Compute Gradient"); + toc(2, "Compute Gradient"); - tic(2, "Compute minimizing step size"); + tic(3, "Compute minimizing step size"); // Compute minimizing step size double RgNormSq = isam.RgProd_.container().vector().squaredNorm(); double step = -gradientSqNorm / RgNormSq; - toc(2, "Compute minimizing step size"); + toc(3, "Compute minimizing step size"); tic(4, "Compute point"); // Compute steepest descent point From d577655f1b0ec406b75205ca069526b929aed95e Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 23 Mar 2012 04:05:51 +0000 Subject: [PATCH 16/16] Removed problematic 'inline' --- gtsam/linear/JacobianFactor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 0218ecc8d..99fd078cd 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -42,20 +42,20 @@ using namespace boost::lambda; namespace gtsam { /* ************************************************************************* */ - inline void JacobianFactor::assertInvariants() const { - #ifndef NDEBUG + void JacobianFactor::assertInvariants() const { +#ifndef NDEBUG GaussianFactor::assertInvariants(); // The base class checks for unique keys assert((size() == 0 && Ab_.rows() == 0 && Ab_.nBlocks() == 0) || size()+1 == Ab_.nBlocks()); assert(firstNonzeroBlocks_.size() == Ab_.rows()); for(size_t i=0; i