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

Modernized, cleaned up, and turned off non-recursive version (fow now) because it has a bug.

release/4.3a0
Frank Dellaert 2018-09-29 17:52:20 -04:00
parent 05d5179bc3
commit a31294d777
6 changed files with 1011 additions and 833 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -11,33 +11,35 @@
/**
* @file ISAM2-impl.cpp
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid
* relinearization.
* @author Michael Kaess
* @author Richard Roberts
*/
#include <gtsam/nonlinear/ISAM2-impl.h>
#include <gtsam/inference/Symbol.h> // for selective linearization thresholds
#include <gtsam/base/debug.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <gtsam/inference/Symbol.h> // for selective linearization thresholds
#include <gtsam/nonlinear/ISAM2-impl.h>
#include <functional>
#include <boost/range/adaptors.hpp>
#include <functional>
#include <limits>
#include <string>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
void ISAM2::Impl::AddVariables(
const Values& newTheta, Values& theta, VectorValues& delta,
VectorValues& deltaNewton, VectorValues& RgProd,
const KeyFormatter& keyFormatter)
{
void ISAM2::Impl::AddVariables(const Values& newTheta, Values& theta,
VectorValues& delta, VectorValues& deltaNewton,
VectorValues& RgProd,
const KeyFormatter& keyFormatter) {
const bool debug = ISDEBUG("ISAM2 AddVariables");
theta.insert(newTheta);
if(debug) newTheta.print("The new variables are: ");
if (debug) newTheta.print("The new variables are: ");
// Add zeros into the VectorValues
delta.insert(newTheta.zeroVectors());
deltaNewton.insert(newTheta.zeroVectors());
@ -45,55 +47,55 @@ void ISAM2::Impl::AddVariables(
}
/* ************************************************************************* */
void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots,
NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices)
{
void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors,
bool useUnusedSlots,
NonlinearFactorGraph& nonlinearFactors,
FactorIndices& newFactorIndices) {
newFactorIndices.resize(newFactors.size());
if(useUnusedSlots)
{
if (useUnusedSlots) {
size_t globalFactorIndex = 0;
for(size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); ++newFactorIndex)
{
for (size_t newFactorIndex = 0; newFactorIndex < newFactors.size();
++newFactorIndex) {
// Loop to find the next available factor slot
do
{
// If we need to add more factors than we have room for, resize nonlinearFactors,
// filling the new slots with NULL factors. Otherwise, check if the current
// factor in nonlinearFactors is already used, and if so, increase
// globalFactorIndex. If the current factor in nonlinearFactors is unused, break
// out of the loop and use the current slot.
if(globalFactorIndex >= nonlinearFactors.size())
nonlinearFactors.resize(nonlinearFactors.size() + newFactors.size() - newFactorIndex);
else if(nonlinearFactors[globalFactorIndex])
++ globalFactorIndex;
do {
// If we need to add more factors than we have room for, resize
// nonlinearFactors, filling the new slots with NULL factors. Otherwise,
// check if the current factor in nonlinearFactors is already used, and
// if so, increase globalFactorIndex. If the current factor in
// nonlinearFactors is unused, break out of the loop and use the current
// slot.
if (globalFactorIndex >= nonlinearFactors.size())
nonlinearFactors.resize(nonlinearFactors.size() + newFactors.size() -
newFactorIndex);
else if (nonlinearFactors[globalFactorIndex])
++globalFactorIndex;
else
break;
} while(true);
} while (true);
// Use the current slot, updating nonlinearFactors and newFactorSlots.
nonlinearFactors[globalFactorIndex] = newFactors[newFactorIndex];
newFactorIndices[newFactorIndex] = globalFactorIndex;
}
}
else
{
} else {
// We're not looking for unused slots, so just add the factors at the end.
for(size_t i = 0; i < newFactors.size(); ++i)
for (size_t i = 0; i < newFactors.size(); ++i)
newFactorIndices[i] = i + nonlinearFactors.size();
nonlinearFactors.push_back(newFactors);
}
}
/* ************************************************************************* */
void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys,
const FastVector<ISAM2::sharedClique>& roots,
Values& theta, VariableIndex& variableIndex,
VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd,
KeySet& replacedKeys, Base::Nodes& nodes,
KeySet& fixedVariables)
{
VectorValues& delta,
VectorValues& deltaNewton,
VectorValues& RgProd, KeySet& replacedKeys,
Base::Nodes& nodes, KeySet& fixedVariables) {
variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end());
for(Key key: unusedKeys) {
for (Key key : unusedKeys) {
delta.erase(key);
deltaNewton.erase(key);
RgProd.erase(key);
@ -105,26 +107,28 @@ void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector<ISA
}
/* ************************************************************************* */
KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold)
{
KeySet ISAM2::Impl::CheckRelinearizationFull(
const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
KeySet relinKeys;
if(const double* threshold = boost::get<double>(&relinearizeThreshold))
{
for(const VectorValues::KeyValuePair& key_delta: delta) {
if (const double* threshold = boost::get<double>(&relinearizeThreshold)) {
for (const VectorValues::KeyValuePair& key_delta : delta) {
double maxDelta = key_delta.second.lpNorm<Eigen::Infinity>();
if(maxDelta >= *threshold)
relinKeys.insert(key_delta.first);
if (maxDelta >= *threshold) relinKeys.insert(key_delta.first);
}
}
else if(const FastMap<char,Vector>* thresholds = boost::get<FastMap<char,Vector> >(&relinearizeThreshold))
{
for(const VectorValues::KeyValuePair& key_delta: delta) {
const Vector& threshold = thresholds->find(Symbol(key_delta.first).chr())->second;
if(threshold.rows() != key_delta.second.rows())
throw std::invalid_argument("Relinearization threshold vector dimensionality for '" + std::string(1, Symbol(key_delta.first).chr()) + "' passed into iSAM2 parameters does not match actual variable dimensionality.");
if((key_delta.second.array().abs() > threshold.array()).any())
} else if (const FastMap<char, Vector>* thresholds =
boost::get<FastMap<char, Vector> >(&relinearizeThreshold)) {
for (const VectorValues::KeyValuePair& key_delta : delta) {
const Vector& threshold =
thresholds->find(Symbol(key_delta.first).chr())->second;
if (threshold.rows() != key_delta.second.rows())
throw std::invalid_argument(
"Relinearization threshold vector dimensionality for '" +
std::string(1, Symbol(key_delta.first).chr()) +
"' passed into iSAM2 parameters does not match actual variable "
"dimensionality.");
if ((key_delta.second.array().abs() > threshold.array()).any())
relinKeys.insert(key_delta.first);
}
}
@ -133,81 +137,86 @@ KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
}
/* ************************************************************************* */
void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold,
const VectorValues& delta, const ISAM2Clique::shared_ptr& clique)
{
static void CheckRelinearizationRecursiveDouble(
double threshold, const VectorValues& delta,
const ISAM2::sharedClique& clique, KeySet* relinKeys) {
// Check the current clique for relinearization
bool relinearize = false;
for(Key var: *clique->conditional()) {
for (Key var : *clique->conditional()) {
double maxDelta = delta[var].lpNorm<Eigen::Infinity>();
if(maxDelta >= threshold) {
relinKeys.insert(var);
if (maxDelta >= threshold) {
relinKeys->insert(var);
relinearize = true;
}
}
// If this node was relinearized, also check its children
if(relinearize) {
for(const ISAM2Clique::shared_ptr& child: clique->children) {
CheckRelinearizationRecursiveDouble(relinKeys, threshold, delta, child);
if (relinearize) {
for (const ISAM2::sharedClique& child : clique->children) {
CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys);
}
}
}
/* ************************************************************************* */
void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMap<char,Vector>& thresholds,
const VectorValues& delta,
const ISAM2Clique::shared_ptr& clique)
{
static void CheckRelinearizationRecursiveMap(
const FastMap<char, Vector>& thresholds, const VectorValues& delta,
const ISAM2::sharedClique& clique, KeySet* relinKeys) {
// Check the current clique for relinearization
bool relinearize = false;
for(Key var: *clique->conditional()) {
for (Key var : *clique->conditional()) {
// Find the threshold for this variable type
const Vector& threshold = thresholds.find(Symbol(var).chr())->second;
const Vector& deltaVar = delta[var];
// Verify the threshold vector matches the actual variable size
if(threshold.rows() != deltaVar.rows())
throw std::invalid_argument("Relinearization threshold vector dimensionality for '" + std::string(1, Symbol(var).chr()) + "' passed into iSAM2 parameters does not match actual variable dimensionality.");
if (threshold.rows() != deltaVar.rows())
throw std::invalid_argument(
"Relinearization threshold vector dimensionality for '" +
std::string(1, Symbol(var).chr()) +
"' passed into iSAM2 parameters does not match actual variable "
"dimensionality.");
// Check for relinearization
if((deltaVar.array().abs() > threshold.array()).any()) {
relinKeys.insert(var);
if ((deltaVar.array().abs() > threshold.array()).any()) {
relinKeys->insert(var);
relinearize = true;
}
}
// If this node was relinearized, also check its children
if(relinearize) {
for(const ISAM2Clique::shared_ptr& child: clique->children) {
CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, child);
if (relinearize) {
for (const ISAM2::sharedClique& child : clique->children) {
CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys);
}
}
}
/* ************************************************************************* */
KeySet ISAM2::Impl::CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold)
{
KeySet ISAM2::Impl::CheckRelinearizationPartial(
const FastVector<ISAM2::sharedClique>& roots, const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
KeySet relinKeys;
for(const ISAM2::sharedClique& root: roots) {
if(relinearizeThreshold.type() == typeid(double))
CheckRelinearizationRecursiveDouble(relinKeys, boost::get<double>(relinearizeThreshold), delta, root);
else if(relinearizeThreshold.type() == typeid(FastMap<char,Vector>))
CheckRelinearizationRecursiveMap(relinKeys, boost::get<FastMap<char,Vector> >(relinearizeThreshold), delta, root);
for (const ISAM2::sharedClique& root : roots) {
if (relinearizeThreshold.type() == typeid(double))
CheckRelinearizationRecursiveDouble(
boost::get<double>(relinearizeThreshold), delta, root, &relinKeys);
else if (relinearizeThreshold.type() == typeid(FastMap<char, Vector>))
CheckRelinearizationRecursiveMap(
boost::get<FastMap<char, Vector> >(relinearizeThreshold), delta, root,
&relinKeys);
}
return relinKeys;
}
/* ************************************************************************* */
void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask)
{
void ISAM2::Impl::FindAll(ISAM2::sharedClique clique, KeySet& keys,
const KeySet& markedMask) {
static const bool debug = false;
// does the separator contain any of the variables?
bool found = false;
for(Key key: clique->conditional()->parents()) {
for (Key key : clique->conditional()->parents()) {
if (markedMask.exists(key)) {
found = true;
break;
@ -215,19 +224,22 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const Ke
}
if (found) {
// then add this clique
keys.insert(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals());
if(debug) clique->print("Key(s) marked in clique ");
if(debug) cout << "so marking key " << clique->conditional()->front() << endl;
keys.insert(clique->conditional()->beginFrontals(),
clique->conditional()->endFrontals());
if (debug) clique->print("Key(s) marked in clique ");
if (debug)
cout << "so marking key " << clique->conditional()->front() << endl;
}
for(const ISAM2Clique::shared_ptr& child: clique->children) {
for (const ISAM2::sharedClique& child : clique->children) {
FindAll(child, keys, markedMask);
}
}
/* ************************************************************************* */
void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta,
const KeySet& mask, boost::optional<VectorValues&> invalidateIfDebug, const KeyFormatter& keyFormatter)
{
void ISAM2::Impl::ExpmapMasked(const VectorValues& delta, const KeySet& mask,
Values* values,
boost::optional<VectorValues&> invalidateIfDebug,
const KeyFormatter& keyFormatter) {
// If debugging, invalidate if requested, otherwise do not invalidate.
// Invalidating means setting expmapped entries to Inf, to trigger assertions
// if we try to re-use them.
@ -235,65 +247,70 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta,
invalidateIfDebug = boost::none;
#endif
assert(values.size() == delta.size());
assert(values->size() == delta.size());
Values::iterator key_value;
VectorValues::const_iterator key_delta;
#ifdef GTSAM_USE_TBB
for(key_value = values.begin(); key_value != values.end(); ++key_value)
{
for (key_value = values->begin(); key_value != values->end(); ++key_value) {
key_delta = delta.find(key_value->key);
#else
for(key_value = values.begin(), key_delta = delta.begin(); key_value != values.end(); ++key_value, ++key_delta)
{
for (key_value = values->begin(), key_delta = delta.begin();
key_value != values->end(); ++key_value, ++key_delta) {
assert(key_value->key == key_delta->first);
#endif
Key var = key_value->key;
assert(delta[var].size() == (int)key_value->value.dim());
assert(static_cast<size_t>(delta[var].size()) == key_value->value.dim());
assert(delta[var].allFinite());
if(mask.exists(var)) {
if (mask.exists(var)) {
Value* retracted = key_value->value.retract_(delta[var]);
key_value->value = *retracted;
retracted->deallocate_();
if(invalidateIfDebug)
(*invalidateIfDebug)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?)
if (invalidateIfDebug)
(*invalidateIfDebug)[var].operator=(Vector::Constant(
delta[var].rows(),
numeric_limits<double>::infinity())); // Strange syntax to work
// with clang++ (bug in
// clang?)
}
}
}
/* ************************************************************************* */
namespace internal {
inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique>& clique, VectorValues& result) {
inline static void optimizeInPlace(const ISAM2::sharedClique& clique,
VectorValues* result) {
// parents are assumed to already be solved and available in result
result.update(clique->conditional()->solve(result));
result->update(clique->conditional()->solve(*result));
// starting from the root, call optimize on each conditional
for(const boost::shared_ptr<ISAM2Clique>& child: clique->children)
for (const ISAM2::sharedClique& child : clique->children)
optimizeInPlace(child, result);
}
}
} // namespace internal
/* ************************************************************************* */
size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots,
const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold) {
size_t ISAM2::Impl::UpdateGaussNewtonDelta(
const FastVector<ISAM2::sharedClique>& roots, const KeySet& replacedKeys,
double wildfireThreshold, VectorValues* delta) {
size_t lastBacksubVariableCount;
if (wildfireThreshold <= 0.0) {
// Threshold is zero or less, so do a full recalculation
for(const ISAM2::sharedClique& root: roots)
for (const ISAM2::sharedClique& root : roots)
internal::optimizeInPlace(root, delta);
lastBacksubVariableCount = delta.size();
lastBacksubVariableCount = delta->size();
} else {
// Optimize with wildfire
lastBacksubVariableCount = 0;
for(const ISAM2::sharedClique& root: roots)
lastBacksubVariableCount += optimizeWildfireNonRecursive(
root, wildfireThreshold, replacedKeys, delta); // modifies delta
for (const ISAM2::sharedClique& root : roots)
lastBacksubVariableCount += optimizeWildfire(
root, wildfireThreshold, replacedKeys, delta); // modifies delta
#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
for (VectorValues::const_iterator key_delta = delta.begin(); key_delta != delta.end(); ++key_delta) {
assert(delta[key_delta->first].allFinite());
for (VectorValues::const_iterator key_delta = delta->begin();
key_delta != delta->end(); ++key_delta) {
assert((*delta)[key_delta->first].allFinite());
}
#endif
}
@ -303,69 +320,77 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>
/* ************************************************************************* */
namespace internal {
void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const KeySet& replacedKeys,
const VectorValues& grad, VectorValues& RgProd, size_t& varsUpdated) {
void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys,
const VectorValues& grad, 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;
for(Key j: *clique->conditional()) {
if(replacedKeys.exists(j)) {
for (Key j : *clique->conditional()) {
if (replacedKeys.exists(j)) {
anyReplaced = true;
break;
}
}
if(anyReplaced) {
if (anyReplaced) {
// Update the current variable
// Get VectorValues slice corresponding to current variables
Vector gR = grad.vector(FastVector<Key>(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()));
Vector gS = grad.vector(FastVector<Key>(clique->conditional()->beginParents(), clique->conditional()->endParents()));
Vector gR =
grad.vector(FastVector<Key>(clique->conditional()->beginFrontals(),
clique->conditional()->endFrontals()));
Vector gS =
grad.vector(FastVector<Key>(clique->conditional()->beginParents(),
clique->conditional()->endParents()));
// Compute R*g and S*g for this clique
Vector RSgProd = clique->conditional()->get_R() * gR + clique->conditional()->get_S() * gS;
Vector RSgProd = clique->conditional()->get_R() * gR +
clique->conditional()->get_S() * gS;
// Write into RgProd vector
DenseIndex vectorPosition = 0;
for(Key frontal: clique->conditional()->frontals()) {
Vector& RgProdValue = RgProd[frontal];
for (Key frontal : clique->conditional()->frontals()) {
Vector& RgProdValue = (*RgProd)[frontal];
RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size());
vectorPosition += RgProdValue.size();
}
// Now solve the part of the Newton's method point for this clique (back-substitution)
//(*clique)->solveInPlace(deltaNewton);
// Now solve the part of the Newton's method point for this clique
// (back-substitution)
// (*clique)->solveInPlace(deltaNewton);
varsUpdated += clique->conditional()->nrFrontals();
*varsUpdated += clique->conditional()->nrFrontals();
// Recurse to children
for(const ISAM2Clique::shared_ptr& child: clique->children) {
updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated); }
for (const ISAM2::sharedClique& child : clique->children) {
updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated);
}
}
}
}
} // namespace internal
/* ************************************************************************* */
size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys,
const VectorValues& gradAtZero, VectorValues& RgProd) {
size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots,
const KeySet& replacedKeys,
const VectorValues& gradAtZero,
VectorValues* RgProd) {
// Update variables
size_t varsUpdated = 0;
for(const ISAM2::sharedClique& root: roots) {
internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, varsUpdated);
for (const ISAM2::sharedClique& root : roots) {
internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd,
&varsUpdated);
}
return varsUpdated;
}
/* ************************************************************************* */
VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero,
const VectorValues& RgProd)
{
const VectorValues& RgProd) {
// Compute gradient squared-magnitude
const double gradientSqNorm = gradAtZero.dot(gradAtZero);
// Compute minimizing step size
double RgNormSq = RgProd.vector().squaredNorm();
double step = -gradientSqNorm / RgNormSq;
@ -374,4 +399,4 @@ VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero,
return step * gradAtZero;
}
}
} // namespace gtsam

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

@ -108,18 +108,17 @@ struct GTSAM_EXPORT ISAM2::Impl {
/**
* 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 [in, out] values The value to expmap in-place
* \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 VectorValues& delta,
const KeySet& mask,
static void ExpmapMasked(
const VectorValues& delta, const KeySet& mask, Values* values,
boost::optional<VectorValues&> invalidateIfDebug = boost::none,
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
@ -127,14 +126,14 @@ struct GTSAM_EXPORT ISAM2::Impl {
* Update the Newton's method step point, using wildfire
*/
static size_t UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots,
const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold);
const KeySet& replacedKeys, double wildfireThreshold, VectorValues* delta);
/**
* Update the RgProd (R*g) incrementally taking into account which variables
* have been recalculated in \c replacedKeys. Only used in Dogleg.
*/
static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys,
const VectorValues& gradAtZero, VectorValues& RgProd);
const VectorValues& gradAtZero, VectorValues* RgProd);
/**
* Compute the gradient-search point. Only used in Dogleg.

287
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)
@ -11,301 +11,24 @@
/**
* @file ISAM2-inl.h
* @brief
* @brief
* @author Richard Roberts
* @date Mar 16, 2012
*/
#pragma once
#include <stack>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
namespace gtsam {
/* ************************************************************************* */
template<class VALUE>
template <class VALUE>
VALUE ISAM2::calculateEstimate(Key key) const {
const Vector& delta = getDelta()[key];
return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta);
}
/* ************************************************************************* */
namespace internal {
template<class CLIQUE>
void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& 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.exists((*clique)->frontals().front());
#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
for(Key frontal: clique->conditional()->frontals()) {
assert(cliqueReplaced == replaced.exists(frontal));
}
#endif
// If not redone, then has one of the separator variables changed significantly?
bool recalculate = cliqueReplaced;
if(!recalculate) {
for(Key parent: clique->conditional()->parents()) {
if(changed.exists(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
FastVector<Vector> originalValues(clique->conditional()->nrFrontals());
GaussianConditional::const_iterator it;
for(it = clique->conditional()->beginFrontals(); it!=clique->conditional()->endFrontals(); it++) {
originalValues[it - clique->conditional()->beginFrontals()] = delta[*it];
}
// Back-substitute
delta.update(clique->conditional()->solve(delta));
count += clique->conditional()->nrFrontals();
// Whether the values changed above a threshold, or always true if the
// clique was replaced.
bool valuesChanged = cliqueReplaced;
for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) {
if(!valuesChanged) {
const Vector& oldValue(originalValues[it - clique->conditional()->beginFrontals()]);
const Vector& 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
for(Key frontal: clique->conditional()->frontals()) {
changed.insert(frontal);
}
} else {
// Replace with the old values
for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) {
delta[*it] = originalValues[it - clique->conditional()->beginFrontals()];
}
}
// Recurse to children
for(const typename CLIQUE::shared_ptr& child: clique->children) {
optimizeWildfire(child, threshold, changed, replaced, delta, count);
}
}
}
template<class CLIQUE>
bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double threshold,
KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& 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.exists(clique->conditional()->frontals().front());
#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
for(Key frontal: clique->conditional()->frontals()) {
assert(cliqueReplaced == replaced.exists(frontal));
}
#endif
// If not redone, then has one of the separator variables changed significantly?
bool recalculate = cliqueReplaced;
if(!recalculate) {
for(Key parent: clique->conditional()->parents()) {
if(changed.exists(parent)) {
recalculate = true;
break;
}
}
}
// Solve clique if it was replaced, or if any parents were changed above the
// threshold or themselves replaced.
// TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, potentially refactor
if(recalculate)
{
// Temporary copy of the original values, to check how much they change
FastVector<Vector> originalValues(clique->conditional()->nrFrontals());
GaussianConditional::const_iterator it;
for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) {
originalValues[it - clique->conditional()->beginFrontals()] = delta[*it];
}
// Back-substitute - special version stores solution pointers in cliques for fast access.
{
// Create solution part pointers if necessary and possible - necessary if solnPointers_ is
// empty, and possible if either we're a root, or we have a parent with valid solnPointers_.
boost::shared_ptr<CLIQUE> parent = clique->parent_.lock();
if(clique->solnPointers_.empty() && (clique->isRoot() || !parent->solnPointers_.empty()))
{
for(Key key: clique->conditional()->frontals())
clique->solnPointers_.insert(std::make_pair(key, delta.find(key)));
for(Key key: clique->conditional()->parents())
clique->solnPointers_.insert(std::make_pair(key, parent->solnPointers_.at(key)));
}
// See if we can use solution part pointers - we can if they either already existed or were
// created above.
if(!clique->solnPointers_.empty())
{
GaussianConditional& c = *clique->conditional();
// Solve matrix
Vector xS;
{
// Count dimensions of vector
DenseIndex dim = 0;
FastVector<VectorValues::const_iterator> parentPointers;
parentPointers.reserve(clique->conditional()->nrParents());
for(Key parent: clique->conditional()->parents()) {
parentPointers.push_back(clique->solnPointers_.at(parent));
dim += parentPointers.back()->second.size();
}
// Fill parent vector
xS.resize(dim);
DenseIndex vectorPos = 0;
for(const VectorValues::const_iterator& parentPointer: parentPointers) {
const Vector& parentVector = parentPointer->second;
xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1);
vectorPos += parentVector.size();
}
}
// NOTE(gareth): We can no longer write: xS = b - S * xS
// This is because Eigen (as of 3.3) no longer evaluates S * xS into
// a temporary, and the operation trashes valus in xS.
// See: http://eigen.tuxfamily.org/index.php?title=3.3
const Vector rhs = c.getb() - c.get_S() * xS;
const Vector solution = c.get_R().triangularView<Eigen::Upper>().solve(rhs);
// Check for indeterminant solution
if(solution.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front());
// Insert solution into a VectorValues
DenseIndex vectorPosition = 0;
for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) {
clique->solnPointers_.at(*frontal)->second = solution.segment(vectorPosition, c.getDim(frontal));
vectorPosition += c.getDim(frontal);
}
}
else
{
// Just call plain solve because we couldn't use solution pointers.
delta.update(clique->conditional()->solve(delta));
}
}
count += clique->conditional()->nrFrontals();
// Whether the values changed above a threshold, or always true if the
// clique was replaced.
bool valuesChanged = cliqueReplaced;
for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) {
if(!valuesChanged) {
const Vector& oldValue(originalValues[it - clique->conditional()->beginFrontals()]);
const Vector& 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
for(Key frontal: clique->conditional()->frontals()) {
changed.insert(frontal);
}
} else {
// Replace with the old values
for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) {
delta[*it] = originalValues[it - clique->conditional()->beginFrontals()];
}
}
}
return recalculate;
}
} // namespace internal
/* ************************************************************************* */
template<class CLIQUE>
size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const KeySet& keys, VectorValues& delta) {
KeySet changed;
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>
size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root, double threshold, const KeySet& keys, VectorValues& delta)
{
KeySet changed;
size_t count = 0;
if (root) {
std::stack<boost::shared_ptr<CLIQUE> > travStack;
travStack.push(root);
boost::shared_ptr<CLIQUE> currentNode = root;
while (!travStack.empty()) {
currentNode = travStack.top();
travStack.pop();
bool recalculate = internal::optimizeWildfireNode(currentNode, threshold, changed, keys, delta, count);
if (recalculate) {
for(const typename CLIQUE::shared_ptr& child: currentNode->children) {
travStack.push(child);
}
}
}
}
return count;
}
/* ************************************************************************* */
template<class CLIQUE>
void nnz_internal(const boost::shared_ptr<CLIQUE>& clique, int& result) {
int dimR = (int)clique->conditional()->rows();
int dimSep = (int)clique->conditional()->get_S().cols();
result += ((dimR+1)*dimR)/2 + dimSep*dimR;
// traverse the children
for(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

1149
gtsam/nonlinear/ISAM2.cpp
View File

File diff suppressed because it is too large Load Diff

78
gtsam/nonlinear/ISAM2.h
View File

@ -509,7 +509,7 @@ class GTSAM_EXPORT ISAM2Clique
Base::FactorType::shared_ptr cachedFactor_;
Vector gradientContribution_;
FastMap<Key, VectorValues::iterator> solnPointers_;
mutable FastMap<Key, VectorValues::iterator> solnPointers_;
/// Default constructor
ISAM2Clique() : Base() {}
@ -546,6 +546,45 @@ class GTSAM_EXPORT ISAM2Clique
void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
/**
* Check if clique was replaced, or if any parents were changed above the
* threshold or themselves replaced.
*/
bool isDirty(const KeySet& replaced, const KeySet& changed) const;
/// Copy values in delta pertaining to this clique.
FastVector<Vector> copyRelevantValues(const VectorValues& delta) const;
/*
* Check whether the values changed above a threshold, or always true if the
* clique was replaced.
*/
bool valuesChanged(const KeySet& replaced,
const FastVector<Vector>& originalValues,
const VectorValues& delta, double threshold) const;
/// Set changed flag for each frontal variable
void markFrontalsAsChanged(KeySet* changed) const;
/// Restore delta to original values, guided by frontal keys.
void restoreFromOriginals(const FastVector<Vector>& originalValues,
VectorValues* delta) const;
void optimizeWildfire(const KeySet& replaced, double threshold, KeySet* changed,
VectorValues* delta,
size_t* count) const;
bool optimizeWildfireNode(const KeySet& replaced, double threshold, KeySet* changed,
VectorValues* delta,
size_t* count) const;
/**
* Starting from the root, add up entries of frontal and conditional matrices
* of each conditional
*/
void nnz_internal(size_t* result) const;
size_t calculate_nnz() const;
private:
/** Serialization function */
friend class boost::serialization::access;
@ -810,30 +849,23 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
template <>
struct traits<ISAM2> : public Testable<ISAM2> {};
/// 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>
size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold,
const KeySet& replaced, VectorValues& delta);
/**
* Optimize the BayesTree, starting from the root.
* @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.
* @param replaced Needs to contain all variables that are contained in the top
* of the Bayes tree that has been redone.
* @return The number of variables that were solved for.
* @param delta The current solution, an offset from the linearization point.
*/
size_t optimizeWildfire(const ISAM2::sharedClique& root, double threshold,
const KeySet& replaced, VectorValues* delta);
template <class CLIQUE>
size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root,
size_t optimizeWildfireNonRecursive(const ISAM2::sharedClique& root,
double threshold, const KeySet& replaced,
VectorValues& delta);
/// 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);
VectorValues* delta);
} // namespace gtsam

2
tests/testGaussianISAM2.cpp
View File

@ -843,7 +843,7 @@ TEST(ISAM2, calculate_nnz)
{
ISAM2 isam = createSlamlikeISAM2();
int expected = 241;
int actual = calculate_nnz(isam.roots().front());
int actual = isam.roots().front()->calculate_nnz();
EXPECT_LONGS_EQUAL(expected, actual);
}