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

Merge remote-tracking branch 'svn/branches/incremental_dogleg_points_to_merge' into trunk 

Conflicts:
	.cproject
	gtsam/linear/GaussianBayesTree-inl.h
	gtsam/linear/GaussianBayesTree.cpp
	gtsam/linear/GaussianBayesTree.h
	gtsam/nonlinear/DoglegOptimizerImpl.h
	gtsam/nonlinear/GaussianISAM2-inl.h
	gtsam/nonlinear/GaussianISAM2.cpp
	gtsam/nonlinear/GaussianISAM2.h
	gtsam/nonlinear/ISAM2-impl.cpp
	gtsam/nonlinear/ISAM2-inl.h
	gtsam/nonlinear/ISAM2.h
release/4.3a0
Richard Roberts 2012-03-23 04:31:54 +00:00
parent c020616ca0 d577655f1b
commit 22ebe16a31
25 changed files with 1406 additions and 1271 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/inference/tests/testEliminationTree.cpp
View File

@ -25,7 +25,8 @@
using namespace gtsam;
using namespace std;
struct EliminationTreeTester {
class EliminationTreeTester {
public:
// build hardcoded tree
static SymbolicEliminationTree::shared_ptr buildHardcodedTree(const SymbolicFactorGraph& fg) {

13
gtsam/linear/GaussianBayesTree-inl.h
View File

@ -25,4 +25,17 @@
namespace gtsam {
/* ************************************************************************* */
namespace internal {
template<class BAYESTREE>
void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result) {
// parents are assumed to already be solved and available in result
clique->conditional()->solveInPlace(result);
// starting from the root, call optimize on each conditional
BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_)
optimizeInPlace<BAYESTREE>(child, result);
}
}
}

24
gtsam/linear/GaussianBayesTree.cpp
View File

@ -23,22 +23,15 @@
namespace gtsam {
/* ************************************************************************* */
namespace internal {
void optimizeInPlace(const boost::shared_ptr<BayesTreeClique<GaussianConditional> >& clique, VectorValues& result) {
// parents are assumed to already be solved and available in result
clique->conditional()->solveInPlace(result);
// starting from the root, call optimize on each conditional
BOOST_FOREACH(const boost::shared_ptr<BayesTreeClique<GaussianConditional> >& child, clique->children_)
optimizeInPlace(child, result);
}
VectorValues optimize(const GaussianBayesTree& bayesTree) {
VectorValues result = *allocateVectorValues(bayesTree);
optimizeInPlace(bayesTree, result);
return result;
}
/* ************************************************************************* */
VectorValues optimize(const GaussianBayesTree& bayesTree) {
VectorValues result = *allocateVectorValues(bayesTree);
internal::optimizeInPlace(bayesTree.root(), result);
return result;
void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result) {
internal::optimizeInPlace<GaussianBayesTree>(bayesTree.root(), result);
}
/* ************************************************************************* */
@ -77,11 +70,6 @@ void optimizeGradientSearchInPlace(const GaussianBayesTree& Rd, VectorValues& gr
toc(4, "Compute point");
}
/* ************************************************************************* */
void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result) {
internal::optimizeInPlace(bayesTree.root(), result);
}
/* ************************************************************************* */
VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0) {
return gradient(FactorGraph<JacobianFactor>(bayesTree), x0);

3
gtsam/linear/GaussianBayesTree.h
View File

@ -34,7 +34,8 @@ VectorValues optimize(const GaussianBayesTree& bayesTree);
void optimizeInPlace(const GaussianBayesTree& clique, VectorValues& result);
namespace internal {
void optimizeInPlace(const boost::shared_ptr<BayesTreeClique<GaussianConditional> >& clique, VectorValues& result);
template<class BAYESTREE>
void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result);
}
/**

48
gtsam/linear/GaussianConditional.cpp
View File

@ -27,42 +27,6 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
// Helper function used only in this file - extracts vectors with variable indices
// in the first and last iterators, and concatenates them in that order into the
// output.
template<class VALUES, typename ITERATOR>
static Vector extractVectorValuesSlices(const VALUES& values, ITERATOR first, ITERATOR last) {
// Find total dimensionality
int dim = 0;
for(ITERATOR j = first; j != last; ++j)
dim += values[*j].rows();
// Copy vectors
Vector ret(dim);
int varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
ret.segment(varStart, values[*j].rows()) = values[*j];
varStart += values[*j].rows();
}
return ret;
}
/* ************************************************************************* */
// Helper function used only in this file - writes to the variables in values
// with indices iterated over by first and last, interpreting vector as the
// concatenated vectors to write.
template<class VECTOR, class VALUES, typename ITERATOR>
static void writeVectorValuesSlices(const VECTOR& vector, VALUES& values, ITERATOR first, ITERATOR last) {
// Copy vectors
int varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
values[*j] = vector.segment(varStart, values[*j].rows());
varStart += values[*j].rows();
}
assert(varStart == vector.rows());
}
/* ************************************************************************* */
GaussianConditional::GaussianConditional() : rsd_(matrix_) {}
@ -230,7 +194,7 @@ inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES
static const bool debug = false;
if(debug) conditional.print("Solving conditional in place");
Vector xS = extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents());
Vector xS = internal::extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents());
xS = conditional.get_d() - conditional.get_S() * xS;
Vector soln = conditional.permutation().transpose() *
conditional.get_R().triangularView<Eigen::Upper>().solve(xS);
@ -238,7 +202,7 @@ inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES
gtsam::print(Matrix(conditional.get_R()), "Calling backSubstituteUpper on ");
gtsam::print(soln, "full back-substitution solution: ");
}
writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals());
internal::writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals());
}
/* ************************************************************************* */
@ -253,7 +217,7 @@ void GaussianConditional::solveInPlace(Permuted<VectorValues>& x) const {
/* ************************************************************************* */
void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
Vector frontalVec = extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
// TODO: verify permutation
frontalVec = permutation_ * gtsam::backSubstituteUpper(frontalVec,Matrix(get_R()));
GaussianConditional::const_iterator it;
@ -261,14 +225,14 @@ void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
const Index i = *it;
transposeMultiplyAdd(-1.0,get_S(it),frontalVec,gy[i]);
}
writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
}
/* ************************************************************************* */
void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const {
Vector frontalVec = extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
frontalVec = emul(frontalVec, get_sigmas());
writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
}
}

2
gtsam/linear/GaussianFactorGraph.h
View File

@ -31,7 +31,7 @@
namespace gtsam {
struct SharedDiagonal;
class SharedDiagonal;
/** return A*x-b
* \todo Make this a member function - affects SubgraphPreconditioner */

2
gtsam/linear/GaussianJunctionTree.cpp
View File

@ -50,7 +50,7 @@ namespace gtsam {
// back-substitution
tic(3, "back-substitute");
internal::optimizeInPlace(rootClique, result);
internal::optimizeInPlace<GaussianBayesTree>(rootClique, result);
toc(3, "back-substitute");
return result;
}

2
gtsam/linear/HessianFactor.h
View File

@ -33,7 +33,7 @@ namespace gtsam {
// Forward declarations
class JacobianFactor;
struct SharedDiagonal;
class SharedDiagonal;
class GaussianConditional;
template<class C> class BayesNet;

6
gtsam/linear/JacobianFactor.cpp
View File

@ -42,20 +42,20 @@ using namespace boost::lambda;
namespace gtsam {
/* ************************************************************************* */
inline void JacobianFactor::assertInvariants() const {
#ifndef NDEBUG
void JacobianFactor::assertInvariants() const {
#ifndef NDEBUG
GaussianFactor::assertInvariants(); // The base class checks for unique keys
assert((size() == 0 && Ab_.rows() == 0 && Ab_.nBlocks() == 0) || size()+1 == Ab_.nBlocks());
assert(firstNonzeroBlocks_.size() == Ab_.rows());
for(size_t i=0; i<firstNonzeroBlocks_.size(); ++i)
assert(firstNonzeroBlocks_[i] < Ab_.nBlocks());
#endif
// Check for non-finite values
for(size_t i=0; i<Ab_.rows(); ++i)
for(size_t j=0; j<Ab_.cols(); ++j)
if(isnan(matrix_(i,j)))
throw invalid_argument("JacobianFactor contains NaN matrix entries.");
#endif
}
/* ************************************************************************* */

2
gtsam/linear/NoiseModel.h
View File

@ -23,7 +23,7 @@
namespace gtsam {
struct SharedDiagonal; // forward declare
class SharedDiagonal; // forward declare
/// All noise models live in the noiseModel namespace
namespace noiseModel {

3
gtsam/linear/SharedDiagonal.h
View File

@ -25,7 +25,8 @@ namespace gtsam { // note, deliberately not in noiseModel namespace
// A useful convenience class to refer to a shared Diagonal model
// There are (somewhat dangerous) constructors from Vector and pair<size_t,double>
// that call Sigmas and Sigma, respectively.
struct SharedDiagonal: public noiseModel::Diagonal::shared_ptr {
class SharedDiagonal: public noiseModel::Diagonal::shared_ptr {
public:
SharedDiagonal() {
}
SharedDiagonal(const noiseModel::Diagonal::shared_ptr& p) :

3
gtsam/linear/SharedGaussian.h
View File

@ -27,7 +27,8 @@ namespace gtsam { // note, deliberately not in noiseModel namespace
* A useful convenience class to refer to a shared Gaussian model
* Also needed to make noise models in matlab
*/
struct SharedGaussian: public SharedNoiseModel {
class SharedGaussian: public SharedNoiseModel {
public:
typedef SharedNoiseModel Base;

19
gtsam/linear/VectorValues.cpp
View File

@ -156,3 +156,22 @@ void VectorValues::operator+=(const VectorValues& c) {
assert(this->hasSameStructure(c));
this->values_ += c.values_;
}
/* ************************************************************************* */
VectorValues& VectorValues::operator=(const Permuted<VectorValues>& rhs) {
if(this->size() != rhs.size())
throw std::invalid_argument("VectorValues assignment from Permuted<VectorValues> requires pre-allocation, see documentation.");
for(size_t j=0; j<this->size(); ++j) {
if(exists(j)) {
SubVector& l(this->at(j));
const SubVector& r(rhs[j]);
if(l.rows() != r.rows())
throw std::invalid_argument("VectorValues assignment from Permuted<VectorValues> requires pre-allocation, see documentation.");
l = r;
} else {
if(rhs.container().exists(rhs.permutation()[j]))
throw std::invalid_argument("VectorValues assignment from Permuted<VectorValues> requires pre-allocation, see documentation.");
}
}
return *this;
}

46
gtsam/linear/VectorValues.h
View File

@ -19,6 +19,7 @@
#include <gtsam/base/Vector.h>
#include <gtsam/base/types.h>
#include <gtsam/inference/Permutation.h>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
@ -177,7 +178,7 @@ namespace gtsam {
/** Construct from a container of variable dimensions (in variable order), without initializing any values. */
template<class CONTAINER>
VectorValues(const CONTAINER& dimensions) { append(dimensions); }
explicit VectorValues(const CONTAINER& dimensions) { append(dimensions); }
/** Construct to hold nVars vectors of varDim dimension each. */
VectorValues(Index nVars, size_t varDim) { resize(nVars, varDim); }
@ -273,6 +274,11 @@ namespace gtsam {
*/
void operator+=(const VectorValues& c);
/** Assignment operator from Permuted<VectorValues>, requires the dimensions
* of the assignee to already be properly pre-allocated.
*/
VectorValues& operator=(const Permuted<VectorValues>& rhs);
/// @}
private:
@ -402,4 +408,42 @@ namespace gtsam {
#endif
}
namespace internal {
/* ************************************************************************* */
// Helper function, extracts vectors with variable indices
// in the first and last iterators, and concatenates them in that order into the
// output.
template<class VALUES, typename ITERATOR>
Vector extractVectorValuesSlices(const VALUES& values, ITERATOR first, ITERATOR last) {
// Find total dimensionality
int dim = 0;
for(ITERATOR j = first; j != last; ++j)
dim += values[*j].rows();
// Copy vectors
Vector ret(dim);
int varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
ret.segment(varStart, values[*j].rows()) = values[*j];
varStart += values[*j].rows();
}
return ret;
}
/* ************************************************************************* */
// Helper function, writes to the variables in values
// with indices iterated over by first and last, interpreting vector as the
// concatenated vectors to write.
template<class VECTOR, class VALUES, typename ITERATOR>
void writeVectorValuesSlices(const VECTOR& vector, VALUES& values, ITERATOR first, ITERATOR last) {
// Copy vectors
int varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
values[*j] = vector.segment(varStart, values[*j].rows());
varStart += values[*j].rows();
}
assert(varStart == vector.rows());
}
}
} // \namespace gtsam

29
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -135,12 +135,18 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
const F& f, const VALUES& x0, const Ordering& ordering, const double f_error, const bool verbose) {
// Compute steepest descent and Newton's method points
tic(0, "Steepest Descent");
VectorValues dx_u = optimizeGradientSearch(Rd);
toc(0, "Steepest Descent");
tic(1, "optimize");
VectorValues dx_n = optimize(Rd);
toc(1, "optimize");
tic(0, "optimizeGradientSearch");
tic(0, "allocateVectorValues");
VectorValues dx_u = *allocateVectorValues(Rd);
toc(0, "allocateVectorValues");
tic(1, "optimizeGradientSearchInPlace");
optimizeGradientSearchInPlace(Rd, dx_u);
toc(1, "optimizeGradientSearchInPlace");
toc(0, "optimizeGradientSearch");
tic(1, "optimizeInPlace");
VectorValues dx_n(VectorValues::SameStructure(dx_u));
optimizeInPlace(Rd, dx_n);
toc(1, "optimizeInPlace");
tic(2, "jfg error");
const GaussianFactorGraph jfg(Rd);
const double M_error = jfg.error(VectorValues::Zero(dx_u));
@ -177,6 +183,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
if(verbose) cout << "f error: " << f_error << " -> " << result.f_error << endl;
if(verbose) cout << "M error: " << M_error << " -> " << new_M_error << endl;
tic(7, "adjust Delta");
// Compute gain ratio. Here we take advantage of the invariant that the
// Bayes' net error at zero is equal to the nonlinear error
const double rho = fabs(M_error - new_M_error) < 1e-15 ?
@ -186,7 +193,6 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
if(verbose) cout << "rho = " << rho << endl;
if(rho >= 0.75) {
tic(7, "Rho >= 0.75");
// M agrees very well with f, so try to increase lambda
const double dx_d_norm = result.dx_d.vector().norm();
const double newDelta = std::max(Delta, 3.0 * dx_d_norm); // Compute new Delta
@ -204,14 +210,12 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
assert(false); }
Delta = newDelta; // Update Delta from new Delta
toc(7, "Rho >= 0.75");
} else if(0.75 > rho && rho >= 0.25) {
// M agrees so-so with f, keep the same Delta
stay = false;
} else if(0.25 > rho && rho >= 0.0) {
tic(8, "0.25 > Rho >= 0.75");
// M does not agree well with f, decrease Delta until it does
double newDelta;
if(Delta > 1e-5)
@ -227,11 +231,8 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
assert(false); }
Delta = newDelta; // Update Delta from new Delta
toc(8, "0.25 > Rho >= 0.75");
}
else {
tic(9, "Rho < 0");
} else {
// f actually increased, so keep decreasing Delta until f does not decrease
assert(0.0 > rho);
if(Delta > 1e-5) {
@ -242,8 +243,8 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
if(verbose) cout << "Warning: Dog leg stopping because cannot decrease error with minimum Delta" << endl;
stay = false;
}
toc(9, "Rho < 0");
}
toc(7, "adjust Delta");
}
// dx_d and f_error have already been filled in during the loop

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

@ -1,176 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file GaussianISAM2
* @brief Full non-linear ISAM
* @author Michael Kaess
*/
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <boost/bind.hpp>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
namespace internal {
template<class CLIQUE>
void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
vector<bool>& changed, const vector<bool>& replaced, Permuted<VectorValues>& delta, int& count) {
// if none of the variables in this clique (frontal and separator!) changed
// significantly, then by the running intersection property, none of the
// cliques in the children need to be processed
// Are any clique variables part of the tree that has been redone?
bool cliqueReplaced = replaced[(*clique)->frontals().front()];
#ifndef NDEBUG
BOOST_FOREACH(Index frontal, (*clique)->frontals()) {
assert(cliqueReplaced == replaced[frontal]);
}
#endif
// If not redone, then has one of the separator variables changed significantly?
bool recalculate = cliqueReplaced;
if(!recalculate) {
BOOST_FOREACH(Index parent, (*clique)->parents()) {
if(changed[parent]) {
recalculate = true;
break;
}
}
}
// Solve clique if it was replaced, or if any parents were changed above the
// threshold or themselves replaced.
if(recalculate) {
// Temporary copy of the original values, to check how much they change
vector<Vector> originalValues((*clique)->nrFrontals());
GaussianConditional::const_iterator it;
for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
originalValues[it - (*clique)->beginFrontals()] = delta[*it];
}
// Back-substitute
(*clique)->solveInPlace(delta);
count += (*clique)->nrFrontals();
// Whether the values changed above a threshold, or always true if the
// clique was replaced.
bool valuesChanged = cliqueReplaced;
for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
if(!valuesChanged) {
const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]);
const SubVector& newValue(delta[*it]);
if((oldValue - newValue).lpNorm<Eigen::Infinity>() >= threshold) {
valuesChanged = true;
break;
}
} else
break;
}
// If the values were above the threshold or this clique was replaced
if(valuesChanged) {
// Set changed flag for each frontal variable and leave the new values
BOOST_FOREACH(Index frontal, (*clique)->frontals()) {
changed[frontal] = true;
}
} else {
// Replace with the old values
for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
delta[*it] = originalValues[it - (*clique)->beginFrontals()];
}
}
// Recurse to children
BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) {
optimizeWildfire(child, threshold, changed, replaced, delta, count);
}
}
}
}
/* ************************************************************************* */
template<class CLIQUE>
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const vector<bool>& keys, Permuted<VectorValues>& delta) {
vector<bool> changed(keys.size(), false);
int count = 0;
// starting from the root, call optimize on each conditional
if(root)
internal::optimizeWildfire(root, threshold, changed, keys, delta, count);
return count;
}
/* ************************************************************************* */
template<class GRAPH>
VectorValues optimizeGradientSearch(const ISAM2<GaussianConditional, GRAPH>& isam) {
tic(0, "Allocate VectorValues");
VectorValues grad = *allocateVectorValues(isam);
toc(0, "Allocate VectorValues");
optimizeGradientSearchInPlace(isam, grad);
return grad;
}
/* ************************************************************************* */
template<class GRAPH>
void optimizeGradientSearchInPlace(const ISAM2<GaussianConditional, GRAPH>& Rd, VectorValues& grad) {
tic(1, "Compute Gradient");
// Compute gradient (call gradientAtZero function, which is defined for various linear systems)
gradientAtZero(Rd, grad);
double gradientSqNorm = grad.dot(grad);
toc(1, "Compute Gradient");
tic(2, "Compute R*g");
// Compute R * g
FactorGraph<JacobianFactor> Rd_jfg(Rd);
Errors Rg = Rd_jfg * grad;
toc(2, "Compute R*g");
tic(3, "Compute minimizing step size");
// Compute minimizing step size
double step = -gradientSqNorm / dot(Rg, Rg);
toc(3, "Compute minimizing step size");
tic(4, "Compute point");
// Compute steepest descent point
scal(step, grad);
toc(4, "Compute point");
}
/* ************************************************************************* */
template<class CLIQUE>
void nnz_internal(const boost::shared_ptr<CLIQUE>& clique, int& result) {
int dimR = (*clique)->dim();
int dimSep = (*clique)->get_S().cols() - dimR;
result += ((dimR+1)*dimR)/2 + dimSep*dimR;
// traverse the children
BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) {
nnz_internal(child, result);
}
}
/* ************************************************************************* */
template<class CLIQUE>
int calculate_nnz(const boost::shared_ptr<CLIQUE>& clique) {
int result = 0;
// starting from the root, add up entries of frontal and conditional matrices of each conditional
nnz_internal(clique, result);
return result;
}
} /// namespace gtsam

60
gtsam/nonlinear/GaussianISAM2.cpp
View File

@ -1,60 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file GaussianISAM2
* @brief Full non-linear ISAM
* @author Michael Kaess
*/
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/GaussianISAM2.h>
using namespace std;
using namespace gtsam;
#include <boost/bind.hpp>
namespace gtsam {
/* ************************************************************************* */
VectorValues gradient(const BayesTree<GaussianConditional, ISAM2Clique<GaussianConditional> >& bayesTree, const VectorValues& x0) {
return gradient(FactorGraph<JacobianFactor>(bayesTree), x0);
}
/* ************************************************************************* */
static void gradientAtZeroTreeAdder(const boost::shared_ptr<ISAM2Clique<GaussianConditional> >& root, VectorValues& g) {
// Loop through variables in each clique, adding contributions
int variablePosition = 0;
for(GaussianConditional::const_iterator jit = root->conditional()->begin(); jit != root->conditional()->end(); ++jit) {
const int dim = root->conditional()->dim(jit);
g[*jit] += root->gradientContribution().segment(variablePosition, dim);
variablePosition += dim;
}
// Recursively add contributions from children
typedef boost::shared_ptr<ISAM2Clique<GaussianConditional> > sharedClique;
BOOST_FOREACH(const sharedClique& child, root->children()) {
gradientAtZeroTreeAdder(child, g);
}
}
/* ************************************************************************* */
void gradientAtZero(const BayesTree<GaussianConditional, ISAM2Clique<GaussianConditional> >& bayesTree, VectorValues& g) {
// Zero-out gradient
g.setZero();
// Sum up contributions for each clique
gradientAtZeroTreeAdder(bayesTree.root(), g);
}
}

153
gtsam/nonlinear/GaussianISAM2.h
View File

@ -1,153 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file GaussianISAM
* @brief Full non-linear ISAM.
* @author Michael Kaess
*/
// \callgraph
#pragma once
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/nonlinear/ISAM2.h>
namespace gtsam {
/**
* @ingroup ISAM2
* @brief The main ISAM2 class that is exposed to gtsam users, see ISAM2 for usage.
*
* This is a thin wrapper around an ISAM2 class templated on
* GaussianConditional, and the values on which that GaussianISAM2 is
* templated.
*
* @tparam VALUES The Values or TupleValues\Emph{N} that contains the
* variables.
* @tparam GRAPH The NonlinearFactorGraph structure to store factors. Defaults to standard NonlinearFactorGraph<VALUES>
*/
template <class GRAPH = NonlinearFactorGraph>
class GaussianISAM2 : public ISAM2<GaussianConditional, GRAPH> {
typedef ISAM2<GaussianConditional, GRAPH> Base;
public:
/// @name Standard Constructors
/// @{
/** Create an empty ISAM2 instance */
GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, GRAPH>(params) {}
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
GaussianISAM2() : ISAM2<GaussianConditional, GRAPH>() {}
/// @}
/// @name Advanced Interface
/// @{
void cloneTo(boost::shared_ptr<GaussianISAM2>& newGaussianISAM2) const {
boost::shared_ptr<Base> isam2 = boost::static_pointer_cast<Base>(newGaussianISAM2);
Base::cloneTo(isam2);
}
/// @}
};
/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
template<class GRAPH>
VectorValues optimize(const ISAM2<GaussianConditional, GRAPH>& isam) {
VectorValues delta = *allocateVectorValues(isam);
internal::optimizeInPlace(isam.root(), delta);
return delta;
}
/// Optimize the BayesTree, starting from the root.
/// @param replaced Needs to contain
/// all variables that are contained in the top of the Bayes tree that has been
/// redone.
/// @param delta The current solution, an offset from the linearization
/// point.
/// @param threshold The maximum change against the PREVIOUS delta for
/// non-replaced variables that can be ignored, ie. the old delta entry is kept
/// and recursive backsubstitution might eventually stop if none of the changed
/// variables are contained in the subtree.
/// @return The number of variables that were solved for
template<class CLIQUE>
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root,
double threshold, const std::vector<bool>& replaced, Permuted<VectorValues>& delta);
/**
* Optimize along the gradient direction, with a closed-form computation to
* perform the line search. The gradient is computed about \f$ \delta x=0 \f$.
*
* This function returns \f$ \delta x \f$ that minimizes a reparametrized
* problem. The error function of a GaussianBayesNet is
*
* \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f]
*
* with gradient and Hessian
*
* \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f]
*
* This function performs the line search in the direction of the
* gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size
* \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$:
*
* \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f]
*
* Optimizing by setting the derivative to zero yields
* \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function
* evaluates the denominator without computing the Hessian \f$ G \f$, returning
*
* \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f]
*/
template<class GRAPH>
VectorValues optimizeGradientSearch(const ISAM2<GaussianConditional, GRAPH>& isam);
/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */
template<class GRAPH>
void optimizeGradientSearchInPlace(const ISAM2<GaussianConditional, GRAPH>& isam, VectorValues& grad);
/// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)
template<class CLIQUE>
int calculate_nnz(const boost::shared_ptr<CLIQUE>& clique);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$,
* centered around \f$ x = x_0 \f$.
* The gradient is \f$ R^T(Rx-d) \f$.
* This specialized version is used with ISAM2, where each clique stores its
* gradient contribution.
* @param bayesTree The Gaussian Bayes Tree $(R,d)$
* @param x0 The center about which to compute the gradient
* @return The gradient as a VectorValues
*/
VectorValues gradient(const BayesTree<GaussianConditional, ISAM2Clique<GaussianConditional> >& bayesTree, const VectorValues& x0);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$,
* centered around zero.
* The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&).
* This specialized version is used with ISAM2, where each clique stores its
* gradient contribution.
* @param bayesTree The Gaussian Bayes Tree $(R,d)$
* @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
* @return The gradient as a VectorValues
*/
void gradientAtZero(const BayesTree<GaussianConditional, ISAM2Clique<GaussianConditional> >& bayesTree, VectorValues& g);
}/// namespace gtsam
#include <gtsam/nonlinear/GaussianISAM2-inl.h>

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

@ -10,128 +10,21 @@
* -------------------------------------------------------------------------- */
/**
* @file ISAM2-impl-inl.h
* @file ISAM2-impl.cpp
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
* @author Michael Kaess, Richard Roberts
*/
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/nonlinear/ISAM2-impl.h>
#include <gtsam/base/debug.h>
namespace gtsam {
using namespace std;
template<class CONDITIONAL, class GRAPH>
struct ISAM2<CONDITIONAL, GRAPH>::Impl {
typedef ISAM2<CONDITIONAL, GRAPH> ISAM2Type;
struct PartialSolveResult {
typename ISAM2Type::sharedClique bayesTree;
Permutation fullReordering;
Permutation fullReorderingInverse;
};
struct ReorderingMode {
size_t nFullSystemVars;
enum { /*AS_ADDED,*/ COLAMD } algorithm;
enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain;
boost::optional<const FastSet<Index>&> constrainedKeys;
};
/**
* Add new variables to the ISAM2 system.
* @param newTheta Initial values for new variables
* @param theta Current solution to be augmented with new initialization
* @param delta Current linear delta to be augmented with zeros
* @param ordering Current ordering to be augmented with new variables
* @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
* @param keyFormatter Formatter for printing nonlinear keys during debugging
*/
static void AddVariables(const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, vector<bool>& replacedKeys,
Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
* Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol
* in each NonlinearFactor, obtains the index by calling ordering[symbol].
* @param ordering The current ordering from which to obtain the variable indices
* @param factors The factors from which to extract the variables
* @return The set of variables indices from the factors
*/
static FastSet<Index> IndicesFromFactors(const Ordering& ordering, const GRAPH& factors);
/**
* Find the set of variables to be relinearized according to relinearizeThreshold.
* Any variables in the VectorValues delta whose vector magnitude is greater than
* or equal to relinearizeThreshold are returned.
* @param delta The linear delta to check against the threshold
* @param keyFormatter Formatter for printing nonlinear keys during debugging
* @return The set of variable indices in delta whose magnitude is greater than or
* equal to relinearizeThreshold
*/
static FastSet<Index> CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
* Recursively search this clique and its children for marked keys appearing
* in the separator, and add the *frontal* keys of any cliques whose
* separator contains any marked keys to the set \c keys. The purpose of
* this is to discover the cliques that need to be redone due to information
* propagating to them from cliques that directly contain factors being
* relinearized.
*
* The original comment says this finds all variables directly connected to
* the marked ones by measurements. Is this true, because it seems like this
* would also pull in variables indirectly connected through other frontal or
* separator variables?
*
* Alternatively could we trace up towards the root for each variable here?
*/
static void FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask);
/**
* Apply expmap to the given values, but only for indices appearing in
* \c markedRelinMask. Values are expmapped in-place.
* \param [in][out] values The value to expmap in-place
* \param delta The linear delta with which to expmap
* \param ordering The ordering
* \param mask Mask on linear indices, only \c true entries are expmapped
* \param invalidateIfDebug If this is true, *and* NDEBUG is not defined,
* expmapped deltas will be set to an invalid value (infinity) to catch bugs
* where we might expmap something twice, or expmap it but then not
* recalculate its delta.
* @param keyFormatter Formatter for printing nonlinear keys during debugging
*/
static void ExpmapMasked(Values& values, const Permuted<VectorValues>& delta,
const Ordering& ordering, const std::vector<bool>& mask,
boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>(),
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
* Reorder and eliminate factors. These factors form a subset of the full
* problem, so along with the BayesTree we get a partial reordering of the
* problem that needs to be applied to the other data in ISAM2, which is the
* VariableIndex, the delta, the ordering, and the orphans (including cached
* factors).
* \param factors The factors to eliminate, representing part of the full
* problem. This is permuted during use and so is cleared when this function
* returns in order to invalidate it.
* \param keys The set of indices used in \c factors.
* \return The eliminated BayesTree and the permutation to be applied to the
* rest of the ISAM2 data.
*/
static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet<Index>& keys,
const ReorderingMode& reorderingMode);
static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique<CONDITIONAL> >& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold);
};
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, vector<bool>& replacedKeys,
Ordering& ordering,typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
void ISAM2::Impl::AddVariables(
const Values& newTheta, Values& theta, Permuted<VectorValues>& delta,
Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& deltaGradSearch, vector<bool>& replacedKeys,
Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
const bool debug = ISDEBUG("ISAM2 AddVariables");
theta.insert(newTheta);
@ -145,10 +38,18 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
delta.container().append(dims);
delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
delta.permutation().resize(originalnVars + newTheta.size());
deltaNewton.container().append(dims);
deltaNewton.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
deltaNewton.permutation().resize(originalnVars + newTheta.size());
deltaGradSearch.container().append(dims);
deltaGradSearch.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
deltaGradSearch.permutation().resize(originalnVars + newTheta.size());
{
Index nextVar = originalnVars;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
delta.permutation()[nextVar] = nextVar;
deltaNewton.permutation()[nextVar] = nextVar;
deltaGradSearch.permutation()[nextVar] = nextVar;
ordering.insert(key_value.key, nextVar);
if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl;
++ nextVar;
@ -163,10 +64,9 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
FastSet<Index> ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors) {
FastSet<Index> indices;
BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) {
BOOST_FOREACH(Key key, factor->keys()) {
indices.insert(ordering[key]);
}
@ -175,8 +75,7 @@ FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::IndicesFromFactors(const Ordering
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering,
FastSet<Index> ISAM2::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
FastSet<Index> relinKeys;
@ -204,8 +103,7 @@ FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::CheckRelinearization(const Permut
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
static const bool debug = false;
// does the separator contain any of the variables?
bool found = false;
@ -219,14 +117,13 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::
if(debug) clique->print("Key(s) marked in clique ");
if(debug) cout << "so marking key " << (*clique)->keys().front() << endl;
}
BOOST_FOREACH(const typename ISAM2Clique<CONDITIONAL>::shared_ptr& child, clique->children_) {
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children_) {
FindAll(child, keys, markedMask);
}
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta, const Ordering& ordering,
void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta, const Ordering& ordering,
const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug, const KeyFormatter& keyFormatter) {
// If debugging, invalidate if requested, otherwise do not invalidate.
// Invalidating means setting expmapped entries to Inf, to trigger assertions
@ -262,9 +159,8 @@ void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permute
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
typename ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolveResult
ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
ISAM2::Impl::PartialSolveResult
ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
const FastSet<Index>& keys, const ReorderingMode& reorderingMode) {
static const bool debug = ISDEBUG("ISAM2 recalculate");
@ -340,14 +236,8 @@ ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
// eliminate into a Bayes net
tic(7,"eliminate");
JunctionTree<GaussianFactorGraph, typename ISAM2Type::Clique> jt(factors, affectedFactorsIndex);
JunctionTree<GaussianFactorGraph, ISAM2::Clique> jt(factors, affectedFactorsIndex);
result.bayesTree = jt.eliminate(EliminatePreferLDL);
if(debug && result.bayesTree) {
if(boost::dynamic_pointer_cast<ISAM2Clique<CONDITIONAL> >(result.bayesTree))
cout << "Is an ISAM2 clique" << endl;
cout << "Re-eliminated BT:\n";
result.bayesTree->printTree("");
}
toc(7,"eliminate");
tic(8,"permute eliminated");
@ -363,19 +253,18 @@ ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
/* ************************************************************************* */
namespace internal {
inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique<GaussianConditional> >& clique, VectorValues& result) {
inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique>& clique, VectorValues& result) {
// parents are assumed to already be solved and available in result
clique->conditional()->solveInPlace(result);
// starting from the root, call optimize on each conditional
BOOST_FOREACH(const boost::shared_ptr<ISAM2Clique<GaussianConditional> >& child, clique->children_)
BOOST_FOREACH(const boost::shared_ptr<ISAM2Clique>& child, clique->children_)
optimizeInPlace(child, result);
}
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
size_t ISAM2<CONDITIONAL,GRAPH>::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique<CONDITIONAL> >& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold) {
size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold) {
size_t lastBacksubVariableCount;
@ -412,4 +301,76 @@ size_t ISAM2<CONDITIONAL,GRAPH>::Impl::UpdateDelta(const boost::shared_ptr<ISAM2
return lastBacksubVariableCount;
}
/* ************************************************************************* */
namespace internal {
void updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, std::vector<bool>& replacedKeys,
const VectorValues& grad, Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd, size_t& varsUpdated) {
// Check if any frontal or separator keys were recalculated, if so, we need
// update deltas and recurse to children, but if not, we do not need to
// recurse further because of the running separator property.
bool anyReplaced = false;
BOOST_FOREACH(Index j, *clique->conditional()) {
if(replacedKeys[j]) {
anyReplaced = true;
break;
}
}
if(anyReplaced) {
// Update the current variable
// Get VectorValues slice corresponding to current variables
Vector gR = internal::extractVectorValuesSlices(grad, (*clique)->beginFrontals(), (*clique)->endFrontals());
Vector gS = internal::extractVectorValuesSlices(grad, (*clique)->beginParents(), (*clique)->endParents());
// Compute R*g and S*g for this clique
Vector RSgProd = ((*clique)->get_R() * (*clique)->permutation().transpose()) * gR + (*clique)->get_S() * gS;
// Write into RgProd vector
internal::writeVectorValuesSlices(RSgProd, RgProd, (*clique)->beginFrontals(), (*clique)->endFrontals());
// Now solve the part of the Newton's method point for this clique (back-substitution)
//(*clique)->solveInPlace(deltaNewton);
varsUpdated += (*clique)->nrFrontals();
// Recurse to children
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) {
updateDoglegDeltas(child, replacedKeys, grad, deltaNewton, RgProd, varsUpdated); }
}
}
}
/* ************************************************************************* */
size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector<bool>& replacedKeys,
Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd) {
// Get gradient
VectorValues grad = *allocateVectorValues(isam);
gradientAtZero(isam, grad);
// Update variables
size_t varsUpdated = 0;
internal::updateDoglegDeltas(isam.root(), replacedKeys, grad, deltaNewton, RgProd, varsUpdated);
optimizeWildfire(isam.root(), wildfireThreshold, replacedKeys, deltaNewton);
#if 0
VectorValues expected = *allocateVectorValues(isam);
internal::optimizeInPlace<ISAM2>(isam.root(), expected);
for(size_t j = 0; j<expected.size(); ++j)
assert(equal_with_abs_tol(expected[j], deltaNewton[j], 1e-2));
FactorGraph<JacobianFactor> Rd_jfg(isam);
Errors Rg = Rd_jfg * grad;
double RgMagExpected = dot(Rg, Rg);
double RgMagActual = RgProd.container().vector().squaredNorm();
cout << fabs(RgMagExpected - RgMagActual) << endl;
assert(fabs(RgMagExpected - RgMagActual) < (1e-8 * RgMagActual + 1e-4));
#endif
replacedKeys.assign(replacedKeys.size(), false);
return varsUpdated;
}
}

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

@ -0,0 +1,134 @@
/* ----------------------------------------------------------------------------
* 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
*/
#pragma once
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/nonlinear/ISAM2.h>
namespace gtsam {
using namespace std;
struct ISAM2::Impl {
struct PartialSolveResult {
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,
Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& deltaGradSearch, vector<bool>& replacedKeys,
Ordering& ordering, 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 NonlinearFactorGraph& 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(ISAM2Clique::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>& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold);
static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector<bool>& replacedKeys,
Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd);
};
}

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

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -10,624 +10,144 @@
* -------------------------------------------------------------------------- */
/**
* @file ISAM2-inl.h
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
* @author Michael Kaess, Richard Roberts
* @file ISAM2-inl.h
* @brief
* @author Richard Roberts
* @date Mar 16, 2012
*/
#pragma once
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <gtsam/base/timing.h>
#include <gtsam/base/debug.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/nonlinear/ISAM2-impl-inl.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <boost/bind.hpp>
namespace gtsam {
using namespace std;
static const bool disableReordering = false;
static const double batchThreshold = 0.65;
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
ISAM2<CONDITIONAL, GRAPH>::ISAM2(const ISAM2Params& params):
delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true), params_(params) {
// See note in gtsam/base/boost_variant_with_workaround.h
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
ISAM2<CONDITIONAL, GRAPH>::ISAM2():
delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true) {
// See note in gtsam/base/boost_variant_with_workaround.h
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
}
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
FastList<size_t> ISAM2<CONDITIONAL, GRAPH>::getAffectedFactors(const FastList<Index>& keys) const {
static const bool debug = false;
if(debug) cout << "Getting affected factors for ";
if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } }
if(debug) cout << endl;
FactorGraph<NonlinearFactor > allAffected;
FastList<size_t> indices;
BOOST_FOREACH(const Index key, keys) {
// const list<size_t> l = nonlinearFactors_.factors(key);
// indices.insert(indices.begin(), l.begin(), l.end());
const VariableIndex::Factors& factors(variableIndex_[key]);
BOOST_FOREACH(size_t factor, factors) {
if(debug) cout << "Variable " << key << " affects factor " << factor << endl;
indices.push_back(factor);
}
}
indices.sort();
indices.unique();
if(debug) cout << "Affected factors are: ";
if(debug) { BOOST_FOREACH(const size_t index, indices) { cout << index << " "; } }
if(debug) cout << endl;
return indices;
}
/* ************************************************************************* */
// retrieve all factors that ONLY contain the affected variables
// (note that the remaining stuff is summarized in the cached factors)
template<class CONDITIONAL, class GRAPH>
FactorGraph<GaussianFactor>::shared_ptr
ISAM2<CONDITIONAL, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
tic(1,"getAffectedFactors");
FastList<size_t> candidates = getAffectedFactors(affectedKeys);
toc(1,"getAffectedFactors");
GRAPH nonlinearAffectedFactors;
tic(2,"affectedKeysSet");
// for fast lookup below
FastSet<Index> affectedKeysSet;
affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end());
toc(2,"affectedKeysSet");
tic(3,"check candidates");
BOOST_FOREACH(size_t idx, candidates) {
bool inside = true;
BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) {
Index var = ordering_[key];
if (affectedKeysSet.find(var) == affectedKeysSet.end()) {
inside = false;
break;
}
}
if (inside)
nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]);
}
toc(3,"check candidates");
tic(4,"linearize");
FactorGraph<GaussianFactor>::shared_ptr linearized(nonlinearAffectedFactors.linearize(theta_, ordering_));
toc(4,"linearize");
return linearized;
}
/* ************************************************************************* */
// find intermediate (linearized) factors from cache that are passed into the affected area
template<class CONDITIONAL, class GRAPH>
FactorGraph<typename ISAM2<CONDITIONAL, GRAPH>::CacheFactor>
ISAM2<CONDITIONAL, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
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;
}
template<class CONDITIONAL, class GRAPH>
boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
const FastSet<Index>& markedKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
const boost::optional<FastSet<Index> >& constrainKeys, ISAM2Result& result) {
// TODO: new factors are linearized twice, the newFactors passed in are not used.
static const bool debug = ISDEBUG("ISAM2 recalculate");
// Input: BayesTree(this), newFactors
//#define PRINT_STATS // figures for paper, disable for timing
#ifdef PRINT_STATS
static int counter = 0;
int maxClique = 0;
double avgClique = 0;
int numCliques = 0;
int nnzR = 0;
if (counter>0) { // cannot call on empty tree
GaussianISAM2_P::CliqueData cdata = this->getCliqueData();
GaussianISAM2_P::CliqueStats cstats = cdata.getStats();
maxClique = cstats.maxCONDITIONALSize;
avgClique = cstats.avgCONDITIONALSize;
numCliques = cdata.conditionalSizes.size();
nnzR = calculate_nnz(this->root());
}
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>
ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
const GRAPH& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices,
const boost::optional<FastSet<Key> >& constrainedKeys, bool force_relinearize) {
static const bool debug = ISDEBUG("ISAM2 update");
static const bool verbose = ISDEBUG("ISAM2 update verbose");
static int count = 0;
count++;
lastAffectedVariableCount = 0;
lastAffectedFactorCount = 0;
lastAffectedCliqueCount = 0;
lastAffectedMarkedCount = 0;
lastBacksubVariableCount = 0;
lastNnzTop = 0;
ISAM2Result result;
const bool relinearizeThisStep = force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0);
if(verbose) {
cout << "ISAM2::update\n";
this->print("ISAM2: ");
}
// 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;
}
/* ************************************************************************* */
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 {
VALUE ISAM2::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_);
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 CONDITIONAL, class GRAPH>
const Permuted<VectorValues>& ISAM2<CONDITIONAL, GRAPH>::getDelta() const {
if(!deltaUptodate_)
updateDelta();
return delta_;
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 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

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

@ -0,0 +1,732 @@
/* ----------------------------------------------------------------------------
* 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/DoglegOptimizerImpl.h>
namespace gtsam {
using namespace std;
static const bool disableReordering = false;
static const double batchThreshold = 0.65;
/* ************************************************************************* */
ISAM2::ISAM2(const ISAM2Params& params):
delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_),
deltaDoglegUptodate_(true), 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_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_),
deltaDoglegUptodate_(true), 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<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);
deltaNewton_.permute(*colamd);
RgProd_.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, 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");
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());
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);
deltaNewton_.permute(partialSolveResult.fullReordering);
RgProd_.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_, deltaNewton_, RgProd_, 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();
deltaDoglegUptodate_ = false;
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");
tic(2, "Copy dx_d");
// 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
toc(2, "Copy dx_d");
}
deltaUptodate_ = true;
}
/* ************************************************************************* */
Values ISAM2::calculateEstimate() const {
// We use ExpmapMasked here instead of regular expmap because the former
// handles Permuted<VectorValues>
tic(1, "Copy Values");
Values ret(theta_);
toc(1, "Copy Values");
tic(2, "getDelta");
const Permuted<VectorValues>& delta(getDelta());
toc(2, "getDelta");
tic(3, "Expmap");
vector<bool> mask(ordering_.nVars(), true);
Impl::ExpmapMasked(ret, delta, ordering_, mask);
toc(3, "Expmap");
return ret;
}
/* ************************************************************************* */
Values ISAM2::calculateBestEstimate() const {
VectorValues delta(theta_.dims(ordering_));
internal::optimizeInPlace<Base>(this->root(), delta);
return theta_.retract(delta, ordering_);
}
/* ************************************************************************* */
const Permuted<VectorValues>& ISAM2::getDelta() const {
if(!deltaUptodate_)
updateDelta();
return delta_;
}
/* ************************************************************************* */
VectorValues optimize(const ISAM2& isam) {
tic(0, "allocateVectorValues");
VectorValues delta = *allocateVectorValues(isam);
toc(0, "allocateVectorValues");
optimizeInPlace(isam, delta);
return delta;
}
/* ************************************************************************* */
void optimizeInPlace(const ISAM2& isam, VectorValues& delta) {
// We may need to update the solution calcaulations
if(!isam.deltaDoglegUptodate_) {
tic(1, "UpdateDoglegDeltas");
double wildfireThreshold = 0.0;
if(isam.params().optimizationParams.type() == typeid(ISAM2GaussNewtonParams))
wildfireThreshold = boost::get<ISAM2GaussNewtonParams>(isam.params().optimizationParams).wildfireThreshold;
else if(isam.params().optimizationParams.type() == typeid(ISAM2DoglegParams))
wildfireThreshold = boost::get<ISAM2DoglegParams>(isam.params().optimizationParams).wildfireThreshold;
else
assert(false);
ISAM2::Impl::UpdateDoglegDeltas(isam, wildfireThreshold, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_);
isam.deltaDoglegUptodate_ = true;
toc(1, "UpdateDoglegDeltas");
}
tic(2, "copy delta");
delta = isam.deltaNewton_;
toc(2, "copy 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& isam, VectorValues& grad) {
// We may need to update the solution calcaulations
if(!isam.deltaDoglegUptodate_) {
tic(1, "UpdateDoglegDeltas");
double wildfireThreshold = 0.0;
if(isam.params().optimizationParams.type() == typeid(ISAM2GaussNewtonParams))
wildfireThreshold = boost::get<ISAM2GaussNewtonParams>(isam.params().optimizationParams).wildfireThreshold;
else if(isam.params().optimizationParams.type() == typeid(ISAM2DoglegParams))
wildfireThreshold = boost::get<ISAM2DoglegParams>(isam.params().optimizationParams).wildfireThreshold;
else
assert(false);
ISAM2::Impl::UpdateDoglegDeltas(isam, wildfireThreshold, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_);
isam.deltaDoglegUptodate_ = true;
toc(1, "UpdateDoglegDeltas");
}
tic(2, "Compute Gradient");
// Compute gradient (call gradientAtZero function, which is defined for various linear systems)
gradientAtZero(isam, grad);
double gradientSqNorm = grad.dot(grad);
toc(2, "Compute Gradient");
tic(3, "Compute minimizing step size");
// Compute minimizing step size
double RgNormSq = isam.RgProd_.container().vector().squaredNorm();
double step = -gradientSqNorm / RgNormSq;
toc(3, "Compute minimizing step size");
tic(4, "Compute point");
// Compute steepest descent point
grad.vector() *= step;
toc(4, "Compute point");
}
/* ************************************************************************* */
VectorValues gradient(const ISAM2& 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 ISAM2& bayesTree, VectorValues& g) {
// Zero-out gradient
g.setZero();
// Sum up contributions for each clique
gradientAtZeroTreeAdder(bayesTree.root(), g);
}
}
/// namespace gtsam

153
gtsam/nonlinear/ISAM2.h
View File

@ -12,7 +12,7 @@
/**
* @file ISAM2.h
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
* @author Michael Kaess
* @author Michael Kaess, Richard Roberts
*/
// \callgraph
@ -21,10 +21,8 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/linear/GaussianBayesTree.h>
// Workaround for boost < 1.47, see note in file
//#include <gtsam/base/boost_variant_with_workaround.h>
#include <boost/variant.hpp>
namespace gtsam {
@ -56,15 +54,18 @@ struct ISAM2GaussNewtonParams {
*/
struct ISAM2DoglegParams {
double initialDelta; ///< The initial trust region radius for Dogleg
double wildfireThreshold; ///< Continue updating the linear delta only when changes are above this threshold (default: 0.001)
DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationMode; ///< See description in DoglegOptimizerImpl::TrustRegionAdaptationMode
bool verbose; ///< Whether Dogleg prints iteration and convergence information
/** Specify parameters as constructor arguments */
ISAM2DoglegParams(
double _initialDelta = 1.0, ///< see ISAM2DoglegParams public variables, ISAM2DoglegParams::initialDelta
DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = DoglegOptimizerImpl::SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams public variables, ISAM2DoglegParams::adaptationMode
bool _verbose = false ///< see ISAM2DoglegParams public variables, ISAM2DoglegParams::verbose
) : initialDelta(_initialDelta), adaptationMode(_adaptationMode), verbose(_verbose) {}
double _initialDelta = 1.0, ///< see ISAM2DoglegParams::initialDelta
double _wildfireThreshold = 1e-5, ///< see ISAM2DoglegParams::wildfireThreshold
DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = DoglegOptimizerImpl::SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams::adaptationMode
bool _verbose = false ///< see ISAM2DoglegParams::verbose
) : initialDelta(_initialDelta), wildfireThreshold(_wildfireThreshold),
adaptationMode(_adaptationMode), verbose(_verbose) {}
};
/**
@ -181,17 +182,16 @@ struct ISAM2Result {
FastVector<size_t> newFactorsIndices;
};
template<class CONDITIONAL>
struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique<CONDITIONAL>, CONDITIONAL> {
struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique, GaussianConditional> {
typedef ISAM2Clique<CONDITIONAL> This;
typedef BayesTreeCliqueBase<This,CONDITIONAL> Base;
typedef ISAM2Clique This;
typedef BayesTreeCliqueBase<This,GaussianConditional> Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
typedef CONDITIONAL ConditionalType;
typedef typename ConditionalType::shared_ptr sharedConditional;
typedef GaussianConditional ConditionalType;
typedef ConditionalType::shared_ptr sharedConditional;
typename Base::FactorType::shared_ptr cachedFactor_;
Base::FactorType::shared_ptr cachedFactor_;
Vector gradientContribution_;
/** Construct from a conditional */
@ -199,7 +199,7 @@ struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique<CONDITIONAL>, CONDIT
throw runtime_error("ISAM2Clique should always be constructed with the elimination result constructor"); }
/** Construct from an elimination result */
ISAM2Clique(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result) :
ISAM2Clique(const std::pair<sharedConditional, boost::shared_ptr<ConditionalType::FactorType> >& result) :
Base(result.first), cachedFactor_(result.second), gradientContribution_(result.first->get_R().cols() + result.first->get_S().cols()) {
// Compute gradient contribution
const ConditionalType& conditional(*result.first);
@ -211,13 +211,13 @@ struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique<CONDITIONAL>, CONDIT
shared_ptr clone() const {
shared_ptr copy(new ISAM2Clique(make_pair(
sharedConditional(new ConditionalType(*Base::conditional_)),
cachedFactor_ ? cachedFactor_->clone() : typename Base::FactorType::shared_ptr())));
cachedFactor_ ? cachedFactor_->clone() : Base::FactorType::shared_ptr())));
copy->gradientContribution_ = gradientContribution_;
return copy;
}
/** Access the cached factor */
typename Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
/** Access the gradient contribution */
const Vector& gradientContribution() const { return gradientContribution_; }
@ -269,8 +269,7 @@ private:
* estimate of all variables.
*
*/
template<class CONDITIONAL, class GRAPH = NonlinearFactorGraph>
class ISAM2: public BayesTree<CONDITIONAL, ISAM2Clique<CONDITIONAL> > {
class ISAM2: public BayesTree<GaussianConditional, ISAM2Clique> {
protected:
@ -296,6 +295,12 @@ protected:
*/
mutable Permuted<VectorValues> delta_;
VectorValues deltaNewtonUnpermuted_;
mutable Permuted<VectorValues> deltaNewton_;
VectorValues RgProdUnpermuted_;
mutable Permuted<VectorValues> RgProd_;
mutable bool deltaDoglegUptodate_;
/** Indicates whether the current delta is up-to-date, only used
* internally - delta will always be updated if necessary when it is
* requested with getDelta() or calculateEstimate().
@ -316,7 +321,7 @@ protected:
mutable std::vector<bool> deltaReplacedMask_;
/** All original nonlinear factors are stored here to use during relinearization */
GRAPH nonlinearFactors_;
NonlinearFactorGraph nonlinearFactors_;
/** The current elimination ordering Symbols to Index (integer) keys.
*
@ -339,9 +344,7 @@ private:
public:
typedef BayesTree<CONDITIONAL,ISAM2Clique<CONDITIONAL> > Base; ///< The BayesTree base class
typedef ISAM2<CONDITIONAL> This; ///< This class
typedef GRAPH Graph;
typedef BayesTree<GaussianConditional,ISAM2Clique> Base; ///< The BayesTree base class
/** Create an empty ISAM2 instance */
ISAM2(const ISAM2Params& params);
@ -349,17 +352,22 @@ public:
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
ISAM2();
typedef typename Base::Clique Clique; ///< 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 Base::Clique Clique; ///< A clique
typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique
typedef Base::Cliques Cliques; ///< List of Clique typedef from base class
void cloneTo(boost::shared_ptr<This>& newISAM2) const {
void cloneTo(boost::shared_ptr<ISAM2>& newISAM2) const {
boost::shared_ptr<Base> bayesTree = boost::static_pointer_cast<Base>(newISAM2);
Base::cloneTo(bayesTree);
newISAM2->theta_ = theta_;
newISAM2->variableIndex_ = variableIndex_;
newISAM2->deltaUnpermuted_ = deltaUnpermuted_;
newISAM2->delta_ = delta_;
newISAM2->deltaNewtonUnpermuted_ = deltaNewtonUnpermuted_;
newISAM2->deltaNewton_ = deltaNewton_;
newISAM2->RgProdUnpermuted_ = RgProdUnpermuted_;
newISAM2->RgProd_ = RgProd_;
newISAM2->deltaDoglegUptodate_ = deltaDoglegUptodate_;
newISAM2->deltaUptodate_ = deltaUptodate_;
newISAM2->deltaReplacedMask_ = deltaReplacedMask_;
newISAM2->nonlinearFactors_ = nonlinearFactors_;
@ -395,7 +403,7 @@ public:
* (Params::relinearizeSkip).
* @return An ISAM2Result struct containing information about the update
*/
ISAM2Result update(const GRAPH& newFactors = GRAPH(), const Values& newTheta = Values(),
ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(),
const boost::optional<FastSet<Key> >& constrainedKeys = boost::none,
bool force_relinearize = false);
@ -432,7 +440,7 @@ public:
const Permuted<VectorValues>& getDelta() const;
/** Access the set of nonlinear factors */
const GRAPH& getFactorsUnsafe() const { return nonlinearFactors_; }
const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; }
/** Access the current ordering */
const Ordering& getOrdering() const { return ordering_; }
@ -460,8 +468,93 @@ private:
// void linear_update(const GaussianFactorGraph& newFactors);
void updateDelta(bool forceFullSolve = false) const;
friend void optimizeInPlace(const ISAM2&, VectorValues&);
friend void optimizeGradientSearchInPlace(const ISAM2&, VectorValues&);
}; // ISAM2
/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
VectorValues optimize(const ISAM2& isam);
/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
void optimizeInPlace(const ISAM2& isam, VectorValues& delta);
/// Optimize the BayesTree, starting from the root.
/// @param replaced Needs to contain
/// all variables that are contained in the top of the Bayes tree that has been
/// 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 ISAM2& bayesTree, const VectorValues& x0);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$,
* centered around zero.
* The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&).
* This specialized version is used with ISAM2, where each clique stores its
* gradient contribution.
* @param bayesTree The Gaussian Bayes Tree $(R,d)$
* @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
* @return The gradient as a VectorValues
*/
void gradientAtZero(const ISAM2& bayesTree, VectorValues& g);
} /// namespace gtsam
#include <gtsam/nonlinear/ISAM2-inl.h>
#include <gtsam/nonlinear/ISAM2-impl.h>

4
gtsam/nonlinear/Values.h
View File

@ -117,6 +117,8 @@ namespace gtsam {
typedef boost::transform_iterator<
boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
typedef KeyValuePair value_type;
private:
template<class ValueType>
struct _KeyValuePair {
@ -143,6 +145,7 @@ namespace gtsam {
/** A key-value pair, with the value a specific derived Value type. */
typedef _KeyValuePair<ValueType> KeyValuePair;
typedef _ConstKeyValuePair<ValueType> ConstKeyValuePair;
typedef KeyValuePair value_type;
typedef
boost::transform_iterator<
@ -208,6 +211,7 @@ namespace gtsam {
public:
/** A const key-value pair, with the value a specific derived Value type. */
typedef _ConstKeyValuePair<ValueType> KeyValuePair;
typedef KeyValuePair value_type;
typedef typename Filtered<ValueType>::const_const_iterator iterator;
typedef typename Filtered<ValueType>::const_const_iterator const_iterator;

109
tests/testGaussianISAM2.cpp
View File

@ -17,7 +17,7 @@ using namespace boost::assign;
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/nonlinear/GaussianISAM2.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/slam/smallExample.h>
#include <gtsam/slam/planarSLAM.h>
@ -29,7 +29,7 @@ using boost::shared_ptr;
const double tol = 1e-4;
/* ************************************************************************* */
TEST(ISAM2, AddVariables) {
TEST_UNSAFE(ISAM2, AddVariables) {
// Create initial state
Values theta;
@ -48,11 +48,31 @@ TEST(ISAM2, AddVariables) {
Permuted<VectorValues> delta(permutation, deltaUnpermuted);
VectorValues deltaNewtonUnpermuted;
deltaNewtonUnpermuted.insert(0, Vector_(3, .1, .2, .3));
deltaNewtonUnpermuted.insert(1, Vector_(2, .4, .5));
Permutation permutationNewton(2);
permutationNewton[0] = 1;
permutationNewton[1] = 0;
Permuted<VectorValues> deltaNewton(permutationNewton, deltaNewtonUnpermuted);
VectorValues deltaRgUnpermuted;
deltaRgUnpermuted.insert(0, Vector_(3, .1, .2, .3));
deltaRgUnpermuted.insert(1, Vector_(2, .4, .5));
Permutation permutationRg(2);
permutationRg[0] = 1;
permutationRg[1] = 0;
Permuted<VectorValues> deltaRg(permutationRg, deltaRgUnpermuted);
vector<bool> replacedKeys(2, false);
Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0);
GaussianISAM2<>::Nodes nodes(2);
ISAM2::Nodes nodes(2);
// Verify initial state
LONGS_EQUAL(0, ordering[planarSLAM::PointKey(0)]);
@ -78,19 +98,47 @@ TEST(ISAM2, AddVariables) {
Permuted<VectorValues> deltaExpected(permutationExpected, deltaUnpermutedExpected);
VectorValues deltaNewtonUnpermutedExpected;
deltaNewtonUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
deltaNewtonUnpermutedExpected.insert(1, Vector_(2, .4, .5));
deltaNewtonUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
Permutation permutationNewtonExpected(3);
permutationNewtonExpected[0] = 1;
permutationNewtonExpected[1] = 0;
permutationNewtonExpected[2] = 2;
Permuted<VectorValues> deltaNewtonExpected(permutationNewtonExpected, deltaNewtonUnpermutedExpected);
VectorValues deltaRgUnpermutedExpected;
deltaRgUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
deltaRgUnpermutedExpected.insert(1, Vector_(2, .4, .5));
deltaRgUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
Permutation permutationRgExpected(3);
permutationRgExpected[0] = 1;
permutationRgExpected[1] = 0;
permutationRgExpected[2] = 2;
Permuted<VectorValues> deltaRgExpected(permutationRgExpected, deltaRgUnpermutedExpected);
vector<bool> replacedKeysExpected(3, false);
Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1);
GaussianISAM2<>::Nodes nodesExpected(
3, GaussianISAM2<>::sharedClique());
ISAM2::Nodes nodesExpected(
3, ISAM2::sharedClique());
// Expand initial state
GaussianISAM2<>::Impl::AddVariables(newTheta, theta, delta, replacedKeys, ordering, nodes);
ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering, nodes);
EXPECT(assert_equal(thetaExpected, theta));
EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted));
EXPECT(assert_equal(deltaExpected.permutation(), delta.permutation()));
EXPECT(assert_equal(deltaNewtonUnpermutedExpected, deltaNewtonUnpermuted));
EXPECT(assert_equal(deltaNewtonExpected.permutation(), deltaNewton.permutation()));
EXPECT(assert_equal(deltaRgUnpermutedExpected, deltaRgUnpermuted));
EXPECT(assert_equal(deltaRgExpected.permutation(), deltaRg.permutation()));
EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys));
EXPECT(assert_equal(orderingExpected, ordering));
}
@ -171,10 +219,10 @@ TEST(ISAM2, optimize2) {
conditional->solveInPlace(expected);
// Clique
GaussianISAM2<>::sharedClique clique(
GaussianISAM2<>::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr())));
ISAM2::sharedClique clique(
ISAM2::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr())));
VectorValues actual(theta.dims(ordering));
internal::optimizeInPlace(clique, actual);
internal::optimizeInPlace<ISAM2::Base>(clique, actual);
// expected.print("expected: ");
// actual.print("actual: ");
@ -182,7 +230,7 @@ TEST(ISAM2, optimize2) {
}
/* ************************************************************************* */
bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const GaussianISAM2<>& isam) {
bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const ISAM2& isam) {
Values actual = isam.calculateEstimate();
Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first;
GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering);
@ -212,7 +260,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
Values fullinit;
planarSLAM::Graph fullgraph;
@ -300,7 +348,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
CHECK(isam_check(fullgraph, fullinit, isam));
// Check gradient at each node
typedef GaussianISAM2<>::sharedClique sharedClique;
typedef ISAM2::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient
FactorGraph<JacobianFactor> jfg;
@ -345,7 +393,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
GaussianISAM2<> isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
Values fullinit;
planarSLAM::Graph fullgraph;
@ -433,7 +481,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
CHECK(isam_check(fullgraph, fullinit, isam));
// Check gradient at each node
typedef GaussianISAM2<>::sharedClique sharedClique;
typedef ISAM2::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient
FactorGraph<JacobianFactor> jfg;
@ -473,7 +521,7 @@ TEST(ISAM2, clone) {
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
Values fullinit;
planarSLAM::Graph fullgraph;
@ -558,8 +606,8 @@ TEST(ISAM2, clone) {
}
// CLONING...
boost::shared_ptr<GaussianISAM2<> > isam2
= boost::shared_ptr<GaussianISAM2<> >(new GaussianISAM2<>());
boost::shared_ptr<ISAM2 > isam2
= boost::shared_ptr<ISAM2 >(new ISAM2());
isam.cloneTo(isam2);
CHECK(assert_equal(isam, *isam2));
@ -567,24 +615,23 @@ TEST(ISAM2, clone) {
/* ************************************************************************* */
TEST(ISAM2, permute_cached) {
typedef ISAM2Clique<GaussianConditional> Clique;
typedef boost::shared_ptr<ISAM2Clique<GaussianConditional> > sharedClique;
typedef boost::shared_ptr<ISAM2Clique> sharedISAM2Clique;
// Construct expected permuted BayesTree (variable 2 has been changed to 1)
BayesTree<GaussianConditional, Clique> expected;
expected.insert(sharedClique(new Clique(make_pair(
BayesTree<GaussianConditional, ISAM2Clique> expected;
expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
boost::make_shared<GaussianConditional>(pair_list_of
(3, Matrix_(1,1,1.0))
(4, Matrix_(1,1,2.0)),
2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4)
HessianFactor::shared_ptr())))); // Cached: empty
expected.insert(sharedClique(new Clique(make_pair(
expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
boost::make_shared<GaussianConditional>(pair_list_of
(2, Matrix_(1,1,1.0))
(3, Matrix_(1,1,2.0)),
1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3)
boost::make_shared<HessianFactor>(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3)
expected.insert(sharedClique(new Clique(make_pair(
expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
boost::make_shared<GaussianConditional>(pair_list_of
(0, Matrix_(1,1,1.0))
(2, Matrix_(1,1,2.0)),
@ -595,20 +642,20 @@ TEST(ISAM2, permute_cached) {
expected.root()->children().front()->children().front()->conditional()->keys()[1] = 1;
// Construct unpermuted BayesTree
BayesTree<GaussianConditional, Clique> actual;
actual.insert(sharedClique(new Clique(make_pair(
BayesTree<GaussianConditional, ISAM2Clique> actual;
actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
boost::make_shared<GaussianConditional>(pair_list_of
(3, Matrix_(1,1,1.0))
(4, Matrix_(1,1,2.0)),
2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4)
HessianFactor::shared_ptr())))); // Cached: empty
actual.insert(sharedClique(new Clique(make_pair(
actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
boost::make_shared<GaussianConditional>(pair_list_of
(2, Matrix_(1,1,1.0))
(3, Matrix_(1,1,2.0)),
1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3)
boost::make_shared<HessianFactor>(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3)
actual.insert(sharedClique(new Clique(make_pair(
actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
boost::make_shared<GaussianConditional>(pair_list_of
(0, Matrix_(1,1,1.0))
(2, Matrix_(1,1,2.0)),
@ -646,7 +693,7 @@ TEST(ISAM2, removeFactors)
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
Values fullinit;
planarSLAM::Graph fullgraph;
@ -740,7 +787,7 @@ TEST(ISAM2, removeFactors)
CHECK(isam_check(fullgraph, fullinit, isam));
// Check gradient at each node
typedef GaussianISAM2<>::sharedClique sharedClique;
typedef ISAM2::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient
FactorGraph<JacobianFactor> jfg;
@ -785,7 +832,7 @@ TEST(ISAM2, constrained_ordering)
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
Values fullinit;
planarSLAM::Graph fullgraph;
@ -883,7 +930,7 @@ TEST(ISAM2, constrained_ordering)
(isam.getOrdering()[planarSLAM::PoseKey(3)] == 13 && isam.getOrdering()[planarSLAM::PoseKey(4)] == 12));
// Check gradient at each node
typedef GaussianISAM2<>::sharedClique sharedClique;
typedef ISAM2::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient
FactorGraph<JacobianFactor> jfg;