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

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

@ -11,33 +11,35 @@
/** /**
* @file ISAM2-impl.cpp * @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 Michael Kaess
* @author Richard Roberts * @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/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 <boost/range/adaptors.hpp>
#include <functional>
#include <limits>
#include <string>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void ISAM2::Impl::AddVariables( void ISAM2::Impl::AddVariables(const Values& newTheta, Values& theta,
const Values& newTheta, Values& theta, VectorValues& delta, VectorValues& delta, VectorValues& deltaNewton,
VectorValues& deltaNewton, VectorValues& RgProd, VectorValues& RgProd,
const KeyFormatter& keyFormatter) const KeyFormatter& keyFormatter) {
{
const bool debug = ISDEBUG("ISAM2 AddVariables"); const bool debug = ISDEBUG("ISAM2 AddVariables");
theta.insert(newTheta); theta.insert(newTheta);
if(debug) newTheta.print("The new variables are: "); if (debug) newTheta.print("The new variables are: ");
// Add zeros into the VectorValues // Add zeros into the VectorValues
delta.insert(newTheta.zeroVectors()); delta.insert(newTheta.zeroVectors());
deltaNewton.insert(newTheta.zeroVectors()); deltaNewton.insert(newTheta.zeroVectors());
@ -45,55 +47,55 @@ void ISAM2::Impl::AddVariables(
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors,
NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices) bool useUnusedSlots,
{ NonlinearFactorGraph& nonlinearFactors,
FactorIndices& newFactorIndices) {
newFactorIndices.resize(newFactors.size()); newFactorIndices.resize(newFactors.size());
if(useUnusedSlots) if (useUnusedSlots) {
{
size_t globalFactorIndex = 0; 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 // Loop to find the next available factor slot
do do {
{ // If we need to add more factors than we have room for, resize
// If we need to add more factors than we have room for, resize nonlinearFactors, // nonlinearFactors, filling the new slots with NULL factors. Otherwise,
// filling the new slots with NULL factors. Otherwise, check if the current // check if the current factor in nonlinearFactors is already used, and
// factor in nonlinearFactors is already used, and if so, increase // if so, increase globalFactorIndex. If the current factor in
// globalFactorIndex. If the current factor in nonlinearFactors is unused, break // nonlinearFactors is unused, break out of the loop and use the current
// out of the loop and use the current slot. // slot.
if(globalFactorIndex >= nonlinearFactors.size()) if (globalFactorIndex >= nonlinearFactors.size())
nonlinearFactors.resize(nonlinearFactors.size() + newFactors.size() - newFactorIndex); nonlinearFactors.resize(nonlinearFactors.size() + newFactors.size() -
else if(nonlinearFactors[globalFactorIndex]) newFactorIndex);
++ globalFactorIndex; else if (nonlinearFactors[globalFactorIndex])
++globalFactorIndex;
else else
break; break;
} while(true); } while (true);
// Use the current slot, updating nonlinearFactors and newFactorSlots. // Use the current slot, updating nonlinearFactors and newFactorSlots.
nonlinearFactors[globalFactorIndex] = newFactors[newFactorIndex]; nonlinearFactors[globalFactorIndex] = newFactors[newFactorIndex];
newFactorIndices[newFactorIndex] = globalFactorIndex; newFactorIndices[newFactorIndex] = globalFactorIndex;
} }
} } else {
else
{
// We're not looking for unused slots, so just add the factors at the end. // 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(); newFactorIndices[i] = i + nonlinearFactors.size();
nonlinearFactors.push_back(newFactors); 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, Values& theta, VariableIndex& variableIndex,
VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, VectorValues& delta,
KeySet& replacedKeys, Base::Nodes& nodes, VectorValues& deltaNewton,
KeySet& fixedVariables) VectorValues& RgProd, KeySet& replacedKeys,
{ Base::Nodes& nodes, KeySet& fixedVariables) {
variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end());
for(Key key: unusedKeys) { for (Key key : unusedKeys) {
delta.erase(key); delta.erase(key);
deltaNewton.erase(key); deltaNewton.erase(key);
RgProd.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, KeySet ISAM2::Impl::CheckRelinearizationFull(
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) const VectorValues& delta,
{ const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
KeySet relinKeys; KeySet relinKeys;
if(const double* threshold = boost::get<double>(&relinearizeThreshold)) if (const double* threshold = boost::get<double>(&relinearizeThreshold)) {
{ for (const VectorValues::KeyValuePair& key_delta : delta) {
for(const VectorValues::KeyValuePair& key_delta: delta) {
double maxDelta = key_delta.second.lpNorm<Eigen::Infinity>(); double maxDelta = key_delta.second.lpNorm<Eigen::Infinity>();
if(maxDelta >= *threshold) if (maxDelta >= *threshold) relinKeys.insert(key_delta.first);
relinKeys.insert(key_delta.first);
} }
} } else if (const FastMap<char, Vector>* thresholds =
else if(const FastMap<char,Vector>* thresholds = boost::get<FastMap<char,Vector> >(&relinearizeThreshold)) boost::get<FastMap<char, Vector> >(&relinearizeThreshold)) {
{ for (const VectorValues::KeyValuePair& key_delta : delta) {
for(const VectorValues::KeyValuePair& key_delta: delta) { const Vector& threshold =
const Vector& threshold = thresholds->find(Symbol(key_delta.first).chr())->second; thresholds->find(Symbol(key_delta.first).chr())->second;
if(threshold.rows() != key_delta.second.rows()) 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."); throw std::invalid_argument(
if((key_delta.second.array().abs() > threshold.array()).any()) "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); relinKeys.insert(key_delta.first);
} }
} }
@ -133,81 +137,86 @@ KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
} }
/* ************************************************************************* */ /* ************************************************************************* */
void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold, static void CheckRelinearizationRecursiveDouble(
const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) double threshold, const VectorValues& delta,
{ const ISAM2::sharedClique& clique, KeySet* relinKeys) {
// Check the current clique for relinearization // Check the current clique for relinearization
bool relinearize = false; bool relinearize = false;
for(Key var: *clique->conditional()) { for (Key var : *clique->conditional()) {
double maxDelta = delta[var].lpNorm<Eigen::Infinity>(); double maxDelta = delta[var].lpNorm<Eigen::Infinity>();
if(maxDelta >= threshold) { if (maxDelta >= threshold) {
relinKeys.insert(var); relinKeys->insert(var);
relinearize = true; relinearize = true;
} }
} }
// If this node was relinearized, also check its children // If this node was relinearized, also check its children
if(relinearize) { if (relinearize) {
for(const ISAM2Clique::shared_ptr& child: clique->children) { for (const ISAM2::sharedClique& child : clique->children) {
CheckRelinearizationRecursiveDouble(relinKeys, threshold, delta, child); CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys);
} }
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMap<char,Vector>& thresholds, static void CheckRelinearizationRecursiveMap(
const VectorValues& delta, const FastMap<char, Vector>& thresholds, const VectorValues& delta,
const ISAM2Clique::shared_ptr& clique) const ISAM2::sharedClique& clique, KeySet* relinKeys) {
{
// Check the current clique for relinearization // Check the current clique for relinearization
bool relinearize = false; bool relinearize = false;
for(Key var: *clique->conditional()) { for (Key var : *clique->conditional()) {
// Find the threshold for this variable type // Find the threshold for this variable type
const Vector& threshold = thresholds.find(Symbol(var).chr())->second; const Vector& threshold = thresholds.find(Symbol(var).chr())->second;
const Vector& deltaVar = delta[var]; const Vector& deltaVar = delta[var];
// Verify the threshold vector matches the actual variable size // Verify the threshold vector matches the actual variable size
if(threshold.rows() != deltaVar.rows()) 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."); 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 // Check for relinearization
if((deltaVar.array().abs() > threshold.array()).any()) { if ((deltaVar.array().abs() > threshold.array()).any()) {
relinKeys.insert(var); relinKeys->insert(var);
relinearize = true; relinearize = true;
} }
} }
// If this node was relinearized, also check its children // If this node was relinearized, also check its children
if(relinearize) { if (relinearize) {
for(const ISAM2Clique::shared_ptr& child: clique->children) { for (const ISAM2::sharedClique& child : clique->children) {
CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, child); CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys);
} }
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
KeySet ISAM2::Impl::CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots, KeySet ISAM2::Impl::CheckRelinearizationPartial(
const VectorValues& delta, const FastVector<ISAM2::sharedClique>& roots, const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
{
KeySet relinKeys; KeySet relinKeys;
for(const ISAM2::sharedClique& root: roots) { for (const ISAM2::sharedClique& root : roots) {
if(relinearizeThreshold.type() == typeid(double)) if (relinearizeThreshold.type() == typeid(double))
CheckRelinearizationRecursiveDouble(relinKeys, boost::get<double>(relinearizeThreshold), delta, root); CheckRelinearizationRecursiveDouble(
else if(relinearizeThreshold.type() == typeid(FastMap<char,Vector>)) boost::get<double>(relinearizeThreshold), delta, root, &relinKeys);
CheckRelinearizationRecursiveMap(relinKeys, boost::get<FastMap<char,Vector> >(relinearizeThreshold), delta, root); else if (relinearizeThreshold.type() == typeid(FastMap<char, Vector>))
CheckRelinearizationRecursiveMap(
boost::get<FastMap<char, Vector> >(relinearizeThreshold), delta, root,
&relinKeys);
} }
return 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; static const bool debug = false;
// does the separator contain any of the variables? // does the separator contain any of the variables?
bool found = false; bool found = false;
for(Key key: clique->conditional()->parents()) { for (Key key : clique->conditional()->parents()) {
if (markedMask.exists(key)) { if (markedMask.exists(key)) {
found = true; found = true;
break; break;
@ -215,19 +224,22 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const Ke
} }
if (found) { if (found) {
// then add this clique // then add this clique
keys.insert(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); keys.insert(clique->conditional()->beginFrontals(),
if(debug) clique->print("Key(s) marked in clique "); clique->conditional()->endFrontals());
if(debug) cout << "so marking key " << clique->conditional()->front() << endl; 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); FindAll(child, keys, markedMask);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, void ISAM2::Impl::ExpmapMasked(const VectorValues& delta, const KeySet& mask,
const KeySet& mask, boost::optional<VectorValues&> invalidateIfDebug, const KeyFormatter& keyFormatter) Values* values,
{ boost::optional<VectorValues&> invalidateIfDebug,
const KeyFormatter& keyFormatter) {
// If debugging, invalidate if requested, otherwise do not invalidate. // If debugging, invalidate if requested, otherwise do not invalidate.
// Invalidating means setting expmapped entries to Inf, to trigger assertions // Invalidating means setting expmapped entries to Inf, to trigger assertions
// if we try to re-use them. // if we try to re-use them.
@ -235,65 +247,70 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta,
invalidateIfDebug = boost::none; invalidateIfDebug = boost::none;
#endif #endif
assert(values.size() == delta.size()); assert(values->size() == delta.size());
Values::iterator key_value; Values::iterator key_value;
VectorValues::const_iterator key_delta; VectorValues::const_iterator key_delta;
#ifdef GTSAM_USE_TBB #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); key_delta = delta.find(key_value->key);
#else #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); assert(key_value->key == key_delta->first);
#endif #endif
Key var = key_value->key; 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()); assert(delta[var].allFinite());
if(mask.exists(var)) { if (mask.exists(var)) {
Value* retracted = key_value->value.retract_(delta[var]); Value* retracted = key_value->value.retract_(delta[var]);
key_value->value = *retracted; key_value->value = *retracted;
retracted->deallocate_(); retracted->deallocate_();
if(invalidateIfDebug) if (invalidateIfDebug)
(*invalidateIfDebug)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?) (*invalidateIfDebug)[var].operator=(Vector::Constant(
delta[var].rows(),
numeric_limits<double>::infinity())); // Strange syntax to work
// with clang++ (bug in
// clang?)
} }
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
namespace internal { 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 // 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 // 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); optimizeInPlace(child, result);
} }
} } // namespace internal
/* ************************************************************************* */ /* ************************************************************************* */
size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots, size_t ISAM2::Impl::UpdateGaussNewtonDelta(
const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold) { const FastVector<ISAM2::sharedClique>& roots, const KeySet& replacedKeys,
double wildfireThreshold, VectorValues* delta) {
size_t lastBacksubVariableCount; size_t lastBacksubVariableCount;
if (wildfireThreshold <= 0.0) { if (wildfireThreshold <= 0.0) {
// Threshold is zero or less, so do a full recalculation // 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); internal::optimizeInPlace(root, delta);
lastBacksubVariableCount = delta.size(); lastBacksubVariableCount = delta->size();
} else { } else {
// Optimize with wildfire // Optimize with wildfire
lastBacksubVariableCount = 0; lastBacksubVariableCount = 0;
for(const ISAM2::sharedClique& root: roots) for (const ISAM2::sharedClique& root : roots)
lastBacksubVariableCount += optimizeWildfireNonRecursive( lastBacksubVariableCount += optimizeWildfire(
root, wildfireThreshold, replacedKeys, delta); // modifies delta root, wildfireThreshold, replacedKeys, delta); // modifies delta
#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) #if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
for (VectorValues::const_iterator key_delta = delta.begin(); key_delta != delta.end(); ++key_delta) { for (VectorValues::const_iterator key_delta = delta->begin();
assert(delta[key_delta->first].allFinite()); key_delta != delta->end(); ++key_delta) {
assert((*delta)[key_delta->first].allFinite());
} }
#endif #endif
} }
@ -303,57 +320,66 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>
/* ************************************************************************* */ /* ************************************************************************* */
namespace internal { namespace internal {
void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const KeySet& replacedKeys, void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys,
const VectorValues& grad, VectorValues& RgProd, size_t& varsUpdated) { const VectorValues& grad, VectorValues* RgProd,
size_t* varsUpdated) {
// Check if any frontal or separator keys were recalculated, if so, we need // 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 // update deltas and recurse to children, but if not, we do not need to
// recurse further because of the running separator property. // recurse further because of the running separator property.
bool anyReplaced = false; bool anyReplaced = false;
for(Key j: *clique->conditional()) { for (Key j : *clique->conditional()) {
if(replacedKeys.exists(j)) { if (replacedKeys.exists(j)) {
anyReplaced = true; anyReplaced = true;
break; break;
} }
} }
if(anyReplaced) { if (anyReplaced) {
// Update the current variable // Update the current variable
// Get VectorValues slice corresponding to current variables // Get VectorValues slice corresponding to current variables
Vector gR = grad.vector(FastVector<Key>(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals())); Vector gR =
Vector gS = grad.vector(FastVector<Key>(clique->conditional()->beginParents(), clique->conditional()->endParents())); 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 // 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 // Write into RgProd vector
DenseIndex vectorPosition = 0; DenseIndex vectorPosition = 0;
for(Key frontal: clique->conditional()->frontals()) { for (Key frontal : clique->conditional()->frontals()) {
Vector& RgProdValue = RgProd[frontal]; Vector& RgProdValue = (*RgProd)[frontal];
RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size()); RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size());
vectorPosition += RgProdValue.size(); vectorPosition += RgProdValue.size();
} }
// Now solve the part of the Newton's method point for this clique (back-substitution) // Now solve the part of the Newton's method point for this clique
//(*clique)->solveInPlace(deltaNewton); // (back-substitution)
// (*clique)->solveInPlace(deltaNewton);
varsUpdated += clique->conditional()->nrFrontals(); *varsUpdated += clique->conditional()->nrFrontals();
// Recurse to children // Recurse to children
for(const ISAM2Clique::shared_ptr& child: clique->children) { for (const ISAM2::sharedClique& child : clique->children) {
updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated); } updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated);
}
} }
} }
} } // namespace internal
/* ************************************************************************* */ /* ************************************************************************* */
size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots,
const VectorValues& gradAtZero, VectorValues& RgProd) { const KeySet& replacedKeys,
const VectorValues& gradAtZero,
VectorValues* RgProd) {
// Update variables // Update variables
size_t varsUpdated = 0; size_t varsUpdated = 0;
for(const ISAM2::sharedClique& root: roots) { for (const ISAM2::sharedClique& root : roots) {
internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, varsUpdated); internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd,
&varsUpdated);
} }
return varsUpdated; return varsUpdated;
@ -361,8 +387,7 @@ size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replac
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero, VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero,
const VectorValues& RgProd) const VectorValues& RgProd) {
{
// Compute gradient squared-magnitude // Compute gradient squared-magnitude
const double gradientSqNorm = gradAtZero.dot(gradAtZero); const double gradientSqNorm = gradAtZero.dot(gradAtZero);
@ -374,4 +399,4 @@ VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero,
return step * 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 * Apply expmap to the given values, but only for indices appearing in
* \c markedRelinMask. Values are expmapped in-place. * \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 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 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, * \param invalidateIfDebug If this is true, *and* NDEBUG is not defined,
* expmapped deltas will be set to an invalid value (infinity) to catch bugs * 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 * where we might expmap something twice, or expmap it but then not
* recalculate its delta. * recalculate its delta.
* @param keyFormatter Formatter for printing nonlinear keys during debugging * @param keyFormatter Formatter for printing nonlinear keys during debugging
*/ */
static void ExpmapMasked(Values& values, const VectorValues& delta, static void ExpmapMasked(
const KeySet& mask, const VectorValues& delta, const KeySet& mask, Values* values,
boost::optional<VectorValues&> invalidateIfDebug = boost::none, boost::optional<VectorValues&> invalidateIfDebug = boost::none,
const KeyFormatter& keyFormatter = DefaultKeyFormatter); const KeyFormatter& keyFormatter = DefaultKeyFormatter);
@ -127,14 +126,14 @@ struct GTSAM_EXPORT ISAM2::Impl {
* Update the Newton's method step point, using wildfire * Update the Newton's method step point, using wildfire
*/ */
static size_t UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots, 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 * Update the RgProd (R*g) incrementally taking into account which variables
* have been recalculated in \c replacedKeys. Only used in Dogleg. * have been recalculated in \c replacedKeys. Only used in Dogleg.
*/ */
static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, 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. * Compute the gradient-search point. Only used in Dogleg.

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

@ -16,296 +16,19 @@
* @date Mar 16, 2012 * @date Mar 16, 2012
*/ */
#pragma once #pragma once
#include <stack> #include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUE> template <class VALUE>
VALUE ISAM2::calculateEstimate(Key key) const { VALUE ISAM2::calculateEstimate(Key key) const {
const Vector& delta = getDelta()[key]; const Vector& delta = getDelta()[key];
return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta); 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_; Base::FactorType::shared_ptr cachedFactor_;
Vector gradientContribution_; Vector gradientContribution_;
FastMap<Key, VectorValues::iterator> solnPointers_; mutable FastMap<Key, VectorValues::iterator> solnPointers_;
/// Default constructor /// Default constructor
ISAM2Clique() : Base() {} ISAM2Clique() : Base() {}
@ -546,6 +546,45 @@ class GTSAM_EXPORT ISAM2Clique
void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const; 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: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
@ -810,30 +849,23 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
template <> template <>
struct traits<ISAM2> : public Testable<ISAM2> {}; struct traits<ISAM2> : public Testable<ISAM2> {};
/// Optimize the BayesTree, starting from the root. /**
/// @param replaced Needs to contain * Optimize the BayesTree, starting from the root.
/// all variables that are contained in the top of the Bayes tree that has been * @param threshold The maximum change against the PREVIOUS delta for
/// redone. * non-replaced variables that can be ignored, ie. the old delta entry is kept
/// @param delta The current solution, an offset from the linearization * and recursive backsubstitution might eventually stop if none of the changed
/// point. * variables are contained in the subtree.
/// @param threshold The maximum change against the PREVIOUS delta for * @param replaced Needs to contain all variables that are contained in the top
/// non-replaced variables that can be ignored, ie. the old delta entry is kept * of the Bayes tree that has been redone.
/// and recursive backsubstitution might eventually stop if none of the changed * @return The number of variables that were solved for.
/// variables are contained in the subtree. * @param delta The current solution, an offset from the linearization point.
/// @return The number of variables that were solved for */
template <class CLIQUE> size_t optimizeWildfire(const ISAM2::sharedClique& root, double threshold,
size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const KeySet& replaced, VectorValues* delta);
const KeySet& replaced, VectorValues& delta);
template <class CLIQUE> size_t optimizeWildfireNonRecursive(const ISAM2::sharedClique& root,
size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root,
double threshold, const KeySet& replaced, double threshold, const KeySet& replaced,
VectorValues& delta); 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);
} // namespace gtsam } // namespace gtsam

2
tests/testGaussianISAM2.cpp
View File

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