12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'feature/isam2-cleaner-solve'

release/4.3a0
Richard Roberts 2014-02-22 20:24:17 -05:00
parent 2fd9401558 9f0959feac
commit bf7f58112c
8 changed files with 97 additions and 224 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/inference/BayesTree-inst.h
View File

@ -413,12 +413,12 @@ namespace gtsam {
void BayesTree<CLIQUE>::removeClique(sharedClique clique)
{
if (clique->isRoot()) {
typename FastVector<sharedClique>::iterator root = std::find(roots_.begin(), roots_.end(), clique);
typename Roots::iterator root = std::find(roots_.begin(), roots_.end(), clique);
if(root != roots_.end())
roots_.erase(root);
} else { // detach clique from parent
sharedClique parent = clique->parent_.lock();
typename FastVector<sharedClique>::iterator child = std::find(parent->children.begin(), parent->children.end(), clique);
typename Roots::iterator child = std::find(parent->children.begin(), parent->children.end(), clique);
assert(child != parent->children.end());
parent->children.erase(child);
}

7
gtsam/inference/BayesTree.h
View File

@ -95,7 +95,10 @@ namespace gtsam {
Nodes nodes_;
/** Root cliques */
FastVector<sharedClique> roots_;
typedef FastVector<sharedClique> Roots;
/** Root cliques */
Roots roots_;
/// @name Standard Constructors
/// @{
@ -141,7 +144,7 @@ namespace gtsam {
const sharedNode operator[](Key j) const { return nodes_.at(j); }
/** return root cliques */
const FastVector<sharedClique>& roots() const { return roots_; }
const Roots& roots() const { return roots_; }
/** alternate syntax for matlab: find the clique that contains the variable with Key j */
const sharedClique& clique(Key j) const {

3
gtsam/linear/GaussianBayesTree.h
View File

@ -103,8 +103,7 @@ namespace gtsam {
* \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 [output] g A VectorValues to store the gradient, which must be preallocated, see
* allocateVectorValues */
* @return A VectorValues storing the gradient. */
VectorValues gradientAtZero() const;
/** Mahalanobis norm error. */

41
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -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;
}
}

21
gtsam/nonlinear/ISAM2-impl.h
View File

@ -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);
};

142
gtsam/nonlinear/ISAM2.cpp
View File

@ -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;
}
}

94
gtsam/nonlinear/ISAM2.h
View File

@ -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>

9
tests/testGaussianISAM2.cpp
View File

@ -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));
}