Removed namespace-functions for ISAM2 optimize* and gradient*, these were only there to try to maintain a consistent interface for linear but really were not correct anyway since ISAM2 is a nonlinear object. They instead caused confusion and made the code complicated since they used a complicated system for updating and caching various components that go into calculating the solution. Replaced all this with much simpler code directly inside updateDelta, which uses clearly-defined functions in the ISAM2::Impl class to calculate the components that go into the solution. Also removed the redundant deltaUpToDate flags - now just checks whether deltaReplacedMask is empty.
Richard Roberts
parent
a9ea1f4033
commit
7192bd2f79
|
|
@ -271,7 +271,8 @@ inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique>& clique,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
size_t ISAM2::Impl::UpdateDelta(const FastVector<ISAM2::sharedClique>& roots, FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold) {
|
||||
size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots,
|
||||
const FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold) {
|
||||
|
||||
size_t lastBacksubVariableCount;
|
||||
|
||||
|
|
@ -294,15 +295,12 @@ size_t ISAM2::Impl::UpdateDelta(const FastVector<ISAM2::sharedClique>& roots, Fa
|
|||
#endif
|
||||
}
|
||||
|
||||
// Clear replacedKeys
|
||||
replacedKeys.clear();
|
||||
|
||||
return lastBacksubVariableCount;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace internal {
|
||||
void updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, const FastSet<Key>& replacedKeys,
|
||||
void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const FastSet<Key>& 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<ISAM2Clique>& 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<Key>& replacedKeys,
|
||||
VectorValues& deltaNewton, VectorValues& RgProd) {
|
||||
|
||||
// Get gradient
|
||||
VectorValues grad;
|
||||
gradientAtZero(isam, grad);
|
||||
size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const FastSet<Key>& 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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -123,11 +123,24 @@ struct GTSAM_EXPORT ISAM2::Impl {
|
|||
boost::optional<VectorValues&> invalidateIfDebug = boost::none,
|
||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
|
||||
|
||||
static size_t UpdateDelta(const FastVector<ISAM2::sharedClique>& roots,
|
||||
FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold);
|
||||
/**
|
||||
* Update the Newton's method step point, using wildfire
|
||||
*/
|
||||
static size_t UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots,
|
||||
const FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold);
|
||||
|
||||
static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, FastSet<Key>& 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<Key>& replacedKeys,
|
||||
const VectorValues& gradAtZero, VectorValues& RgProd);
|
||||
|
||||
/**
|
||||
* Compute the gradient-search point. Only used in Dogleg.
|
||||
*/
|
||||
static VectorValues ComputeGradientSearch(const VectorValues& gradAtZero,
|
||||
const VectorValues& RgProd);
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ISAM2::ISAM2():
|
||||
deltaDoglegUptodate_(true), deltaUptodate_(true) {
|
||||
ISAM2::ISAM2() {
|
||||
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
|
||||
doglegDelta_ = boost::get<ISAM2DoglegParams>(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<ISAM2GaussNewtonParams>(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<ISAM2DoglegParams>(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<ISAM2GaussNewtonParams>(isam.params().optimizationParams).wildfireThreshold;
|
||||
else if(isam.params().optimizationParams.type() == typeid(ISAM2DoglegParams))
|
||||
wildfireThreshold = boost::get<ISAM2DoglegParams>(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<ISAM2GaussNewtonParams>(isam.params().optimizationParams).wildfireThreshold;
|
||||
else if(isam.params().optimizationParams.type() == typeid(ISAM2DoglegParams))
|
||||
wildfireThreshold = boost::get<ISAM2DoglegParams>(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<ISAM2Clique>& root, VectorValues& g) {
|
||||
// Loop through variables in each clique, adding contributions
|
||||
|
|
@ -1147,13 +1072,16 @@ static void gradientAtZeroTreeAdder(const boost::shared_ptr<ISAM2Clique>& 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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<FastSet<Key> > recalculate(const FastSet<Key>& markedKeys, const FastSet<Key>& relinKeys,
|
||||
const std::vector<Key>& observedKeys, const FastSet<Key>& unusedIndices, const boost::optional<FastMap<Key,int> >& 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<class CLIQUE>
|
|||
size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root,
|
||||
double threshold, const FastSet<Key>& 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<class CLIQUE>
|
||||
int calculate_nnz(const boost::shared_ptr<CLIQUE>& 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 <gtsam/nonlinear/ISAM2-inl.h>
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue