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

In progress

release/4.3a0
Richard Roberts 2012-03-16 20:55:21 +00:00
parent 92bd4e280d
commit 99c3371474
10 changed files with 1022 additions and 1130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.cproject
View File

@ -21,7 +21,7 @@
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder arguments="" buildPath="${ProjDirPath}/build" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="5" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<builder arguments="" buildPath="${ProjDirPath}/build" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="false" parallelizationNumber="4" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">

2
.project
View File

@ -23,7 +23,7 @@
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildArguments</key>
<value>-j5</value>
<value></value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildCommand</key>

176
gtsam/nonlinear/GaussianISAM2-inl.h
View File

@ -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 <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <boost/bind.hpp>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
namespace internal {
template<class CLIQUE>
void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
vector<bool>& changed, const vector<bool>& replaced, Permuted<VectorValues>& 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<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<Eigen::Infinity>() >= 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<class CLIQUE>
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const vector<bool>& keys, Permuted<VectorValues>& delta) {
vector<bool> 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<class GRAPH>
VectorValues optimizeGradientSearch(const ISAM2<GaussianConditional, GRAPH>& isam) {
tic(0, "Allocate VectorValues");
VectorValues grad = *allocateVectorValues(isam);
toc(0, "Allocate VectorValues");
optimizeGradientSearchInPlace(isam, grad);
return grad;
}
/* ************************************************************************* */
template<class GRAPH>
void optimizeGradientSearchInPlace(const ISAM2<GaussianConditional, GRAPH>& 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<JacobianFactor> 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<class CLIQUE>
void nnz_internal(const boost::shared_ptr<CLIQUE>& 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<class CLIQUE>
int calculate_nnz(const boost::shared_ptr<CLIQUE>& 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

60
gtsam/nonlinear/GaussianISAM2.cpp
View File

@ -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 <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/GaussianISAM2.h>
using namespace std;
using namespace gtsam;
#include <boost/bind.hpp>
namespace gtsam {
/* ************************************************************************* */
VectorValues gradient(const BayesTree<GaussianConditional, ISAM2Clique<GaussianConditional> >& bayesTree, const VectorValues& x0) {
return gradient(FactorGraph<JacobianFactor>(bayesTree), x0);
}
/* ************************************************************************* */
static void gradientAtZeroTreeAdder(const boost::shared_ptr<ISAM2Clique<GaussianConditional> >& 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<ISAM2Clique<GaussianConditional> > sharedClique;
BOOST_FOREACH(const sharedClique& child, root->children()) {
gradientAtZeroTreeAdder(child, g);
}
}
/* ************************************************************************* */
void gradientAtZero(const BayesTree<GaussianConditional, ISAM2Clique<GaussianConditional> >& bayesTree, VectorValues& g) {
// Zero-out gradient
g.setZero();
// Sum up contributions for each clique
gradientAtZeroTreeAdder(bayesTree.root(), g);
}
}

153
gtsam/nonlinear/GaussianISAM2.h
View File

@ -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 <gtsam/linear/GaussianConditional.h>
#include <gtsam/nonlinear/ISAM2.h>
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<VALUES>
*/
template <class GRAPH = NonlinearFactorGraph>
class GaussianISAM2 : public ISAM2<GaussianConditional, GRAPH> {
typedef ISAM2<GaussianConditional, GRAPH> Base;
public:
/// @name Standard Constructors
/// @{
/** Create an empty ISAM2 instance */
GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, GRAPH>(params) {}
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
GaussianISAM2() : ISAM2<GaussianConditional, GRAPH>() {}
/// @}
/// @name Advanced Interface
/// @{
void cloneTo(boost::shared_ptr<GaussianISAM2>& newGaussianISAM2) const {
boost::shared_ptr<Base> isam2 = boost::static_pointer_cast<Base>(newGaussianISAM2);
Base::cloneTo(isam2);
}
/// @}
};
/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
template<class GRAPH>
VectorValues optimize(const ISAM2<GaussianConditional, GRAPH>& 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<class CLIQUE>
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root,
double threshold, const std::vector<bool>& replaced, Permuted<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]
*/
template<class GRAPH>
VectorValues optimizeGradientSearch(const ISAM2<GaussianConditional, GRAPH>& isam);
/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */
template<class GRAPH>
void optimizeGradientSearchInPlace(const ISAM2<GaussianConditional, GRAPH>& 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
*/
VectorValues gradient(const BayesTree<GaussianConditional, ISAM2Clique<GaussianConditional> >& 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<GaussianConditional, ISAM2Clique<GaussianConditional> >& bayesTree, VectorValues& g);
}/// namespace gtsam
#include <gtsam/nonlinear/GaussianISAM2-inl.h>

149
gtsam/nonlinear/ISAM2-impl-inl.h → gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -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 <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/nonlinear/ISAM2-impl.h>
namespace gtsam {
using namespace std;
template<class CONDITIONAL, class GRAPH>
struct ISAM2<CONDITIONAL, GRAPH>::Impl {
typedef ISAM2<CONDITIONAL, GRAPH> 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<const FastSet<Index>&> 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<VectorValues>& delta, vector<bool>& 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<Index> 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<Index> CheckRelinearization(const Permuted<VectorValues>& 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<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& 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<VectorValues>& delta,
const Ordering& ordering, const std::vector<bool>& mask,
boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>(),
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<Index>& keys,
const ReorderingMode& reorderingMode);
static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique<CONDITIONAL> >& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold);
};
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
void ISAM2::Impl::AddVariables(
const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, vector<bool>& replacedKeys,
Ordering& ordering,typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
const bool debug = ISDEBUG("ISAM2 AddVariables");
@ -163,8 +54,7 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
FastSet<Index> ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
FastSet<Index> indices;
BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) {
BOOST_FOREACH(Key key, factor->keys()) {
@ -175,8 +65,7 @@ FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::IndicesFromFactors(const Ordering
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering,
FastSet<Index> ISAM2::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
FastSet<Index> relinKeys;
@ -204,8 +93,7 @@ FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::CheckRelinearization(const Permut
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
void ISAM2::Impl::FindAll(typename ISAM2Clique::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
static const bool debug = false;
// does the separator contain any of the variables?
bool found = false;
@ -219,14 +107,13 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::
if(debug) clique->print("Key(s) marked in clique ");
if(debug) cout << "so marking key " << (*clique)->keys().front() << endl;
}
BOOST_FOREACH(const typename ISAM2Clique<CONDITIONAL>::shared_ptr& child, clique->children_) {
BOOST_FOREACH(const typename ISAM2Clique::shared_ptr& child, clique->children_) {
FindAll(child, keys, markedMask);
}
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta, const Ordering& ordering,
void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta, const Ordering& ordering,
const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> 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<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permute
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
typename ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolveResult
ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
ISAM2::Impl::PartialSolveResult
ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
const FastSet<Index>& keys, const ReorderingMode& reorderingMode) {
static const bool debug = ISDEBUG("ISAM2 recalculate");
@ -340,14 +226,8 @@ ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
// eliminate into a Bayes net
tic(7,"eliminate");
JunctionTree<GaussianFactorGraph, typename ISAM2Type::Clique> jt(factors, affectedFactorsIndex);
JunctionTree<GaussianFactorGraph, typename ISAM2::Clique> jt(factors, affectedFactorsIndex);
result.bayesTree = jt.eliminate(EliminatePreferLDL);
if(debug && result.bayesTree) {
if(boost::dynamic_pointer_cast<ISAM2Clique<CONDITIONAL> >(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<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
/* ************************************************************************* */
namespace internal {
inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique<GaussianConditional> >& clique, VectorValues& result) {
inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique>& 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<ISAM2Clique<GaussianConditional> >& child, clique->children_)
BOOST_FOREACH(const boost::shared_ptr<ISAM2Clique>& child, clique->children_)
optimizeInPlace(child, result);
}
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
size_t ISAM2<CONDITIONAL,GRAPH>::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique<CONDITIONAL> >& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold) {
size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold) {
size_t lastBacksubVariableCount;

127
gtsam/nonlinear/ISAM2-impl.h Normal file
View File

@ -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 <gtsam/linear/GaussianBayesTree.h>
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<const FastSet<Index>&> 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<VectorValues>& delta, vector<bool>& 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<Index> 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<Index> CheckRelinearization(const Permuted<VectorValues>& 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<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& 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<VectorValues>& delta,
const Ordering& ordering, const std::vector<bool>& mask,
boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>(),
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<Index>& keys,
const ReorderingMode& reorderingMode);
static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique<CONDITIONAL> >& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold);
};
}

660
gtsam/nonlinear/ISAM2-inl.h
View File

@ -11,623 +11,135 @@
/**
* @file ISAM2-inl.h
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
* @author Michael Kaess, Richard Roberts
* @brief
* @author Richard Roberts
* @date Mar 16, 2012
*/
#pragma once
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <gtsam/base/timing.h>
#include <gtsam/base/debug.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/nonlinear/ISAM2-impl-inl.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <boost/bind.hpp>
namespace gtsam {
using namespace std;
static const bool disableReordering = false;
static const double batchThreshold = 0.65;
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
ISAM2<CONDITIONAL, GRAPH>::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<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
namespace internal {
template<class CLIQUE>
void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
vector<bool>& changed, const vector<bool>& replaced, Permuted<VectorValues>& 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
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
ISAM2<CONDITIONAL, GRAPH>::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<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
FastList<size_t> ISAM2<CONDITIONAL, GRAPH>::getAffectedFactors(const FastList<Index>& 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<NonlinearFactor > allAffected;
FastList<size_t> indices;
BOOST_FOREACH(const Index key, keys) {
// const list<size_t> 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)
template<class CONDITIONAL, class GRAPH>
FactorGraph<GaussianFactor>::shared_ptr
ISAM2<CONDITIONAL, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
tic(1,"getAffectedFactors");
FastList<size_t> candidates = getAffectedFactors(affectedKeys);
toc(1,"getAffectedFactors");
GRAPH nonlinearAffectedFactors;
tic(2,"affectedKeysSet");
// for fast lookup below
FastSet<Index> 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<GaussianFactor>::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
template<class CONDITIONAL, class GRAPH>
FactorGraph<typename ISAM2<CONDITIONAL, GRAPH>::CacheFactor>
ISAM2<CONDITIONAL, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
// Solve clique if it was replaced, or if any parents were changed above the
// threshold or themselves replaced.
if(recalculate) {
static const bool debug = false;
FactorGraph<CacheFactor> cachedBoundary;
BOOST_FOREACH(sharedClique orphan, orphans) {
// find the last variable that was eliminated
Index key = (*orphan)->frontals().back();
#ifndef NDEBUG
// typename BayesNet<CONDITIONAL>::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<CacheFactor>(orphan->cachedFactor()));
if(debug) { cout << "Cached factor for variable " << key; orphan->cachedFactor()->print(""); }
// Temporary copy of the original values, to check how much they change
vector<Vector> originalValues((*clique)->nrFrontals());
GaussianConditional::const_iterator it;
for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
originalValues[it - (*clique)->beginFrontals()] = delta[*it];
}
return cachedBoundary;
// 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<Eigen::Infinity>() >= threshold) {
valuesChanged = true;
break;
}
} else
break;
}
template<class CONDITIONAL, class GRAPH>
boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
const FastSet<Index>& markedKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
const boost::optional<FastSet<Index> >& 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());
// 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;
}
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<GaussianConditional> affectedBayesNet;
this->removeTop(markedKeys, affectedBayesNet, orphans);
toc(1, "removetop");
if(debug) affectedBayesNet.print("Removed top: ");
if(debug) orphans.print("Orphans: ");
// FactorGraph<GaussianFactor> 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<Index> affectedKeys = affectedBayesNet.ordering();
toc(2,"affectedKeys");
if(affectedKeys.size() >= theta_.size() * batchThreshold) {
tic(3,"batch");
tic(0,"add keys");
boost::shared_ptr<FastSet<Index> > affectedKeysSet(new FastSet<Index>());
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<int> cmember(theta_.size(), 0);
FastSet<Index> 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<GaussianFactorGraph, typename Base::Clique> 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<Index> 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<CacheFactor> 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)));
// Replace with the old values
for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
delta[*it] = originalValues[it - (*clique)->beginFrontals()];
}
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<FastSet<Index> > affectedKeysSet(new FastSet<Index>(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<Index>(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);
// Recurse to children
BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) {
optimizeWildfire(child, threshold, changed, replaced, delta, count);
}
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<class CONDITIONAL, class GRAPH>
ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
const GRAPH& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices,
const boost::optional<FastSet<Key> >& 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: ");
template<class CLIQUE>
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const vector<bool>& keys, Permuted<VectorValues>& delta) {
vector<bool> 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;
}
// 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<newFactors.size(); ++i)
result.newFactorsIndices[i] = i + nonlinearFactors_.size();
// 1. Add any new factors \Factors:=\Factors\cup\Factors'.
if(debug || verbose) newFactors.print("The new factors are: ");
nonlinearFactors_.push_back(newFactors);
// Remove the removed factors
GRAPH removeFactors; removeFactors.reserve(removeFactorIndices.size());
BOOST_FOREACH(size_t index, removeFactorIndices) {
removeFactors.push_back(nonlinearFactors_[index]);
nonlinearFactors_.remove(index);
}
// Remove removed factors from the variable index so we do not attempt to relinearize them
variableIndex_.remove(removeFactorIndices, *removeFactors.symbolic(ordering_));
toc(1,"push_back factors");
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_);
toc(2,"add new variables");
tic(3,"evaluate error before");
if(params_.evaluateNonlinearError)
result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate()));
toc(3,"evaluate error before");
tic(4,"gather involved keys");
// 3. Mark linear update
FastSet<Index> markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors
// Also mark keys involved in removed factors
{
FastSet<Index> 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<Index> 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<bool> markedRelinMask(ordering_.nVars(), false);
// 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
FastSet<Index> 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<bool>(ordering_.nVars(), false);
#endif
}
tic(8,"linearize new");
tic(1,"linearize");
// 7. Linearize new factors
FactorGraph<GaussianFactor>::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<FastSet<Index> > constrainedIndices;
if(constrainedKeys) {
constrainedIndices.reset(FastSet<Index>());
BOOST_FOREACH(Key key, *constrainedKeys) {
constrainedIndices->insert(ordering_[key]);
/* ************************************************************************* */
template<class CLIQUE>
void nnz_internal(const boost::shared_ptr<CLIQUE>& 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);
}
}
boost::shared_ptr<FastSet<Index> > 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<class CLIQUE>
int calculate_nnz(const boost::shared_ptr<CLIQUE>& 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<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL, GRAPH>::updateDelta(bool forceFullSolve) const {
if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) {
// If using Gauss-Newton, update with wildfireThreshold
const ISAM2GaussNewtonParams& gaussNewtonParams =
boost::get<ISAM2GaussNewtonParams>(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<ISAM2DoglegParams>(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<class CONDITIONAL, class GRAPH>
Values ISAM2<CONDITIONAL, GRAPH>::calculateEstimate() const {
// We use ExpmapMasked here instead of regular expmap because the former
// handles Permuted<VectorValues>
Values ret(theta_);
vector<bool> mask(ordering_.nVars(), true);
Impl::ExpmapMasked(ret, getDelta(), ordering_, mask);
return ret;
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
template<class VALUE>
VALUE ISAM2<CONDITIONAL, GRAPH>::calculateEstimate(Key key) const {
const Index index = getOrdering()[key];
const SubVector delta = getDelta()[index];
return theta_.at<VALUE>(key).retract(delta);
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
Values ISAM2<CONDITIONAL, GRAPH>::calculateBestEstimate() const {
VectorValues delta(theta_.dims(ordering_));
optimize2(this->root(), delta);
return theta_.retract(delta, ordering_);
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
const Permuted<VectorValues>& ISAM2<CONDITIONAL, GRAPH>::getDelta() const {
if(!deltaUptodate_)
updateDelta();
return delta_;
}
}
/// namespace gtsam

687
gtsam/nonlinear/ISAM2.cpp Normal file
View File

@ -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 <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <gtsam/base/timing.h>
#include <gtsam/base/debug.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/ISAM2-impl-inl.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
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<ISAM2DoglegParams>(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<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
}
/* ************************************************************************* */
FastList<size_t> ISAM2::getAffectedFactors(const FastList<Index>& 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<NonlinearFactor > allAffected;
FastList<size_t> indices;
BOOST_FOREACH(const Index key, keys) {
// const list<size_t> 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<GaussianFactor>::shared_ptr
ISAM2::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
tic(1,"getAffectedFactors");
FastList<size_t> candidates = getAffectedFactors(affectedKeys);
toc(1,"getAffectedFactors");
NonlinearFactorGraph nonlinearAffectedFactors;
tic(2,"affectedKeysSet");
// for fast lookup below
FastSet<Index> 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<GaussianFactor>::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<typename ISAM2::CacheFactor>
ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
static const bool debug = false;
FactorGraph<CacheFactor> cachedBoundary;
BOOST_FOREACH(sharedClique orphan, orphans) {
// find the last variable that was eliminated
Index key = (*orphan)->frontals().back();
#ifndef NDEBUG
// typename BayesNet<CONDITIONAL>::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<CacheFactor>(orphan->cachedFactor()));
if(debug) { cout << "Cached factor for variable " << key; orphan->cachedFactor()->print(""); }
}
return cachedBoundary;
}
boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
const FastSet<Index>& markedKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
const boost::optional<FastSet<Index> >& 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<GaussianConditional> affectedBayesNet;
this->removeTop(markedKeys, affectedBayesNet, orphans);
toc(1, "removetop");
if(debug) affectedBayesNet.print("Removed top: ");
if(debug) orphans.print("Orphans: ");
// FactorGraph<GaussianFactor> 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<Index> affectedKeys = affectedBayesNet.ordering();
toc(2,"affectedKeys");
if(affectedKeys.size() >= theta_.size() * batchThreshold) {
tic(3,"batch");
tic(0,"add keys");
boost::shared_ptr<FastSet<Index> > affectedKeysSet(new FastSet<Index>());
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<int> cmember(theta_.size(), 0);
FastSet<Index> 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<GaussianFactorGraph, typename Base::Clique> 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<Index> 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<CacheFactor> 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<FastSet<Index> > affectedKeysSet(new FastSet<Index>(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<Index>(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<size_t>& removeFactorIndices,
const boost::optional<FastSet<Key> >& 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<newFactors.size(); ++i)
result.newFactorsIndices[i] = i + nonlinearFactors_.size();
// 1. Add any new factors \Factors:=\Factors\cup\Factors'.
if(debug || verbose) newFactors.print("The new factors are: ");
nonlinearFactors_.push_back(newFactors);
// Remove the removed factors
NonlinearFactorGraph removeFactors; removeFactors.reserve(removeFactorIndices.size());
BOOST_FOREACH(size_t index, removeFactorIndices) {
removeFactors.push_back(nonlinearFactors_[index]);
nonlinearFactors_.remove(index);
}
// Remove removed factors from the variable index so we do not attempt to relinearize them
variableIndex_.remove(removeFactorIndices, *removeFactors.symbolic(ordering_));
toc(1,"push_back factors");
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_);
toc(2,"add new variables");
tic(3,"evaluate error before");
if(params_.evaluateNonlinearError)
result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate()));
toc(3,"evaluate error before");
tic(4,"gather involved keys");
// 3. Mark linear update
FastSet<Index> markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors
// Also mark keys involved in removed factors
{
FastSet<Index> 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<Index> 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<bool> markedRelinMask(ordering_.nVars(), false);
// 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
FastSet<Index> 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<bool>(ordering_.nVars(), false);
#endif
}
tic(8,"linearize new");
tic(1,"linearize");
// 7. Linearize new factors
FactorGraph<GaussianFactor>::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<FastSet<Index> > constrainedIndices;
if(constrainedKeys) {
constrainedIndices.reset(FastSet<Index>());
BOOST_FOREACH(Key key, *constrainedKeys) {
constrainedIndices->insert(ordering_[key]);
}
}
boost::shared_ptr<FastSet<Index> > 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<ISAM2GaussNewtonParams>(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<ISAM2DoglegParams>(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<VectorValues>
Values ret(theta_);
vector<bool> mask(ordering_.nVars(), true);
Impl::ExpmapMasked(ret, getDelta(), ordering_, mask);
return ret;
}
/* ************************************************************************* */
template<class VALUE>
VALUE ISAM2::calculateEstimate(Key key) const {
const Index index = getOrdering()[key];
const SubVector delta = getDelta()[index];
return theta_.at<VALUE>(key).retract(delta);
}
/* ************************************************************************* */
Values ISAM2::calculateBestEstimate() const {
VectorValues delta(theta_.dims(ordering_));
optimize2(this->root(), delta);
return theta_.retract(delta, ordering_);
}
/* ************************************************************************* */
const Permuted<VectorValues>& 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<JacobianFactor> 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<GaussianConditional, ISAM2Clique>& bayesTree, const VectorValues& x0) {
return gradient(FactorGraph<JacobianFactor>(bayesTree), x0);
}
/* ************************************************************************* */
static void gradientAtZeroTreeAdder(const boost::shared_ptr<ISAM2Clique>& 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<ISAM2Clique> sharedClique;
BOOST_FOREACH(const sharedClique& child, root->children()) {
gradientAtZeroTreeAdder(child, g);
}
}
/* ************************************************************************* */
void gradientAtZero(const BayesTree<GaussianConditional, ISAM2Clique>& bayesTree, VectorValues& g) {
// Zero-out gradient
g.setZero();
// Sum up contributions for each clique
gradientAtZeroTreeAdder(bayesTree.root(), g);
}
}
/// namespace gtsam

110
gtsam/nonlinear/ISAM2.h
View File

@ -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 <gtsam/nonlinear/DoglegOptimizerImpl.h>
#include <gtsam/inference/BayesTree.h>
// Workaround for boost < 1.47, see note in file
//#include <gtsam/base/boost_variant_with_workaround.h>
#include <boost/variant.hpp>
namespace gtsam {
@ -181,14 +179,13 @@ struct ISAM2Result {
FastVector<size_t> newFactorsIndices;
};
template<class CONDITIONAL>
struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique<CONDITIONAL>, CONDITIONAL> {
struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique, GaussianConditional> {
typedef ISAM2Clique<CONDITIONAL> This;
typedef BayesTreeCliqueBase<This,CONDITIONAL> Base;
typedef ISAM2Clique This;
typedef BayesTreeCliqueBase<This,GaussianConditional> Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> 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 CONDITIONAL, class GRAPH = NonlinearFactorGraph>
class ISAM2: public BayesTree<CONDITIONAL, ISAM2Clique<CONDITIONAL> > {
class ISAM2: public BayesTree<GaussianConditional, ISAM2Clique> {
protected:
@ -316,7 +312,7 @@ protected:
mutable std::vector<bool> 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<CONDITIONAL,ISAM2Clique<CONDITIONAL> > Base; ///< The BayesTree base class
typedef ISAM2<CONDITIONAL> This; ///< This class
typedef GRAPH Graph;
typedef BayesTree<GaussianConditional,ISAM2Clique> 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<This>& newISAM2) const {
void cloneTo(boost::shared_ptr<ISAM2>& newISAM2) const {
boost::shared_ptr<Base> bayesTree = boost::static_pointer_cast<Base>(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<size_t>& removeFactorIndices = FastVector<size_t>(),
const boost::optional<FastSet<Key> >& constrainedKeys = boost::none,
bool force_relinearize = false);
@ -432,7 +426,7 @@ public:
const Permuted<VectorValues>& 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<class CLIQUE>
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root,
double threshold, const std::vector<bool>& replaced, Permuted<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]
*/
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<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
*/
VectorValues gradient(const BayesTree<GaussianConditional, ISAM2Clique>& 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<GaussianConditional, ISAM2Clique>& bayesTree, VectorValues& g);
} /// namespace gtsam
#include <gtsam/nonlinear/ISAM2-inl.h>