diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 4308d02e1..41ee52b3a 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -271,7 +271,8 @@ inline static void optimizeInPlace(const boost::shared_ptr& clique, } /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateDelta(const FastVector& roots, FastSet& replacedKeys, VectorValues& delta, double wildfireThreshold) { +size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector& roots, + const FastSet& replacedKeys, VectorValues& delta, double wildfireThreshold) { size_t lastBacksubVariableCount; @@ -294,15 +295,12 @@ size_t ISAM2::Impl::UpdateDelta(const FastVector& roots, Fa #endif } - // Clear replacedKeys - replacedKeys.clear(); - return lastBacksubVariableCount; } /* ************************************************************************* */ namespace internal { -void updateDoglegDeltas(const boost::shared_ptr& clique, const FastSet& replacedKeys, +void updateRgProd(const boost::shared_ptr& clique, const FastSet& replacedKeys, const VectorValues& grad, VectorValues& RgProd, size_t& varsUpdated) { // Check if any frontal or separator keys were recalculated, if so, we need @@ -340,30 +338,37 @@ void updateDoglegDeltas(const boost::shared_ptr& clique, const Fast // Recurse to children BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { - updateDoglegDeltas(child, replacedKeys, grad, RgProd, varsUpdated); } + updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated); } } } } /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, FastSet& replacedKeys, - VectorValues& deltaNewton, VectorValues& RgProd) { - - // Get gradient - VectorValues grad; - gradientAtZero(isam, grad); +size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const FastSet& replacedKeys, + const VectorValues& gradAtZero, VectorValues& RgProd) { // Update variables size_t varsUpdated = 0; - BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()) - { - internal::updateDoglegDeltas(root, replacedKeys, grad, RgProd, varsUpdated); - optimizeWildfireNonRecursive(root, wildfireThreshold, replacedKeys, deltaNewton); + BOOST_FOREACH(const ISAM2::sharedClique& root, roots) { + internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, varsUpdated); } - replacedKeys.clear(); - return varsUpdated; } + +/* ************************************************************************* */ +VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero, + const VectorValues& RgProd) +{ + // Compute gradient squared-magnitude + const double gradientSqNorm = gradAtZero.dot(gradAtZero); + + // Compute minimizing step size + double RgNormSq = RgProd.vector().squaredNorm(); + double step = -gradientSqNorm / RgNormSq; + + // Compute steepest descent point + return step * gradAtZero; +} } diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index d76201f7f..a8d03df13 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -123,11 +123,24 @@ struct GTSAM_EXPORT ISAM2::Impl { boost::optional invalidateIfDebug = boost::none, const KeyFormatter& keyFormatter = DefaultKeyFormatter); - static size_t UpdateDelta(const FastVector& roots, - FastSet& replacedKeys, VectorValues& delta, double wildfireThreshold); + /** + * Update the Newton's method step point, using wildfire + */ + static size_t UpdateGaussNewtonDelta(const FastVector& roots, + const FastSet& replacedKeys, VectorValues& delta, double wildfireThreshold); - static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, FastSet& replacedKeys, - VectorValues& deltaNewton, VectorValues& RgProd); + /** + * Update the RgProd (R*g) incrementally taking into account which variables + * have been recalculated in \c replacedKeys. Only used in Dogleg. + */ + static size_t UpdateRgProd(const ISAM2::Roots& roots, const FastSet& replacedKeys, + const VectorValues& gradAtZero, VectorValues& RgProd); + + /** + * Compute the gradient-search point. Only used in Dogleg. + */ + static VectorValues ComputeGradientSearch(const VectorValues& gradAtZero, + const VectorValues& RgProd); }; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index f57e02ca8..2c2b4b719 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -151,15 +151,13 @@ void ISAM2Clique::print(const std::string& s, const KeyFormatter& formatter) con } /* ************************************************************************* */ -ISAM2::ISAM2(const ISAM2Params& params): - deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) { +ISAM2::ISAM2(const ISAM2Params& params): params_(params) { if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; } /* ************************************************************************* */ -ISAM2::ISAM2(): - deltaDoglegUptodate_(true), deltaUptodate_(true) { +ISAM2::ISAM2() { if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; } @@ -745,8 +743,6 @@ ISAM2Result ISAM2::update( gttoc(remove_variables); } result.cliques = this->nodes().size(); - deltaDoglegUptodate_ = false; - deltaUptodate_ = false; gttic(evaluate_error_after); if(params_.evaluateNonlinearError) @@ -975,20 +971,36 @@ void ISAM2::updateDelta(bool forceFullSolve) const boost::get(params_.optimizationParams); const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; gttic(Wildfire_update); - lastBacksubVariableCount = Impl::UpdateDelta(roots_, deltaReplacedMask_, delta_, effectiveWildfireThreshold); + lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta( + roots_, deltaReplacedMask_, delta_, effectiveWildfireThreshold); + deltaReplacedMask_.clear(); gttoc(Wildfire_update); } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { // If using Dogleg, do a Dogleg step const ISAM2DoglegParams& doglegParams = boost::get(params_.optimizationParams); + const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : doglegParams.wildfireThreshold; // Do one Dogleg iteration gttic(Dogleg_Iterate); - VectorValues dx_u = gtsam::optimizeGradientSearch(*this); - VectorValues dx_n = gtsam::optimize(*this); + + // Compute Newton's method step + gttic(Wildfire_update); + lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta(roots_, deltaReplacedMask_, deltaNewton_, effectiveWildfireThreshold); + gttoc(Wildfire_update); + + // Compute steepest descent step + const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient + Impl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, RgProd_); // Update RgProd + const VectorValues dx_u = Impl::ComputeGradientSearch(gradAtZero, RgProd_); // Compute gradient search point + + // Clear replaced keys mask because now we've updated deltaNewton_ and RgProd_ + deltaReplacedMask_.clear(); + + // Compute dogleg point DoglegOptimizerImpl::IterationResult doglegResult(DoglegOptimizerImpl::Iterate( - *doglegDelta_, doglegParams.adaptationMode, dx_u, dx_n, *this, nonlinearFactors_, + *doglegDelta_, doglegParams.adaptationMode, dx_u, deltaNewton_, *this, nonlinearFactors_, theta_, nonlinearFactors_.error(theta_), doglegParams.verbose)); gttoc(Dogleg_Iterate); @@ -998,21 +1010,15 @@ void ISAM2::updateDelta(bool forceFullSolve) const delta_ = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution gttoc(Copy_dx_d); } - - deltaUptodate_ = true; } /* ************************************************************************* */ Values ISAM2::calculateEstimate() const { gttic(ISAM2_calculateEstimate); - gttic(Copy_Values); - Values ret(theta_); - gttoc(Copy_Values); const VectorValues& delta(getDelta()); gttic(Expmap); - ret = ret.retract(delta); + return theta_.retract(delta); gttoc(Expmap); - return ret; } /* ************************************************************************* */ @@ -1023,7 +1029,8 @@ const Value& ISAM2::calculateEstimate(Key key) const { /* ************************************************************************* */ Values ISAM2::calculateBestEstimate() const { - return theta_.retract(internal::linearAlgorithms::optimizeBayesTree(*this)); + updateDelta(true); // Force full solve when updating delta_ + return theta_.retract(delta_); } /* ************************************************************************* */ @@ -1033,7 +1040,7 @@ Matrix ISAM2::marginalCovariance(Key key) const { /* ************************************************************************* */ const VectorValues& ISAM2::getDelta() const { - if(!deltaUptodate_) + if(!deltaReplacedMask_.empty()) updateDelta(); return delta_; } @@ -1044,88 +1051,6 @@ double ISAM2::error(const VectorValues& x) const return GaussianFactorGraph(*this).error(x); } -/* ************************************************************************* */ -VectorValues optimize(const ISAM2& isam) { - VectorValues delta; - optimizeInPlace(isam, delta); - return delta; -} - -/* ************************************************************************* */ -void optimizeInPlace(const ISAM2& isam, VectorValues& delta) -{ - gttic(ISAM2_optimizeInPlace); - // We may need to update the solution calculations - if(!isam.deltaDoglegUptodate_) { - gttic(UpdateDoglegDeltas); - 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; - gttoc(UpdateDoglegDeltas); - } - - gttic(copy_delta); - delta = isam.deltaNewton_; - gttoc(copy_delta); -} - -/* ************************************************************************* */ - -VectorValues optimizeGradientSearch(const ISAM2& isam) { - VectorValues grad; - optimizeGradientSearchInPlace(isam, grad); - return grad; -} - -/* ************************************************************************* */ -void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) -{ - gttic(ISAM2_optimizeGradientSearchInPlace); - // We may need to update the solution calcaulations - if(!isam.deltaDoglegUptodate_) { - gttic(UpdateDoglegDeltas); - 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; - gttoc(UpdateDoglegDeltas); - } - - gttic(Compute_Gradient); - // Compute gradient (call gradientAtZero function, which is defined for various linear systems) - gradientAtZero(isam, grad); - double gradientSqNorm = grad.dot(grad); - gttoc(Compute_Gradient); - - gttic(Compute_minimizing_step_size); - // Compute minimizing step size - double RgNormSq = isam.RgProd_.vector().squaredNorm(); - double step = -gradientSqNorm / RgNormSq; - gttoc(Compute_minimizing_step_size); - - gttic(Compute_point); - // Compute steepest descent point - grad *= step; - gttoc(Compute_point); -} - -/* ************************************************************************* */ - -VectorValues gradient(const ISAM2& bayesTree, const VectorValues& x0) { - return GaussianFactorGraph(bayesTree).gradient(x0); -} - /* ************************************************************************* */ static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, VectorValues& g) { // Loop through variables in each clique, adding contributions @@ -1147,13 +1072,16 @@ static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, } /* ************************************************************************* */ -void gradientAtZero(const ISAM2& bayesTree, VectorValues& g) { - // Zero-out gradient - g.setZero(); - +VectorValues ISAM2::gradientAtZero() const +{ + // Create result + VectorValues g; + // Sum up contributions for each clique - BOOST_FOREACH(const ISAM2::sharedClique& root, bayesTree.roots()) - gradientAtZeroTreeAdder(root, g); + BOOST_FOREACH(const ISAM2::sharedClique& root, this->roots()) + gradientAtZeroTreeAdder(root, g); + + return g; } } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 04569e22d..9eac64b4d 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -439,28 +439,14 @@ protected: */ mutable VectorValues delta_; - mutable VectorValues deltaNewton_; - mutable VectorValues 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 - * requested with getDelta() or calculateEstimate(). - * - * This is \c mutable because it is used internally to not update delta_ - * until it is needed. - */ - mutable bool deltaUptodate_; + mutable VectorValues deltaNewton_; // Only used when using Dogleg - stores the Gauss-Newton update + mutable VectorValues RgProd_; // Only used when using Dogleg - stores R*g and is updated incrementally /** A cumulative mask for the variables that were replaced and have not yet * been updated in the linear solution delta_, this is only used internally, * delta will always be updated if necessary when requested with getDelta() * or calculateEstimate(). * - * This does not need to be permuted because any change in variable ordering - * that would cause a permutation will also mark variables as needing to be - * updated in this mask. - * * This is \c mutable because it is used internally to not update delta_ * until it is needed. */ @@ -622,7 +608,15 @@ public: /** prints out clique statistics */ void printStats() const { getCliqueData().getStats().print(); } - + + /** 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&). + * + * @return A VectorValues storing the gradient. + */ + VectorValues gradientAtZero() const; + /// @} protected: @@ -633,20 +627,10 @@ protected: virtual boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, const std::vector& observedKeys, const FastSet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); - // void linear_update(const GaussianFactorGraph& newFactors); void updateDelta(bool forceFullSolve = false) const; - friend GTSAM_EXPORT void optimizeInPlace(const ISAM2&, VectorValues&); - friend GTSAM_EXPORT void optimizeGradientSearchInPlace(const ISAM2&, VectorValues&); - }; // ISAM2 -/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ -GTSAM_EXPORT VectorValues optimize(const ISAM2& isam); - -/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ -GTSAM_EXPORT 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 @@ -666,66 +650,10 @@ template size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, double threshold, const FastSet& replaced, VectorValues& 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] - */ -GTSAM_EXPORT VectorValues optimizeGradientSearch(const ISAM2& isam); - -/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */ -GTSAM_EXPORT 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 - */ -GTSAM_EXPORT VectorValues gradient(const ISAM2& 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 - */ -GTSAM_EXPORT void gradientAtZero(const ISAM2& bayesTree, VectorValues& g); - } /// namespace gtsam #include diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index be8165b10..3bcb313b0 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -274,8 +274,7 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c // Check gradient VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero(); VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient)); - VectorValues actualGradient; - gradientAtZero(isam, actualGradient); + VectorValues actualGradient = isam.gradientAtZero(); bool expectedGradOk = assert_equal(expectedGradient2, expectedGradient); EXPECT(expectedGradOk); bool totalGradOk = assert_equal(expectedGradient, actualGradient); @@ -503,8 +502,7 @@ TEST(ISAM2, swapFactors) // Check gradient VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero(); VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient)); - VectorValues actualGradient; - gradientAtZero(isam, actualGradient); + VectorValues actualGradient = isam.gradientAtZero(); EXPECT(assert_equal(expectedGradient2, expectedGradient)); EXPECT(assert_equal(expectedGradient, actualGradient)); } @@ -629,8 +627,7 @@ TEST(ISAM2, constrained_ordering) // Check gradient VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero(); VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient)); - VectorValues actualGradient; - gradientAtZero(isam, actualGradient); + VectorValues actualGradient = isam.gradientAtZero(); EXPECT(assert_equal(expectedGradient2, expectedGradient)); EXPECT(assert_equal(expectedGradient, actualGradient)); }