In progress
Richard Roberts
parent
92bd4e280d
commit
99c3371474
|
|
@ -21,7 +21,7 @@
|
||||||
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
|
<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">
|
<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"/>
|
<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.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">
|
<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">
|
<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">
|
||||||
|
|
|
||||||
2
.project
2
.project
|
|
@ -23,7 +23,7 @@
|
||||||
</dictionary>
|
</dictionary>
|
||||||
<dictionary>
|
<dictionary>
|
||||||
<key>org.eclipse.cdt.make.core.buildArguments</key>
|
<key>org.eclipse.cdt.make.core.buildArguments</key>
|
||||||
<value>-j5</value>
|
<value></value>
|
||||||
</dictionary>
|
</dictionary>
|
||||||
<dictionary>
|
<dictionary>
|
||||||
<key>org.eclipse.cdt.make.core.buildCommand</key>
|
<key>org.eclipse.cdt.make.core.buildCommand</key>
|
||||||
|
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -10,126 +10,17 @@
|
||||||
* -------------------------------------------------------------------------- */
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file ISAM2-impl-inl.h
|
* @file ISAM2-impl.cpp
|
||||||
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
|
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
|
||||||
* @author Michael Kaess, Richard Roberts
|
* @author Michael Kaess, Richard Roberts
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <gtsam/linear/GaussianBayesTree.h>
|
#include <gtsam/nonlinear/ISAM2-impl.h>
|
||||||
|
|
||||||
namespace gtsam {
|
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::Impl::AddVariables(
|
||||||
void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
|
|
||||||
const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, vector<bool>& replacedKeys,
|
const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, vector<bool>& replacedKeys,
|
||||||
Ordering& ordering,typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
|
Ordering& ordering,typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
|
||||||
const bool debug = ISDEBUG("ISAM2 AddVariables");
|
const bool debug = ISDEBUG("ISAM2 AddVariables");
|
||||||
|
|
@ -163,8 +54,7 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template<class CONDITIONAL, class GRAPH>
|
FastSet<Index> ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
|
||||||
FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
|
|
||||||
FastSet<Index> indices;
|
FastSet<Index> indices;
|
||||||
BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) {
|
BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) {
|
||||||
BOOST_FOREACH(Key key, factor->keys()) {
|
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::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering,
|
||||||
FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering,
|
|
||||||
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
|
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
|
||||||
FastSet<Index> relinKeys;
|
FastSet<Index> relinKeys;
|
||||||
|
|
||||||
|
|
@ -204,8 +93,7 @@ FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::CheckRelinearization(const Permut
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template<class CONDITIONAL, class GRAPH>
|
void ISAM2::Impl::FindAll(typename ISAM2Clique::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
|
||||||
void ISAM2<CONDITIONAL,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
|
|
||||||
static const bool debug = false;
|
static const bool debug = false;
|
||||||
// does the separator contain any of the variables?
|
// does the separator contain any of the variables?
|
||||||
bool found = false;
|
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) clique->print("Key(s) marked in clique ");
|
||||||
if(debug) cout << "so marking key " << (*clique)->keys().front() << endl;
|
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);
|
FindAll(child, keys, markedMask);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template<class CONDITIONAL, class GRAPH>
|
void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta, const Ordering& ordering,
|
||||||
void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta, const Ordering& ordering,
|
|
||||||
const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug, const KeyFormatter& keyFormatter) {
|
const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug, const KeyFormatter& keyFormatter) {
|
||||||
// If debugging, invalidate if requested, otherwise do not invalidate.
|
// If debugging, invalidate if requested, otherwise do not invalidate.
|
||||||
// Invalidating means setting expmapped entries to Inf, to trigger assertions
|
// 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>
|
ISAM2::Impl::PartialSolveResult
|
||||||
typename ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolveResult
|
ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
|
||||||
ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
|
|
||||||
const FastSet<Index>& keys, const ReorderingMode& reorderingMode) {
|
const FastSet<Index>& keys, const ReorderingMode& reorderingMode) {
|
||||||
|
|
||||||
static const bool debug = ISDEBUG("ISAM2 recalculate");
|
static const bool debug = ISDEBUG("ISAM2 recalculate");
|
||||||
|
|
@ -340,14 +226,8 @@ ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
|
||||||
|
|
||||||
// eliminate into a Bayes net
|
// eliminate into a Bayes net
|
||||||
tic(7,"eliminate");
|
tic(7,"eliminate");
|
||||||
JunctionTree<GaussianFactorGraph, typename ISAM2Type::Clique> jt(factors, affectedFactorsIndex);
|
JunctionTree<GaussianFactorGraph, typename ISAM2::Clique> jt(factors, affectedFactorsIndex);
|
||||||
result.bayesTree = jt.eliminate(EliminatePreferLDL);
|
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");
|
toc(7,"eliminate");
|
||||||
|
|
||||||
tic(8,"permute eliminated");
|
tic(8,"permute eliminated");
|
||||||
|
|
@ -363,19 +243,18 @@ ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
namespace internal {
|
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
|
// parents are assumed to already be solved and available in result
|
||||||
clique->conditional()->solveInPlace(result);
|
clique->conditional()->solveInPlace(result);
|
||||||
|
|
||||||
// starting from the root, call optimize on each conditional
|
// 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);
|
optimizeInPlace(child, result);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template<class CONDITIONAL, class GRAPH>
|
size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold) {
|
||||||
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 lastBacksubVariableCount;
|
size_t lastBacksubVariableCount;
|
||||||
|
|
||||||
|
|
@ -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);
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
|
@ -1,6 +1,6 @@
|
||||||
/* ----------------------------------------------------------------------------
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
* Atlanta, Georgia 30332-0415
|
* Atlanta, Georgia 30332-0415
|
||||||
* All Rights Reserved
|
* All Rights Reserved
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
@ -10,624 +10,136 @@
|
||||||
* -------------------------------------------------------------------------- */
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file ISAM2-inl.h
|
* @file ISAM2-inl.h
|
||||||
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
|
* @brief
|
||||||
* @author Michael Kaess, Richard Roberts
|
* @author Richard Roberts
|
||||||
|
* @date Mar 16, 2012
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
||||||
#pragma once
|
#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/inference/FactorGraph.h>
|
||||||
#include <gtsam/base/debug.h>
|
#include <gtsam/linear/JacobianFactor.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 <boost/bind.hpp>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
||||||
static const bool disableReordering = false;
|
|
||||||
static const double batchThreshold = 0.65;
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template<class CONDITIONAL, class GRAPH>
|
namespace internal {
|
||||||
ISAM2<CONDITIONAL, GRAPH>::ISAM2(const ISAM2Params& params):
|
template<class CLIQUE>
|
||||||
delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true), params_(params) {
|
void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
|
||||||
// See note in gtsam/base/boost_variant_with_workaround.h
|
vector<bool>& changed, const vector<bool>& replaced, Permuted<VectorValues>& delta, int& count) {
|
||||||
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
|
// if none of the variables in this clique (frontal and separator!) changed
|
||||||
doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
|
// 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?
|
||||||
template<class CONDITIONAL, class GRAPH>
|
bool cliqueReplaced = replaced[(*clique)->frontals().front()];
|
||||||
ISAM2<CONDITIONAL, GRAPH>::ISAM2():
|
#ifndef NDEBUG
|
||||||
delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true) {
|
BOOST_FOREACH(Index frontal, (*clique)->frontals()) {
|
||||||
// See note in gtsam/base/boost_variant_with_workaround.h
|
assert(cliqueReplaced == replaced[frontal]);
|
||||||
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();
|
#endif
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
// If not redone, then has one of the separator variables changed significantly?
|
||||||
// retrieve all factors that ONLY contain the affected variables
|
bool recalculate = cliqueReplaced;
|
||||||
// (note that the remaining stuff is summarized in the cached factors)
|
if(!recalculate) {
|
||||||
template<class CONDITIONAL, class GRAPH>
|
BOOST_FOREACH(Index parent, (*clique)->parents()) {
|
||||||
FactorGraph<GaussianFactor>::shared_ptr
|
if(changed[parent]) {
|
||||||
ISAM2<CONDITIONAL, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
|
recalculate = true;
|
||||||
|
|
||||||
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;
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (inside)
|
|
||||||
nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]);
|
|
||||||
}
|
}
|
||||||
toc(3,"check candidates");
|
|
||||||
|
|
||||||
tic(4,"linearize");
|
// Solve clique if it was replaced, or if any parents were changed above the
|
||||||
FactorGraph<GaussianFactor>::shared_ptr linearized(nonlinearAffectedFactors.linearize(theta_, ordering_));
|
// threshold or themselves replaced.
|
||||||
toc(4,"linearize");
|
if(recalculate) {
|
||||||
return linearized;
|
|
||||||
|
// 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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// find intermediate (linearized) factors from cache that are passed into the affected area
|
template<class CLIQUE>
|
||||||
template<class CONDITIONAL, class GRAPH>
|
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const vector<bool>& keys, Permuted<VectorValues>& delta) {
|
||||||
FactorGraph<typename ISAM2<CONDITIONAL, GRAPH>::CacheFactor>
|
vector<bool> changed(keys.size(), false);
|
||||||
ISAM2<CONDITIONAL, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
|
int count = 0;
|
||||||
|
// starting from the root, call optimize on each conditional
|
||||||
static const bool debug = false;
|
if(root)
|
||||||
|
internal::optimizeWildfire(root, threshold, changed, keys, delta, count);
|
||||||
FactorGraph<CacheFactor> cachedBoundary;
|
return count;
|
||||||
|
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
template<class CONDITIONAL, class GRAPH>
|
/* ************************************************************************* */
|
||||||
boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
|
template<class CLIQUE>
|
||||||
const FastSet<Index>& markedKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
|
void nnz_internal(const boost::shared_ptr<CLIQUE>& clique, int& result) {
|
||||||
const boost::optional<FastSet<Index> >& constrainKeys, ISAM2Result& result) {
|
int dimR = (*clique)->dim();
|
||||||
|
int dimSep = (*clique)->get_S().cols() - dimR;
|
||||||
// TODO: new factors are linearized twice, the newFactors passed in are not used.
|
result += ((dimR+1)*dimR)/2 + dimSep*dimR;
|
||||||
|
// traverse the children
|
||||||
static const bool debug = ISDEBUG("ISAM2 recalculate");
|
BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) {
|
||||||
|
nnz_internal(child, result);
|
||||||
// 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;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template<class CONDITIONAL, class GRAPH>
|
template<class CLIQUE>
|
||||||
ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
|
int calculate_nnz(const boost::shared_ptr<CLIQUE>& clique) {
|
||||||
const GRAPH& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices,
|
int result = 0;
|
||||||
const boost::optional<FastSet<Key> >& constrainedKeys, bool force_relinearize) {
|
// starting from the root, add up entries of frontal and conditional matrices of each conditional
|
||||||
|
nnz_internal(clique, result);
|
||||||
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
|
|
||||||
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]);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
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;
|
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
|
|
||||||
|
|
|
||||||
|
|
@ -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
|
||||||
|
|
@ -12,7 +12,7 @@
|
||||||
/**
|
/**
|
||||||
* @file ISAM2.h
|
* @file ISAM2.h
|
||||||
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
|
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
|
||||||
* @author Michael Kaess
|
* @author Michael Kaess, Richard Roberts
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// \callgraph
|
// \callgraph
|
||||||
|
|
@ -23,8 +23,6 @@
|
||||||
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
|
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
|
||||||
#include <gtsam/inference/BayesTree.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>
|
#include <boost/variant.hpp>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
@ -181,14 +179,13 @@ struct ISAM2Result {
|
||||||
FastVector<size_t> newFactorsIndices;
|
FastVector<size_t> newFactorsIndices;
|
||||||
};
|
};
|
||||||
|
|
||||||
template<class CONDITIONAL>
|
struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique, GaussianConditional> {
|
||||||
struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique<CONDITIONAL>, CONDITIONAL> {
|
|
||||||
|
|
||||||
typedef ISAM2Clique<CONDITIONAL> This;
|
typedef ISAM2Clique This;
|
||||||
typedef BayesTreeCliqueBase<This,CONDITIONAL> Base;
|
typedef BayesTreeCliqueBase<This,GaussianConditional> Base;
|
||||||
typedef boost::shared_ptr<This> shared_ptr;
|
typedef boost::shared_ptr<This> shared_ptr;
|
||||||
typedef boost::weak_ptr<This> weak_ptr;
|
typedef boost::weak_ptr<This> weak_ptr;
|
||||||
typedef CONDITIONAL ConditionalType;
|
typedef GaussianConditional ConditionalType;
|
||||||
typedef typename ConditionalType::shared_ptr sharedConditional;
|
typedef typename ConditionalType::shared_ptr sharedConditional;
|
||||||
|
|
||||||
typename Base::FactorType::shared_ptr cachedFactor_;
|
typename Base::FactorType::shared_ptr cachedFactor_;
|
||||||
|
|
@ -269,8 +266,7 @@ private:
|
||||||
* estimate of all variables.
|
* estimate of all variables.
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
template<class CONDITIONAL, class GRAPH = NonlinearFactorGraph>
|
class ISAM2: public BayesTree<GaussianConditional, ISAM2Clique> {
|
||||||
class ISAM2: public BayesTree<CONDITIONAL, ISAM2Clique<CONDITIONAL> > {
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
|
|
@ -316,7 +312,7 @@ protected:
|
||||||
mutable std::vector<bool> deltaReplacedMask_;
|
mutable std::vector<bool> deltaReplacedMask_;
|
||||||
|
|
||||||
/** All original nonlinear factors are stored here to use during relinearization */
|
/** All original nonlinear factors are stored here to use during relinearization */
|
||||||
GRAPH nonlinearFactors_;
|
NonlinearFactorGraph nonlinearFactors_;
|
||||||
|
|
||||||
/** The current elimination ordering Symbols to Index (integer) keys.
|
/** The current elimination ordering Symbols to Index (integer) keys.
|
||||||
*
|
*
|
||||||
|
|
@ -339,9 +335,7 @@ private:
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
typedef BayesTree<CONDITIONAL,ISAM2Clique<CONDITIONAL> > Base; ///< The BayesTree base class
|
typedef BayesTree<GaussianConditional,ISAM2Clique> Base; ///< The BayesTree base class
|
||||||
typedef ISAM2<CONDITIONAL> This; ///< This class
|
|
||||||
typedef GRAPH Graph;
|
|
||||||
|
|
||||||
/** Create an empty ISAM2 instance */
|
/** Create an empty ISAM2 instance */
|
||||||
ISAM2(const ISAM2Params& params);
|
ISAM2(const ISAM2Params& params);
|
||||||
|
|
@ -353,7 +347,7 @@ public:
|
||||||
typedef typename Base::sharedClique sharedClique; ///< Shared pointer to a clique
|
typedef typename Base::sharedClique sharedClique; ///< Shared pointer to a clique
|
||||||
typedef typename Base::Cliques Cliques; ///< List of Clique typedef from base class
|
typedef 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);
|
boost::shared_ptr<Base> bayesTree = boost::static_pointer_cast<Base>(newISAM2);
|
||||||
Base::cloneTo(bayesTree);
|
Base::cloneTo(bayesTree);
|
||||||
newISAM2->theta_ = theta_;
|
newISAM2->theta_ = theta_;
|
||||||
|
|
@ -395,7 +389,7 @@ public:
|
||||||
* (Params::relinearizeSkip).
|
* (Params::relinearizeSkip).
|
||||||
* @return An ISAM2Result struct containing information about the update
|
* @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 FastVector<size_t>& removeFactorIndices = FastVector<size_t>(),
|
||||||
const boost::optional<FastSet<Key> >& constrainedKeys = boost::none,
|
const boost::optional<FastSet<Key> >& constrainedKeys = boost::none,
|
||||||
bool force_relinearize = false);
|
bool force_relinearize = false);
|
||||||
|
|
@ -432,7 +426,7 @@ public:
|
||||||
const Permuted<VectorValues>& getDelta() const;
|
const Permuted<VectorValues>& getDelta() const;
|
||||||
|
|
||||||
/** Access the set of nonlinear factors */
|
/** Access the set of nonlinear factors */
|
||||||
const GRAPH& getFactorsUnsafe() const { return nonlinearFactors_; }
|
const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; }
|
||||||
|
|
||||||
/** Access the current ordering */
|
/** Access the current ordering */
|
||||||
const Ordering& getOrdering() const { return ordering_; }
|
const Ordering& getOrdering() const { return ordering_; }
|
||||||
|
|
@ -462,6 +456,88 @@ private:
|
||||||
|
|
||||||
}; // ISAM2
|
}; // 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
|
} /// namespace gtsam
|
||||||
|
|
||||||
#include <gtsam/nonlinear/ISAM2-inl.h>
|
#include <gtsam/nonlinear/ISAM2-inl.h>
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue