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

Merge pull request #42 from borglab/feature/iSAM2_refactor 

iSAM2 refactor
release/4.3a0
Frank Dellaert 2019-06-03 22:05:37 -04:00 committed by GitHub
parent 161e363190 5667c6ffdd
commit e9d2f8775f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
17 changed files with 1017 additions and 1064 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/inference/BayesTree.h
View File

@ -89,14 +89,14 @@ namespace gtsam {
/** Map from keys to Clique */ /** Map from keys to Clique */
typedef ConcurrentMap<Key, sharedClique> Nodes; typedef ConcurrentMap<Key, sharedClique> Nodes;
/** Root cliques */
typedef FastVector<sharedClique> Roots;
protected: protected:
/** Map from indices to Clique */ /** Map from indices to Clique */
Nodes nodes_; Nodes nodes_;
/** Root cliques */
typedef FastVector<sharedClique> Roots;
/** Root cliques */ /** Root cliques */
Roots roots_; Roots roots_;

2
gtsam/inference/EliminationTree-inst.h
View File

@ -98,7 +98,7 @@ namespace gtsam {
for (size_t j = 0; j < n; j++) for (size_t j = 0; j < n; j++)
{ {
// Retrieve the factors involving this variable and create the current node // Retrieve the factors involving this variable and create the current node
const VariableIndex::Factors& factors = structure[order[j]]; const FactorIndices& factors = structure[order[j]];
const sharedNode node = boost::make_shared<Node>(); const sharedNode node = boost::make_shared<Node>();
node->key = order[j]; node->key = order[j];

2
gtsam/inference/Ordering.cpp
View File

@ -79,7 +79,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
size_t index = 0; size_t index = 0;
for (auto key_factors: variableIndex) { for (auto key_factors: variableIndex) {
// Arrange factor indices into COLAMD format // Arrange factor indices into COLAMD format
const VariableIndex::Factors& column = key_factors.second; const FactorIndices& column = key_factors.second;
for(size_t factorIndex: column) { for(size_t factorIndex: column) {
A[count++] = (int) factorIndex; // copy sparse column A[count++] = (int) factorIndex; // copy sparse column
} }

4
gtsam/inference/VariableIndex-inl.h
View File

@ -67,8 +67,8 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor,
"Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove"); "Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
if (factors[i]) { if (factors[i]) {
for(Key j: *factors[i]) { for(Key j: *factors[i]) {
Factors& factorEntries = internalAt(j); FactorIndices& factorEntries = internalAt(j);
Factors::iterator entry = std::find(factorEntries.begin(), auto entry = std::find(factorEntries.begin(),
factorEntries.end(), *factorIndex); factorEntries.end(), *factorIndex);
if (entry == factorEntries.end()) if (entry == factorEntries.end())
throw std::invalid_argument( throw std::invalid_argument(

31
gtsam/inference/VariableIndex.h
View File

@ -42,14 +42,12 @@ namespace gtsam {
*/ */
class GTSAM_EXPORT VariableIndex { class GTSAM_EXPORT VariableIndex {
public: public:
typedef boost::shared_ptr<VariableIndex> shared_ptr; typedef boost::shared_ptr<VariableIndex> shared_ptr;
typedef FactorIndices Factors; typedef FactorIndices::iterator Factor_iterator;
typedef Factors::iterator Factor_iterator; typedef FactorIndices::const_iterator Factor_const_iterator;
typedef Factors::const_iterator Factor_const_iterator;
protected: protected:
typedef FastMap<Key,Factors> KeyMap; typedef FastMap<Key, FactorIndices> KeyMap;
KeyMap index_; KeyMap index_;
size_t nFactors_; // Number of factors in the original factor graph. size_t nFactors_; // Number of factors in the original factor graph.
size_t nEntries_; // Sum of involved variable counts of each factor. size_t nEntries_; // Sum of involved variable counts of each factor.
@ -59,8 +57,6 @@ public:
typedef KeyMap::const_iterator iterator; typedef KeyMap::const_iterator iterator;
typedef KeyMap::value_type value_type; typedef KeyMap::value_type value_type;
public:
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -72,7 +68,9 @@ public:
* of a factor graph. * of a factor graph.
*/ */
template <class FG> template <class FG>
VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) { augment(factorGraph); } explicit VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) {
augment(factorGraph);
}
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
@ -88,7 +86,7 @@ public:
size_t nEntries() const { return nEntries_; } size_t nEntries() const { return nEntries_; }
/// Access a list of factors by variable /// Access a list of factors by variable
const Factors& operator[](Key variable) const { const FactorIndices& operator[](Key variable) const {
KeyMap::const_iterator item = index_.find(variable); KeyMap::const_iterator item = index_.find(variable);
if(item == index_.end()) if(item == index_.end())
throw std::invalid_argument("Requested non-existent variable from VariableIndex"); throw std::invalid_argument("Requested non-existent variable from VariableIndex");
@ -96,6 +94,11 @@ public:
return item->second; return item->second;
} }
/// Return true if no factors associated with a variable
const bool empty(Key variable) const {
return (*this)[variable].empty();
}
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{
@ -166,16 +169,18 @@ protected:
Factor_const_iterator factorsEnd(Key variable) const { return internalAt(variable).end(); } Factor_const_iterator factorsEnd(Key variable) const { return internalAt(variable).end(); }
/// Internal version of 'at' that asserts existence /// Internal version of 'at' that asserts existence
const Factors& internalAt(Key variable) const { const FactorIndices& internalAt(Key variable) const {
const KeyMap::const_iterator item = index_.find(variable); const KeyMap::const_iterator item = index_.find(variable);
assert(item != index_.end()); assert(item != index_.end());
return item->second; } return item->second;
}
/// Internal version of 'at' that asserts existence /// Internal version of 'at' that asserts existence
Factors& internalAt(Key variable) { FactorIndices& internalAt(Key variable) {
const KeyMap::iterator item = index_.find(variable); const KeyMap::iterator item = index_.find(variable);
assert(item != index_.end()); assert(item != index_.end());
return item->second; } return item->second;
}
/// @} /// @}
}; };

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

@ -31,150 +31,6 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors,
bool useUnusedSlots,
NonlinearFactorGraph* nonlinearFactors,
FactorIndices* newFactorIndices) {
newFactorIndices->resize(newFactors.size());
if (useUnusedSlots) {
size_t globalFactorIndex = 0;
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;
else
break;
} while (true);
// Use the current slot, updating nonlinearFactors and newFactorSlots.
(*nonlinearFactors)[globalFactorIndex] = newFactors[newFactorIndex];
(*newFactorIndices)[newFactorIndex] = globalFactorIndex;
}
} 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)
(*newFactorIndices)[i] = i + nonlinearFactors->size();
nonlinearFactors->push_back(newFactors);
}
}
/* ************************************************************************* */
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) {
double maxDelta = key_delta.second.lpNorm<Eigen::Infinity>();
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())
relinKeys.insert(key_delta.first);
}
}
return relinKeys;
}
/* ************************************************************************* */
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()) {
double maxDelta = delta[var].lpNorm<Eigen::Infinity>();
if (maxDelta >= threshold) {
relinKeys->insert(var);
relinearize = true;
}
}
// If this node was relinearized, also check its children
if (relinearize) {
for (const ISAM2::sharedClique& child : clique->children) {
CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys);
}
}
}
/* ************************************************************************* */
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()) {
// 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.");
// Check for relinearization
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 ISAM2::sharedClique& child : clique->children) {
CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys);
}
}
}
/* ************************************************************************* */
KeySet ISAM2::Impl::CheckRelinearizationPartial(
const ISAM2::Roots& roots, const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
KeySet relinKeys;
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;
}
/* ************************************************************************* */ /* ************************************************************************* */
namespace internal { namespace internal {
inline static void optimizeInPlace(const ISAM2::sharedClique& clique, inline static void optimizeInPlace(const ISAM2::sharedClique& clique,
@ -189,7 +45,7 @@ inline static void optimizeInPlace(const ISAM2::sharedClique& clique,
} // namespace internal } // namespace internal
/* ************************************************************************* */ /* ************************************************************************* */
size_t ISAM2::Impl::UpdateGaussNewtonDelta(const ISAM2::Roots& roots, size_t DeltaImpl::UpdateGaussNewtonDelta(const ISAM2::Roots& roots,
const KeySet& replacedKeys, const KeySet& replacedKeys,
double wildfireThreshold, double wildfireThreshold,
VectorValues* delta) { VectorValues* delta) {
@ -272,7 +128,7 @@ void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys,
} // namespace internal } // namespace internal
/* ************************************************************************* */ /* ************************************************************************* */
size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, size_t DeltaImpl::UpdateRgProd(const ISAM2::Roots& roots,
const KeySet& replacedKeys, const KeySet& replacedKeys,
const VectorValues& gradAtZero, const VectorValues& gradAtZero,
VectorValues* RgProd) { VectorValues* RgProd) {
@ -287,7 +143,7 @@ size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots,
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero, VectorValues DeltaImpl::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);

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

@ -11,18 +11,65 @@
/** /**
* @file ISAM2-impl.h * @file ISAM2-impl.h
* @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid
* @author Michael Kaess, Richard Roberts * relinearization.
* @author Michael Kaess, Richard Roberts, Frank Dellaert
*/ */
#pragma once #pragma once
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/nonlinear/ISAM2.h> #include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/ISAM2Result.h>
#include <gtsam/base/debug.h>
#include <gtsam/inference/JunctionTree-inst.h> // We need the inst file because we'll make a special JT templated on ISAM2
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm/copy.hpp>
namespace br {
using namespace boost::range;
using namespace boost::adaptors;
} // namespace br
#include <algorithm>
#include <limits>
#include <string>
#include <utility>
namespace gtsam { namespace gtsam {
struct GTSAM_EXPORT ISAM2::Impl { /* ************************************************************************* */
// Special BayesTree class that uses ISAM2 cliques - this is the result of
// reeliminating ISAM2 subtrees.
class ISAM2BayesTree : public ISAM2::Base {
public:
typedef ISAM2::Base Base;
typedef ISAM2BayesTree This;
typedef boost::shared_ptr<This> shared_ptr;
ISAM2BayesTree() {}
};
/* ************************************************************************* */
// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for
// reeliminating ISAM2 subtrees.
class ISAM2JunctionTree
: public JunctionTree<ISAM2BayesTree, GaussianFactorGraph> {
public:
typedef JunctionTree<ISAM2BayesTree, GaussianFactorGraph> Base;
typedef ISAM2JunctionTree This;
typedef boost::shared_ptr<This> shared_ptr;
explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree)
: Base(eliminationTree) {}
};
/* ************************************************************************* */
struct GTSAM_EXPORT DeltaImpl {
struct GTSAM_EXPORT PartialSolveResult { struct GTSAM_EXPORT PartialSolveResult {
ISAM2::sharedClique bayesTree; ISAM2::sharedClique bayesTree;
}; };
@ -34,57 +81,468 @@ struct GTSAM_EXPORT ISAM2::Impl {
boost::optional<FastMap<Key, int> > constrainedKeys; boost::optional<FastMap<Key, int> > constrainedKeys;
}; };
/// Perform the first part of the bookkeeping updates for adding new factors. Adds them to the
/// complete list of nonlinear factors, and populates the list of new factor indices, both
/// optionally finding and reusing empty factor slots.
static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots,
NonlinearFactorGraph* nonlinearFactors, FactorIndices* newFactorIndices);
/**
* 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 KeySet CheckRelinearizationFull(const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
/**
* Find the set of variables to be relinearized according to relinearizeThreshold.
* This check is performed recursively, starting at the top of the tree. Once a
* variable in the tree does not need to be relinearized, no further checks in
* that branch are performed. This is an approximation of the Full version, designed
* to save time at the expense of accuracy.
* @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 KeySet CheckRelinearizationPartial(const ISAM2::Roots& roots,
const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
/** /**
* Update the Newton's method step point, using wildfire * Update the Newton's method step point, using wildfire
*/ */
static size_t UpdateGaussNewtonDelta(const ISAM2::Roots& roots, static size_t UpdateGaussNewtonDelta(const ISAM2::Roots& roots,
const KeySet& replacedKeys, double wildfireThreshold, VectorValues* delta); 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 VectorValues& gradAtZero, VectorValues* RgProd); const KeySet& replacedKeys,
const VectorValues& gradAtZero,
VectorValues* RgProd);
/** /**
* Compute the gradient-search point. Only used in Dogleg. * Compute the gradient-search point. Only used in Dogleg.
*/ */
static VectorValues ComputeGradientSearch(const VectorValues& gradAtZero, static VectorValues ComputeGradientSearch(const VectorValues& gradAtZero,
const VectorValues& RgProd); const VectorValues& RgProd);
}; };
/* ************************************************************************* */
/**
* Implementation functions for update method
* All of the methods below have clear inputs and outputs, even if not
* functional: iSAM2 is inherintly imperative.
*/
struct GTSAM_EXPORT UpdateImpl {
const ISAM2Params& params_;
const ISAM2UpdateParams& updateParams_;
UpdateImpl(const ISAM2Params& params, const ISAM2UpdateParams& updateParams)
: params_(params), updateParams_(updateParams) {}
// Provide some debugging information at the start of update
static void LogStartingUpdate(const NonlinearFactorGraph& newFactors,
const ISAM2& isam2) {
gttic(pushBackFactors);
const bool debug = ISDEBUG("ISAM2 update");
const bool verbose = ISDEBUG("ISAM2 update verbose");
if (verbose) {
std::cout << "ISAM2::update\n";
isam2.print("ISAM2: ");
} }
if (debug || verbose) {
newFactors.print("The new factors are: ");
}
}
// Check relinearization if we're at the nth step, or we are using a looser
// loop relinerization threshold.
bool relinarizationNeeded(size_t update_count) const {
return updateParams_.force_relinearize ||
(params_.enableRelinearization &&
update_count % params_.relinearizeSkip == 0);
}
// Add any new factors \Factors:=\Factors\cup\Factors'.
void pushBackFactors(const NonlinearFactorGraph& newFactors,
NonlinearFactorGraph* nonlinearFactors,
GaussianFactorGraph* linearFactors,
VariableIndex* variableIndex,
FactorIndices* newFactorsIndices,
KeySet* keysWithRemovedFactors) const {
gttic(pushBackFactors);
// Perform the first part of the bookkeeping updates for adding new factors.
// Adds them to the complete list of nonlinear factors, and populates the
// list of new factor indices, both optionally finding and reusing empty
// factor slots.
*newFactorsIndices = nonlinearFactors->add_factors(
newFactors, params_.findUnusedFactorSlots);
// Remove the removed factors
NonlinearFactorGraph removedFactors;
removedFactors.reserve(updateParams_.removeFactorIndices.size());
for (const auto index : updateParams_.removeFactorIndices) {
removedFactors.push_back(nonlinearFactors->at(index));
nonlinearFactors->remove(index);
if (params_.cacheLinearizedFactors) linearFactors->remove(index);
}
// Remove removed factors from the variable index so we do not attempt to
// relinearize them
variableIndex->remove(updateParams_.removeFactorIndices.begin(),
updateParams_.removeFactorIndices.end(),
removedFactors);
*keysWithRemovedFactors = removedFactors.keys();
}
// Get keys from removed factors and new factors, and compute unused keys,
// i.e., keys that are empty now and do not appear in the new factors.
void computeUnusedKeys(const NonlinearFactorGraph& newFactors,
const VariableIndex& variableIndex,
const KeySet& keysWithRemovedFactors,
KeySet* unusedKeys) const {
gttic(computeUnusedKeys);
KeySet removedAndEmpty;
for (Key key : keysWithRemovedFactors) {
if (variableIndex.empty(key))
removedAndEmpty.insert(removedAndEmpty.end(), key);
}
KeySet newFactorSymbKeys = newFactors.keys();
std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(),
newFactorSymbKeys.begin(), newFactorSymbKeys.end(),
std::inserter(*unusedKeys, unusedKeys->end()));
}
// Calculate nonlinear error
void error(const NonlinearFactorGraph& nonlinearFactors,
const Values& estimate, boost::optional<double>* result) const {
gttic(error);
result->reset(nonlinearFactors.error(estimate));
}
// Mark linear update
void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors,
const NonlinearFactorGraph& nonlinearFactors,
const KeySet& keysWithRemovedFactors,
KeySet* markedKeys) const {
gttic(gatherInvolvedKeys);
*markedKeys = newFactors.keys(); // Get keys from new factors
// Also mark keys involved in removed factors
markedKeys->insert(keysWithRemovedFactors.begin(),
keysWithRemovedFactors.end());
// Also mark any provided extra re-eliminate keys
if (updateParams_.extraReelimKeys) {
for (Key key : *updateParams_.extraReelimKeys) {
markedKeys->insert(key);
}
}
// Also, keys that were not observed in existing factors, but whose affected
// keys have been extended now (e.g. smart factors)
if (updateParams_.newAffectedKeys) {
for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) {
const auto factorIdx = factorAddedKeys.first;
const auto& affectedKeys = nonlinearFactors.at(factorIdx)->keys();
markedKeys->insert(affectedKeys.begin(), affectedKeys.end());
}
}
}
// Update detail, unused, and observed keys from markedKeys
void updateKeys(const KeySet& markedKeys, ISAM2Result* result) const {
gttic(updateKeys);
// Observed keys for detailed results
if (result->detail && params_.enableDetailedResults) {
for (Key key : markedKeys) {
result->detail->variableStatus[key].isObserved = true;
}
}
for (Key index : markedKeys) {
// Only add if not unused
if (result->unusedKeys.find(index) == result->unusedKeys.end())
// Make a copy of these, as we'll soon add to them
result->observedKeys.push_back(index);
}
}
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()) {
// 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.");
// Check for relinearization
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 ISAM2::sharedClique& child : clique->children) {
CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys);
}
}
}
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()) {
double maxDelta = delta[var].lpNorm<Eigen::Infinity>();
if (maxDelta >= threshold) {
relinKeys->insert(var);
relinearize = true;
}
}
// If this node was relinearized, also check its children
if (relinearize) {
for (const ISAM2::sharedClique& child : clique->children) {
CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys);
}
}
}
/**
* Find the set of variables to be relinearized according to
* relinearizeThreshold. This check is performed recursively, starting at
* the top of the tree. Once a variable in the tree does not need to be
* relinearized, no further checks in that branch are performed. This is an
* approximation of the Full version, designed to save time at the expense
* of accuracy.
* @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 KeySet CheckRelinearizationPartial(
const ISAM2::Roots& roots, const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
KeySet relinKeys;
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;
}
/**
* 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 KeySet 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) {
double maxDelta = key_delta.second.lpNorm<Eigen::Infinity>();
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())
relinKeys.insert(key_delta.first);
}
}
return relinKeys;
}
// Mark keys in \Delta above threshold \beta:
KeySet gatherRelinearizeKeys(const ISAM2::Roots& roots,
const VectorValues& delta,
const KeySet& fixedVariables,
KeySet* markedKeys) const {
gttic(gatherRelinearizeKeys);
// J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
KeySet relinKeys =
params_.enablePartialRelinearizationCheck
? CheckRelinearizationPartial(roots, delta,
params_.relinearizeThreshold)
: CheckRelinearizationFull(delta, params_.relinearizeThreshold);
if (updateParams_.forceFullSolve)
relinKeys = CheckRelinearizationFull(delta, 0.0); // for debugging
// Remove from relinKeys any keys whose linearization points are fixed
for (Key key : fixedVariables) {
relinKeys.erase(key);
}
if (updateParams_.noRelinKeys) {
for (Key key : *updateParams_.noRelinKeys) {
relinKeys.erase(key);
}
}
// Add the variables being relinearized to the marked keys
markedKeys->insert(relinKeys.begin(), relinKeys.end());
return relinKeys;
}
// Record relinerization threshold keys in detailed results
void recordRelinearizeDetail(const KeySet& relinKeys,
ISAM2Result::DetailedResults* detail) const {
if (detail && params_.enableDetailedResults) {
for (Key key : relinKeys) {
detail->variableStatus[key].isAboveRelinThreshold = true;
detail->variableStatus[key].isRelinearized = true;
}
}
}
// Mark all cliques that involve marked variables \Theta_{J} and all
// their ancestors.
void findFluid(const ISAM2::Roots& roots, const KeySet& relinKeys,
KeySet* markedKeys,
ISAM2Result::DetailedResults* detail) const {
gttic(findFluid);
for (const auto& root : roots)
// add other cliques that have the marked ones in the separator
root->findAll(relinKeys, markedKeys);
// Relinearization-involved keys for detailed results
if (detail && params_.enableDetailedResults) {
KeySet involvedRelinKeys;
for (const auto& root : roots)
root->findAll(relinKeys, &involvedRelinKeys);
for (Key key : involvedRelinKeys) {
if (!detail->variableStatus[key].isAboveRelinThreshold) {
detail->variableStatus[key].isRelinearizeInvolved = true;
detail->variableStatus[key].isRelinearized = true;
}
}
}
}
/**
* Apply expmap to the given values, but only for indices appearing in
* \c mask. Values are expmapped in-place.
* \param mask Mask on linear indices, only \c true entries are expmapped
*/
static void ExpmapMasked(const VectorValues& delta, const KeySet& mask,
Values* theta) {
gttic(ExpmapMasked);
assert(theta->size() == delta.size());
Values::iterator key_value;
VectorValues::const_iterator key_delta;
#ifdef GTSAM_USE_TBB
for (key_value = theta->begin(); key_value != theta->end(); ++key_value) {
key_delta = delta.find(key_value->key);
#else
for (key_value = theta->begin(), key_delta = delta.begin();
key_value != theta->end(); ++key_value, ++key_delta) {
assert(key_value->key == key_delta->first);
#endif
Key var = key_value->key;
assert(static_cast<size_t>(delta[var].size()) == key_value->value.dim());
assert(delta[var].allFinite());
if (mask.exists(var)) {
Value* retracted = key_value->value.retract_(delta[var]);
key_value->value = *retracted;
retracted->deallocate_();
}
}
}
// Linearize new factors
void linearizeNewFactors(const NonlinearFactorGraph& newFactors,
const Values& theta, size_t numNonlinearFactors,
const FactorIndices& newFactorsIndices,
GaussianFactorGraph* linearFactors) const {
gttic(linearizeNewFactors);
auto linearized = newFactors.linearize(theta);
if (params_.findUnusedFactorSlots) {
linearFactors->resize(numNonlinearFactors);
for (size_t i = 0; i < newFactors.size(); ++i)
(*linearFactors)[newFactorsIndices[i]] = (*linearized)[i];
} else {
linearFactors->push_back(*linearized);
}
assert(linearFactors->size() == numNonlinearFactors);
}
void augmentVariableIndex(const NonlinearFactorGraph& newFactors,
const FactorIndices& newFactorsIndices,
VariableIndex* variableIndex) const {
gttic(augmentVariableIndex);
// Augment the variable index with the new factors
if (params_.findUnusedFactorSlots)
variableIndex->augment(newFactors, newFactorsIndices);
else
variableIndex->augment(newFactors);
// Augment it with existing factors which now affect to more variables:
if (updateParams_.newAffectedKeys) {
for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) {
const auto factorIdx = factorAddedKeys.first;
variableIndex->augmentExistingFactor(factorIdx, factorAddedKeys.second);
}
}
}
static void LogRecalculateKeys(const ISAM2Result& result) {
const bool debug = ISDEBUG("ISAM2 recalculate");
if (debug) {
std::cout << "markedKeys: ";
for (const Key key : result.markedKeys) {
std::cout << key << " ";
}
std::cout << std::endl;
std::cout << "observedKeys: ";
for (const Key key : result.observedKeys) {
std::cout << key << " ";
}
std::cout << std::endl;
}
}
static FactorIndexSet GetAffectedFactors(const KeyList& keys,
const VariableIndex& variableIndex) {
gttic(GetAffectedFactors);
FactorIndexSet indices;
for (const Key key : keys) {
const FactorIndices& factors(variableIndex[key]);
indices.insert(factors.begin(), factors.end());
}
return indices;
}
// find intermediate (linearized) factors from cache that are passed into
// the affected area
static GaussianFactorGraph GetCachedBoundaryFactors(
const ISAM2::Cliques& orphans) {
GaussianFactorGraph cachedBoundary;
for (const auto& orphan : orphans) {
// retrieve the cached factor and add to boundary
cachedBoundary.push_back(orphan->cachedFactor());
}
return cachedBoundary;
}
};
} // namespace gtsam

715
gtsam/nonlinear/ISAM2.cpp
View File

@ -16,24 +16,16 @@
* @author Michael Kaess, Richard Roberts, Frank Dellaert * @author Michael Kaess, Richard Roberts, Frank Dellaert
*/ */
#include <gtsam/nonlinear/ISAM2-impl.h>
#include <gtsam/nonlinear/ISAM2.h> #include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/ISAM2Result.h>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/inference/BayesTree-inst.h> #include <gtsam/inference/BayesTree-inst.h>
#include <gtsam/inference/JunctionTree-inst.h> // We need the inst file because we'll make a special JT templated on ISAM2
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/nonlinear/LinearContainerFactor.h> #include <gtsam/nonlinear/LinearContainerFactor.h>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm/copy.hpp>
namespace br {
using namespace boost::range;
using namespace boost::adaptors;
} // namespace br
#include <algorithm> #include <algorithm>
#include <limits>
#include <map> #include <map>
#include <utility> #include <utility>
@ -44,35 +36,6 @@ namespace gtsam {
// Instantiate base class // Instantiate base class
template class BayesTree<ISAM2Clique>; template class BayesTree<ISAM2Clique>;
static const bool kDisableReordering = false;
static const double kBatchThreshold = 0.65;
/* ************************************************************************* */
// Special BayesTree class that uses ISAM2 cliques - this is the result of
// reeliminating ISAM2 subtrees.
class ISAM2BayesTree : public ISAM2::Base {
public:
typedef ISAM2::Base Base;
typedef ISAM2BayesTree This;
typedef boost::shared_ptr<This> shared_ptr;
ISAM2BayesTree() {}
};
/* ************************************************************************* */
// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for
// reeliminating ISAM2 subtrees.
class ISAM2JunctionTree
: public JunctionTree<ISAM2BayesTree, GaussianFactorGraph> {
public:
typedef JunctionTree<ISAM2BayesTree, GaussianFactorGraph> Base;
typedef ISAM2JunctionTree This;
typedef boost::shared_ptr<This> shared_ptr;
explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree)
: Base(eliminationTree) {}
};
/* ************************************************************************* */ /* ************************************************************************* */
ISAM2::ISAM2(const ISAM2Params& params) : params_(params), update_count_(0) { ISAM2::ISAM2(const ISAM2Params& params) : params_(params), update_count_(0) {
if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
@ -96,40 +59,12 @@ bool ISAM2::equals(const ISAM2& other, double tol) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
FactorIndexSet ISAM2::getAffectedFactors(const KeyList& keys) const { GaussianFactorGraph ISAM2::relinearizeAffectedFactors(
static const bool debug = false; const ISAM2UpdateParams& updateParams, const FastList<Key>& affectedKeys,
if (debug) cout << "Getting affected factors for "; const KeySet& relinKeys) {
if (debug) { gttic(relinearizeAffectedFactors);
for (const Key key : keys) { FactorIndexSet candidates =
cout << key << " "; UpdateImpl::GetAffectedFactors(affectedKeys, variableIndex_);
}
}
if (debug) cout << endl;
FactorIndexSet indices;
for (const Key key : keys) {
const VariableIndex::Factors& factors(variableIndex_[key]);
indices.insert(factors.begin(), factors.end());
}
if (debug) cout << "Affected factors are: ";
if (debug) {
for (const auto 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)
GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors(
const FastList<Key>& affectedKeys, const KeySet& relinKeys) const {
gttic(getAffectedFactors);
FactorIndexSet candidates = getAffectedFactors(affectedKeys);
gttoc(getAffectedFactors);
gttic(affectedKeysSet); gttic(affectedKeysSet);
// for fast lookup below // for fast lookup below
@ -138,7 +73,7 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors(
gttoc(affectedKeysSet); gttoc(affectedKeysSet);
gttic(check_candidates_and_linearize); gttic(check_candidates_and_linearize);
auto linearized = boost::make_shared<GaussianFactorGraph>(); GaussianFactorGraph linearized;
for (const FactorIndex idx : candidates) { for (const FactorIndex idx : candidates) {
bool inside = true; bool inside = true;
bool useCachedLinear = params_.cacheLinearizedFactors; bool useCachedLinear = params_.cacheLinearizedFactors;
@ -156,10 +91,10 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors(
assert(linearFactors_[idx]); assert(linearFactors_[idx]);
assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->keys()); assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->keys());
#endif #endif
linearized->push_back(linearFactors_[idx]); linearized.push_back(linearFactors_[idx]);
} else { } else {
auto linearFactor = nonlinearFactors_[idx]->linearize(theta_); auto linearFactor = nonlinearFactors_[idx]->linearize(theta_);
linearized->push_back(linearFactor); linearized.push_back(linearFactor);
if (params_.cacheLinearizedFactors) { if (params_.cacheLinearizedFactors) {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
assert(linearFactors_[idx]->keys() == linearFactor->keys()); assert(linearFactors_[idx]->keys() == linearFactor->keys());
@ -175,92 +110,37 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors(
} }
/* ************************************************************************* */ /* ************************************************************************* */
// find intermediate (linearized) factors from cache that are passed into the void ISAM2::recalculate(const ISAM2UpdateParams& updateParams,
// affected area const KeySet& relinKeys, ISAM2Result* result) {
gttic(recalculate);
UpdateImpl::LogRecalculateKeys(*result);
GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) { if (!result->markedKeys.empty() || !result->observedKeys.empty()) {
GaussianFactorGraph cachedBoundary; // Remove top of Bayes tree and convert to a factor graph:
for (sharedClique orphan : orphans) {
// retrieve the cached factor and add to boundary
cachedBoundary.push_back(orphan->cachedFactor());
}
return cachedBoundary;
}
/* ************************************************************************* */
boost::shared_ptr<KeySet> ISAM2::recalculate(
const KeySet& markedKeys, const KeySet& relinKeys,
const KeyVector& observedKeys, const KeySet& unusedIndices,
const boost::optional<FastMap<Key, int> >& constrainKeys,
ISAM2Result* result) {
// TODO(dellaert): new factors are linearized twice,
// the newFactors passed in are not used.
const bool debug = ISDEBUG("ISAM2 recalculate");
// Input: BayesTree(this), newFactors
// 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: ";
for (const Key key : markedKeys) {
cout << key << " ";
}
cout << endl;
cout << "observedKeys: ";
for (const Key key : observedKeys) {
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 // (a) For each affected variable, remove the corresponding clique and all
// parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of // parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of
// removed cliques. // removed cliques.
gttic(removetop);
Cliques orphans;
GaussianBayesNet affectedBayesNet; GaussianBayesNet affectedBayesNet;
this->removeTop(KeyVector(markedKeys.begin(), markedKeys.end()), Cliques orphans;
this->removeTop(
KeyVector(result->markedKeys.begin(), result->markedKeys.end()),
&affectedBayesNet, &orphans); &affectedBayesNet, &orphans);
gttoc(removetop);
// FactorGraph<GaussianFactor> factors(affectedBayesNet); // FactorGraph<GaussianFactor> factors(affectedBayesNet);
// bug was here: we cannot reuse the original factors, because then the cached // bug was here: we cannot reuse the original factors, because then the
// factors get messed up [all the necessary data is actually contained in the // cached factors get messed up [all the necessary data is actually
// affectedBayesNet, including what was passed in from the boundaries, // contained in the affectedBayesNet, including what was passed in from the
// so this would be correct; however, in the process we also generate new // boundaries, so this would be correct; however, in the process we also
// cached_ entries that will be wrong (ie. they don't contain what would be // generate new cached_ entries that will be wrong (ie. they don't contain
// passed up at a certain point if batch elimination was done, but that's // what would be passed up at a certain point if batch elimination was done,
// what we need); we could choose not to update cached_ from here, but then // but that's what we need); we could choose not to update cached_ from
// the new information (and potentially different variable ordering) is not // here, but then the new information (and potentially different variable
// reflected in the cached_ values which again will be wrong] // ordering) is not reflected in the cached_ values which again will be
// so instead we have to retrieve the original linearized factors AND add the // wrong] so instead we have to retrieve the original linearized factors AND
// cached factors from the boundary // 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
// ordering provides all keys in conditionals, there cannot be others because
// path to root included
gttic(affectedKeys); gttic(affectedKeys);
FastList<Key> affectedKeys; FastList<Key> affectedKeys;
for (const auto& conditional : affectedBayesNet) for (const auto& conditional : affectedBayesNet)
@ -268,12 +148,32 @@ boost::shared_ptr<KeySet> ISAM2::recalculate(
conditional->endFrontals()); conditional->endFrontals());
gttoc(affectedKeys); gttoc(affectedKeys);
boost::shared_ptr<KeySet> affectedKeysSet( KeySet affectedKeysSet;
new KeySet()); // Will return this result static const double kBatchThreshold = 0.65;
if (affectedKeys.size() >= theta_.size() * kBatchThreshold) { if (affectedKeys.size() >= theta_.size() * kBatchThreshold) {
// Do a batch step - reorder and relinearize all variables // Do a batch step - reorder and relinearize all variables
gttic(batch); recalculateBatch(updateParams, &affectedKeysSet, result);
} else {
recalculateIncremental(updateParams, relinKeys, affectedKeys,
&affectedKeysSet, &orphans, result);
}
// Root clique variables for detailed results
if (result->detail && params_.enableDetailedResults) {
for (const auto& root : roots_)
for (Key var : *root->conditional())
result->detail->variableStatus[var].inRootClique = true;
}
// Update replaced keys mask (accumulates until back-substitution happens)
deltaReplacedMask_.insert(affectedKeysSet.begin(), affectedKeysSet.end());
}
}
/* ************************************************************************* */
void ISAM2::recalculateBatch(const ISAM2UpdateParams& updateParams,
KeySet* affectedKeysSet, ISAM2Result* result) {
gttic(recalculateBatch);
gttic(add_keys); gttic(add_keys);
br::copy(variableIndex_ | br::map_keys, br::copy(variableIndex_ | br::map_keys,
@ -282,24 +182,24 @@ boost::shared_ptr<KeySet> ISAM2::recalculate(
// Removed unused keys: // Removed unused keys:
VariableIndex affectedFactorsVarIndex = variableIndex_; VariableIndex affectedFactorsVarIndex = variableIndex_;
affectedFactorsVarIndex.removeUnusedVariables(unusedIndices.begin(), affectedFactorsVarIndex.removeUnusedVariables(result->unusedKeys.begin(),
unusedIndices.end()); result->unusedKeys.end());
for (const Key key : unusedIndices) { for (const Key key : result->unusedKeys) {
affectedKeysSet->erase(key); affectedKeysSet->erase(key);
} }
gttoc(add_keys); gttoc(add_keys);
gttic(ordering); gttic(ordering);
Ordering order; Ordering order;
if (constrainKeys) { if (updateParams.constrainedKeys) {
order = order = Ordering::ColamdConstrained(affectedFactorsVarIndex,
Ordering::ColamdConstrained(affectedFactorsVarIndex, *constrainKeys); *updateParams.constrainedKeys);
} else { } else {
if (theta_.size() > observedKeys.size()) { if (theta_.size() > result->observedKeys.size()) {
// Only if some variables are unconstrained // Only if some variables are unconstrained
FastMap<Key, int> constraintGroups; FastMap<Key, int> constraintGroups;
for (Key var : observedKeys) constraintGroups[var] = 1; for (Key var : result->observedKeys) constraintGroups[var] = 1;
order = Ordering::ColamdConstrained(affectedFactorsVarIndex, order = Ordering::ColamdConstrained(affectedFactorsVarIndex,
constraintGroups); constraintGroups);
} else { } else {
@ -309,62 +209,63 @@ boost::shared_ptr<KeySet> ISAM2::recalculate(
gttoc(ordering); gttoc(ordering);
gttic(linearize); gttic(linearize);
GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_); auto linearized = nonlinearFactors_.linearize(theta_);
if (params_.cacheLinearizedFactors) linearFactors_ = linearized; if (params_.cacheLinearizedFactors) linearFactors_ = *linearized;
gttoc(linearize); gttoc(linearize);
gttic(eliminate); gttic(eliminate);
ISAM2BayesTree::shared_ptr bayesTree = ISAM2BayesTree::shared_ptr bayesTree =
ISAM2JunctionTree( ISAM2JunctionTree(
GaussianEliminationTree(linearized, affectedFactorsVarIndex, order)) GaussianEliminationTree(*linearized, affectedFactorsVarIndex, order))
.eliminate(params_.getEliminationFunction()) .eliminate(params_.getEliminationFunction())
.first; .first;
gttoc(eliminate); gttoc(eliminate);
gttic(insert); gttic(insert);
this->clear(); roots_.clear();
this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), roots_.insert(roots_.end(), bayesTree->roots().begin(),
bayesTree->roots().end()); bayesTree->roots().end());
this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); nodes_.clear();
nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end());
gttoc(insert); gttoc(insert);
result->variablesReeliminated = affectedKeysSet->size(); result->variablesReeliminated = affectedKeysSet->size();
result->factorsRecalculated = nonlinearFactors_.size(); result->factorsRecalculated = nonlinearFactors_.size();
lastAffectedMarkedCount = markedKeys.size();
lastAffectedVariableCount = affectedKeysSet->size();
lastAffectedFactorCount = linearized.size();
// Reeliminated keys for detailed results // Reeliminated keys for detailed results
if (params_.enableDetailedResults) { if (params_.enableDetailedResults) {
for (Key key : theta_.keys()) { for (Key key : theta_.keys()) {
result->detail->variableStatus[key].isReeliminated = true; result->detail->variableStatus[key].isReeliminated = true;
} }
} }
}
gttoc(batch); /* ************************************************************************* */
void ISAM2::recalculateIncremental(const ISAM2UpdateParams& updateParams,
} else { const KeySet& relinKeys,
gttic(incremental); const FastList<Key>& affectedKeys,
KeySet* affectedKeysSet, Cliques* orphans,
ISAM2Result* result) {
gttic(recalculateIncremental);
const bool debug = ISDEBUG("ISAM2 recalculate");
// 2. Add the new factors \Factors' into the resulting factor graph // 2. Add the new factors \Factors' into the resulting factor graph
FastList<Key> affectedAndNewKeys; FastList<Key> affectedAndNewKeys;
affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(),
affectedKeys.end()); affectedKeys.end());
affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), affectedAndNewKeys.insert(affectedAndNewKeys.end(),
observedKeys.end()); result->observedKeys.begin(),
gttic(relinearizeAffected); result->observedKeys.end());
GaussianFactorGraph factors( GaussianFactorGraph factors =
*relinearizeAffectedFactors(affectedAndNewKeys, relinKeys)); relinearizeAffectedFactors(updateParams, affectedAndNewKeys, relinKeys);
if (debug) factors.print("Relinearized factors: ");
gttoc(relinearizeAffected);
if (debug) { if (debug) {
cout << "Affected keys: "; factors.print("Relinearized factors: ");
std::cout << "Affected keys: ";
for (const Key key : affectedKeys) { for (const Key key : affectedKeys) {
cout << key << " "; std::cout << key << " ";
} }
cout << endl; std::cout << std::endl;
} }
// Reeliminated keys for detailed results // Reeliminated keys for detailed results
@ -376,34 +277,22 @@ boost::shared_ptr<KeySet> ISAM2::recalculate(
result->variablesReeliminated = affectedAndNewKeys.size(); result->variablesReeliminated = affectedAndNewKeys.size();
result->factorsRecalculated = factors.size(); result->factorsRecalculated = factors.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
gttic(cached); gttic(cached);
// add the cached intermediate results from the boundary of the orphans ... // Add the cached intermediate results from the boundary of the orphans...
GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); GaussianFactorGraph cachedBoundary =
UpdateImpl::GetCachedBoundaryFactors(*orphans);
if (debug) cachedBoundary.print("Boundary factors: "); if (debug) cachedBoundary.print("Boundary factors: ");
factors.push_back(cachedBoundary); factors.push_back(cachedBoundary);
gttoc(cached); gttoc(cached);
gttic(orphans); gttic(orphans);
// Add the orphaned subtrees // Add the orphaned subtrees
for (const sharedClique& orphan : orphans) for (const auto& orphan : *orphans)
factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan); factors +=
boost::make_shared<BayesTreeOrphanWrapper<ISAM2::Clique> >(orphan);
gttoc(orphans); gttoc(orphans);
// END OF COPIED CODE
// 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm // 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:eliminate]), and re-assemble into a new Bayes tree (Algorithm
// [alg:BayesTree]) // [alg:BayesTree])
@ -412,9 +301,9 @@ boost::shared_ptr<KeySet> ISAM2::recalculate(
gttic(list_to_set); gttic(list_to_set);
// create a partial reordering for the new and contaminated factors // create a partial reordering for the new and contaminated factors
// markedKeys are passed in: those variables will be forced to the end in // result->markedKeys are passed in: those variables will be forced to the
// the ordering // end in the ordering
affectedKeysSet->insert(markedKeys.begin(), markedKeys.end()); affectedKeysSet->insert(result->markedKeys.begin(), result->markedKeys.end());
affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end());
gttoc(list_to_set); gttoc(list_to_set);
@ -423,21 +312,21 @@ boost::shared_ptr<KeySet> ISAM2::recalculate(
gttic(ordering_constraints); gttic(ordering_constraints);
// Create ordering constraints // Create ordering constraints
FastMap<Key, int> constraintGroups; FastMap<Key, int> constraintGroups;
if (constrainKeys) { if (updateParams.constrainedKeys) {
constraintGroups = *constrainKeys; constraintGroups = *updateParams.constrainedKeys;
} else { } else {
constraintGroups = FastMap<Key, int>(); constraintGroups = FastMap<Key, int>();
const int group = const int group =
observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; result->observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0;
for (Key var : observedKeys) for (Key var : result->observedKeys)
constraintGroups.insert(make_pair(var, group)); constraintGroups.insert(std::make_pair(var, group));
} }
// Remove unaffected keys from the constraints // Remove unaffected keys from the constraints
for (FastMap<Key, int>::iterator iter = constraintGroups.begin(); for (FastMap<Key, int>::iterator iter = constraintGroups.begin();
iter != constraintGroups.end(); iter != constraintGroups.end();
/*Incremented in loop ++iter*/) { /*Incremented in loop ++iter*/) {
if (unusedIndices.exists(iter->first) || if (result->unusedKeys.exists(iter->first) ||
!affectedKeysSet->exists(iter->first)) !affectedKeysSet->exists(iter->first))
constraintGroups.erase(iter++); constraintGroups.erase(iter++);
else else
@ -447,53 +336,50 @@ boost::shared_ptr<KeySet> ISAM2::recalculate(
// Generate ordering // Generate ordering
gttic(Ordering); gttic(Ordering);
Ordering ordering = const Ordering ordering =
Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups);
gttoc(Ordering); gttoc(Ordering);
ISAM2BayesTree::shared_ptr bayesTree = // Do elimination
ISAM2JunctionTree( GaussianEliminationTree etree(factors, affectedFactorsVarIndex, ordering);
GaussianEliminationTree(factors, affectedFactorsVarIndex, ordering)) auto bayesTree = ISAM2JunctionTree(etree)
.eliminate(params_.getEliminationFunction()) .eliminate(params_.getEliminationFunction())
.first; .first;
gttoc(reorder_and_eliminate); gttoc(reorder_and_eliminate);
gttic(reassemble); gttic(reassemble);
this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), roots_.insert(roots_.end(), bayesTree->roots().begin(),
bayesTree->roots().end()); bayesTree->roots().end());
this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end());
gttoc(reassemble); gttoc(reassemble);
// 4. The orphans have already been inserted during elimination // 4. The orphans have already been inserted during elimination
gttoc(incremental);
}
// Root clique variables for detailed results
if (params_.enableDetailedResults) {
for (const sharedNode& root : this->roots())
for (Key var : *root->conditional())
result->detail->variableStatus[var].inRootClique = true;
}
return affectedKeysSet;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ISAM2::addVariables(const Values& newTheta) { void ISAM2::addVariables(const Values& newTheta,
const bool debug = ISDEBUG("ISAM2 AddVariables"); ISAM2Result::DetailedResults* detail) {
gttic(addNewVariables);
theta_.insert(newTheta); theta_.insert(newTheta);
if (debug) newTheta.print("The new variables are: "); if (ISDEBUG("ISAM2 AddVariables")) 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());
RgProd_.insert(newTheta.zeroVectors()); RgProd_.insert(newTheta.zeroVectors());
// New keys for detailed results
if (detail && params_.enableDetailedResults) {
for (Key key : newTheta.keys()) {
detail->variableStatus[key].isNew = true;
}
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ISAM2::removeVariables(const KeySet& unusedKeys) { void ISAM2::removeVariables(const KeySet& unusedKeys) {
gttic(removeVariables);
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);
@ -506,36 +392,6 @@ void ISAM2::removeVariables(const KeySet& unusedKeys) {
} }
} }
/* ************************************************************************* */
void ISAM2::expmapMasked(const KeySet& mask) {
assert(theta_.size() == delta_.size());
Values::iterator key_value;
VectorValues::const_iterator key_delta;
#ifdef GTSAM_USE_TBB
for (key_value = theta_.begin(); key_value != theta_.end(); ++key_value) {
key_delta = delta_.find(key_value->key);
#else
for (key_value = theta_.begin(), key_delta = delta_.begin();
key_value != theta_.end(); ++key_value, ++key_delta) {
assert(key_value->key == key_delta->first);
#endif
Key var = key_value->key;
assert(static_cast<size_t>(delta_[var].size()) == key_value->value.dim());
assert(delta_[var].allFinite());
if (mask.exists(var)) {
Value* retracted = key_value->value.retract_(delta_[var]);
key_value->value = *retracted;
retracted->deallocate_();
#ifndef NDEBUG
// If debugging, invalidate delta_ entries to Inf, to trigger assertions
// if we try to re-use them.
delta_[var] = Vector::Constant(delta_[var].rows(),
numeric_limits<double>::infinity());
#endif
}
}
}
/* ************************************************************************* */ /* ************************************************************************* */
ISAM2Result ISAM2::update( ISAM2Result ISAM2::update(
const NonlinearFactorGraph& newFactors, const Values& newTheta, const NonlinearFactorGraph& newFactors, const Values& newTheta,
@ -544,7 +400,6 @@ ISAM2Result ISAM2::update(
const boost::optional<FastList<Key> >& noRelinKeys, const boost::optional<FastList<Key> >& noRelinKeys,
const boost::optional<FastList<Key> >& extraReelimKeys, const boost::optional<FastList<Key> >& extraReelimKeys,
bool force_relinearize) { bool force_relinearize) {
ISAM2UpdateParams params; ISAM2UpdateParams params;
params.constrainedKeys = constrainedKeys; params.constrainedKeys = constrainedKeys;
params.extraReelimKeys = extraReelimKeys; params.extraReelimKeys = extraReelimKeys;
@ -559,279 +414,64 @@ ISAM2Result ISAM2::update(
ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
const Values& newTheta, const Values& newTheta,
const ISAM2UpdateParams& updateParams) { const ISAM2UpdateParams& updateParams) {
const bool debug = ISDEBUG("ISAM2 update");
const bool verbose = ISDEBUG("ISAM2 update verbose");
gttic(ISAM2_update); gttic(ISAM2_update);
this->update_count_ += 1;
this->update_count_++; UpdateImpl::LogStartingUpdate(newFactors, *this);
ISAM2Result result(params_.enableDetailedResults);
lastAffectedVariableCount = 0; UpdateImpl update(params_, updateParams);
lastAffectedFactorCount = 0;
lastAffectedCliqueCount = 0;
lastAffectedMarkedCount = 0;
lastBacksubVariableCount = 0;
lastNnzTop = 0;
ISAM2Result result;
if (params_.enableDetailedResults)
result.detail = ISAM2Result::DetailedResults();
const bool relinearizeThisStep =
updateParams.force_relinearize || (params_.enableRelinearization &&
update_count_ % params_.relinearizeSkip == 0);
if (verbose) {
cout << "ISAM2::update\n";
this->print("ISAM2: ");
}
// Update delta if we need it to check relinearization later // Update delta if we need it to check relinearization later
if (relinearizeThisStep) { if (update.relinarizationNeeded(update_count_))
gttic(updateDelta); updateDelta(updateParams.forceFullSolve);
updateDelta(kDisableReordering);
gttoc(updateDelta);
}
gttic(push_back_factors);
// 1. Add any new factors \Factors:=\Factors\cup\Factors'. // 1. Add any new factors \Factors:=\Factors\cup\Factors'.
// Add the new factor indices to the result struct update.pushBackFactors(newFactors, &nonlinearFactors_, &linearFactors_,
if (debug || verbose) newFactors.print("The new factors are: "); &variableIndex_, &result.newFactorsIndices,
Impl::AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, &result.keysWithRemovedFactors);
&nonlinearFactors_, &result.newFactorsIndices); update.computeUnusedKeys(newFactors, variableIndex_,
result.keysWithRemovedFactors, &result.unusedKeys);
// Remove the removed factors
NonlinearFactorGraph removeFactors;
removeFactors.reserve(updateParams.removeFactorIndices.size());
for (const auto index : updateParams.removeFactorIndices) {
removeFactors.push_back(nonlinearFactors_[index]);
nonlinearFactors_.remove(index);
if (params_.cacheLinearizedFactors) linearFactors_.remove(index);
}
// Remove removed factors from the variable index so we do not attempt to
// relinearize them
variableIndex_.remove(updateParams.removeFactorIndices.begin(),
updateParams.removeFactorIndices.end(),
removeFactors);
// Compute unused keys and indices
KeySet unusedKeys;
KeySet unusedIndices;
{
// Get keys from removed factors and new factors, and compute unused keys,
// i.e., keys that are empty now and do not appear in the new factors.
KeySet removedAndEmpty;
for (Key key : removeFactors.keys()) {
if (variableIndex_[key].empty())
removedAndEmpty.insert(removedAndEmpty.end(), key);
}
KeySet newFactorSymbKeys = newFactors.keys();
std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(),
newFactorSymbKeys.begin(), newFactorSymbKeys.end(),
std::inserter(unusedKeys, unusedKeys.end()));
// Get indices for unused keys
for (Key key : unusedKeys) {
unusedIndices.insert(unusedIndices.end(), key);
}
}
gttoc(push_back_factors);
gttic(add_new_variables);
// 2. Initialize any new variables \Theta_{new} and add // 2. Initialize any new variables \Theta_{new} and add
// \Theta:=\Theta\cup\Theta_{new}. // \Theta:=\Theta\cup\Theta_{new}.
addVariables(newTheta); addVariables(newTheta, result.details());
// New keys for detailed results
if (params_.enableDetailedResults) {
for (Key key : newTheta.keys()) {
result.detail->variableStatus[key].isNew = true;
}
}
gttoc(add_new_variables);
gttic(evaluate_error_before);
if (params_.evaluateNonlinearError) if (params_.evaluateNonlinearError)
result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate())); update.error(nonlinearFactors_, calculateEstimate(), &result.errorBefore);
gttoc(evaluate_error_before);
gttic(gather_involved_keys);
// 3. Mark linear update // 3. Mark linear update
KeySet markedKeys = newFactors.keys(); // Get keys from new factors update.gatherInvolvedKeys(newFactors, nonlinearFactors_,
// Also mark keys involved in removed factors result.keysWithRemovedFactors, &result.markedKeys);
{ update.updateKeys(result.markedKeys, &result);
KeySet markedRemoveKeys =
removeFactors.keys(); // Get keys involved in removed factors
markedKeys.insert(
markedRemoveKeys.begin(),
markedRemoveKeys.end()); // Add to the overall set of marked keys
}
// Also mark any provided extra re-eliminate keys
if (updateParams.extraReelimKeys) {
for (Key key : *updateParams.extraReelimKeys) {
markedKeys.insert(key);
}
}
// Also, keys that were not observed in existing factors, but whose affected
// keys have been extended now (e.g. smart factors)
if (updateParams.newAffectedKeys) {
for (const auto &factorAddedKeys : *updateParams.newAffectedKeys) {
const auto factorIdx = factorAddedKeys.first;
const auto& affectedKeys = nonlinearFactors_.at(factorIdx)->keys();
markedKeys.insert(affectedKeys.begin(),affectedKeys.end());
}
}
// Observed keys for detailed results
if (params_.enableDetailedResults) {
for (Key key : markedKeys) {
result.detail->variableStatus[key].isObserved = true;
}
}
KeyVector observedKeys;
for (Key index : markedKeys) {
// Only add if not unused
if (unusedIndices.find(index) == unusedIndices.end())
// Make a copy of these, as we'll soon add to them
observedKeys.push_back(index);
}
gttoc(gather_involved_keys);
// Check relinearization if we're at the nth step, or we are using a looser
// loop relin threshold
KeySet relinKeys; KeySet relinKeys;
if (relinearizeThisStep) { result.variablesRelinearized = 0;
gttic(gather_relinearize_keys); if (update.relinarizationNeeded(update_count_)) {
// 4. Mark keys in \Delta above threshold \beta: // 4. Mark keys in \Delta above threshold \beta:
// J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. relinKeys = update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_,
if (params_.enablePartialRelinearizationCheck) &result.markedKeys);
relinKeys = Impl::CheckRelinearizationPartial( update.recordRelinearizeDetail(relinKeys, result.details());
roots_, delta_, params_.relinearizeThreshold);
else
relinKeys =
Impl::CheckRelinearizationFull(delta_, params_.relinearizeThreshold);
if (kDisableReordering)
relinKeys = Impl::CheckRelinearizationFull(
delta_, 0.0); // This is used for debugging
// Remove from relinKeys any keys whose linearization points are fixed
for (Key key : fixedVariables_) {
relinKeys.erase(key);
}
if (updateParams.noRelinKeys) {
for (Key key : *updateParams.noRelinKeys) {
relinKeys.erase(key);
}
}
// Above relin threshold keys for detailed results
if (params_.enableDetailedResults) {
for (Key key : relinKeys) {
result.detail->variableStatus[key].isAboveRelinThreshold = true;
result.detail->variableStatus[key].isRelinearized = true;
}
}
// Add the variables being relinearized to the marked keys
KeySet markedRelinMask;
for (const Key key : relinKeys) markedRelinMask.insert(key);
markedKeys.insert(relinKeys.begin(), relinKeys.end());
gttoc(gather_relinearize_keys);
gttic(fluid_find_all);
// 5. Mark all cliques that involve marked variables \Theta_{J} and all
// their ancestors.
if (!relinKeys.empty()) { if (!relinKeys.empty()) {
for (const sharedClique& root : roots_) // 5. Mark cliques that involve marked variables \Theta_{J} and ancestors.
// add other cliques that have the marked ones in the separator update.findFluid(roots_, relinKeys, &result.markedKeys, result.details());
root->findAll(markedRelinMask, &markedKeys);
// Relin involved keys for detailed results
if (params_.enableDetailedResults) {
KeySet involvedRelinKeys;
for (const sharedClique& root : roots_)
root->findAll(markedRelinMask, &involvedRelinKeys);
for (Key key : involvedRelinKeys) {
if (!result.detail->variableStatus[key].isAboveRelinThreshold) {
result.detail->variableStatus[key].isRelinearizeInvolved = true;
result.detail->variableStatus[key].isRelinearized = true;
}
}
}
}
gttoc(fluid_find_all);
gttic(expmap);
// 6. Update linearization point for marked variables: // 6. Update linearization point for marked variables:
// \Theta_{J}:=\Theta_{J}+\Delta_{J}. // \Theta_{J}:=\Theta_{J}+\Delta_{J}.
if (!relinKeys.empty()) expmapMasked(markedRelinMask); UpdateImpl::ExpmapMasked(delta_, relinKeys, &theta_);
gttoc(expmap); }
result.variablesRelinearized = result.markedKeys.size();
result.variablesRelinearized = markedKeys.size();
} else {
result.variablesRelinearized = 0;
} }
gttic(linearize_new);
// 7. Linearize new factors // 7. Linearize new factors
if (params_.cacheLinearizedFactors) { update.linearizeNewFactors(newFactors, theta_, nonlinearFactors_.size(),
gttic(linearize); result.newFactorsIndices, &linearFactors_);
auto linearFactors = newFactors.linearize(theta_); update.augmentVariableIndex(newFactors, result.newFactorsIndices,
if (params_.findUnusedFactorSlots) { &variableIndex_);
linearFactors_.resize(nonlinearFactors_.size());
for (size_t newFactorI = 0; newFactorI < newFactors.size(); ++newFactorI)
linearFactors_[result.newFactorsIndices[newFactorI]] =
(*linearFactors)[newFactorI];
} else {
linearFactors_.push_back(*linearFactors);
}
assert(nonlinearFactors_.size() == linearFactors_.size());
gttoc(linearize);
}
gttoc(linearize_new);
gttic(augment_VI); // 8. Redo top of Bayes tree and update data structures
// Augment the variable index with the new factors recalculate(updateParams, relinKeys, &result);
if (params_.findUnusedFactorSlots) if (!result.unusedKeys.empty()) removeVariables(result.unusedKeys);
variableIndex_.augment(newFactors, result.newFactorsIndices);
else
variableIndex_.augment(newFactors);
// Augment it with existing factors which now affect to more variables:
if (updateParams.newAffectedKeys) {
for (const auto &factorAddedKeys : *updateParams.newAffectedKeys) {
const auto factorIdx = factorAddedKeys.first;
variableIndex_.augmentExistingFactor(
factorIdx, factorAddedKeys.second);
}
}
gttoc(augment_VI);
gttic(recalculate);
// 8. Redo top of Bayes tree
boost::shared_ptr<KeySet> replacedKeys;
if (!markedKeys.empty() || !observedKeys.empty())
replacedKeys = recalculate(
markedKeys, relinKeys, observedKeys, unusedIndices,
updateParams.constrainedKeys, &result);
// Update replaced keys mask (accumulates until back-substitution takes place)
if (replacedKeys)
deltaReplacedMask_.insert(replacedKeys->begin(), replacedKeys->end());
gttoc(recalculate);
// Update data structures to remove unused keys
if (!unusedKeys.empty()) {
gttic(remove_variables);
removeVariables(unusedKeys);
gttoc(remove_variables);
}
result.cliques = this->nodes().size(); result.cliques = this->nodes().size();
gttic(evaluate_error_after);
if (params_.evaluateNonlinearError) if (params_.evaluateNonlinearError)
result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate())); update.error(nonlinearFactors_, calculateEstimate(), &result.errorAfter);
gttoc(evaluate_error_after);
return result; return result;
} }
@ -1057,6 +697,7 @@ void ISAM2::marginalizeLeaves(
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Marked const but actually changes mutable delta
void ISAM2::updateDelta(bool forceFullSolve) const { void ISAM2::updateDelta(bool forceFullSolve) const {
gttic(updateDelta); gttic(updateDelta);
if (params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { if (params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) {
@ -1066,8 +707,8 @@ void ISAM2::updateDelta(bool forceFullSolve) const {
const double effectiveWildfireThreshold = const double effectiveWildfireThreshold =
forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold;
gttic(Wildfire_update); gttic(Wildfire_update);
lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta( DeltaImpl::UpdateGaussNewtonDelta(roots_, deltaReplacedMask_,
roots_, deltaReplacedMask_, effectiveWildfireThreshold, &delta_); effectiveWildfireThreshold, &delta_);
deltaReplacedMask_.clear(); deltaReplacedMask_.clear();
gttoc(Wildfire_update); gttoc(Wildfire_update);
@ -1083,15 +724,15 @@ void ISAM2::updateDelta(bool forceFullSolve) const {
// Compute Newton's method step // Compute Newton's method step
gttic(Wildfire_update); gttic(Wildfire_update);
lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta( DeltaImpl::UpdateGaussNewtonDelta(
roots_, deltaReplacedMask_, effectiveWildfireThreshold, &deltaNewton_); roots_, deltaReplacedMask_, effectiveWildfireThreshold, &deltaNewton_);
gttoc(Wildfire_update); gttoc(Wildfire_update);
// Compute steepest descent step // Compute steepest descent step
const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient
Impl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, DeltaImpl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero,
&RgProd_); // Update RgProd &RgProd_); // Update RgProd
const VectorValues dx_u = Impl::ComputeGradientSearch( const VectorValues dx_u = DeltaImpl::ComputeGradientSearch(
gradAtZero, RgProd_); // Compute gradient search point gradAtZero, RgProd_); // Compute gradient search point
// Clear replaced keys mask because now we've updated deltaNewton_ and // Clear replaced keys mask because now we've updated deltaNewton_ and
@ -1113,6 +754,8 @@ void ISAM2::updateDelta(bool forceFullSolve) const {
doglegResult doglegResult
.dx_d; // Copy the VectorValues containing with the linear solution .dx_d; // Copy the VectorValues containing with the linear solution
gttoc(Copy_dx_d); gttoc(Copy_dx_d);
} else {
throw std::runtime_error("iSAM2: unknown ISAM2Params type");
} }
} }
@ -1155,37 +798,13 @@ double ISAM2::error(const VectorValues& x) const {
return GaussianFactorGraph(*this).error(x); return GaussianFactorGraph(*this).error(x);
} }
/* ************************************************************************* */
static void gradientAtZeroTreeAdder(const boost::shared_ptr<ISAM2Clique>& root,
VectorValues* g) {
// Loop through variables in each clique, adding contributions
DenseIndex variablePosition = 0;
for (GaussianConditional::const_iterator jit = root->conditional()->begin();
jit != root->conditional()->end(); ++jit) {
const DenseIndex dim = root->conditional()->getDim(jit);
pair<VectorValues::iterator, bool> pos_ins = g->tryInsert(
*jit, root->gradientContribution().segment(variablePosition, dim));
if (!pos_ins.second)
pos_ins.first->second +=
root->gradientContribution().segment(variablePosition, dim);
variablePosition += dim;
}
// Recursively add contributions from children
typedef boost::shared_ptr<ISAM2Clique> sharedClique;
for (const sharedClique& child : root->children) {
gradientAtZeroTreeAdder(child, g);
}
}
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues ISAM2::gradientAtZero() const { VectorValues ISAM2::gradientAtZero() const {
// Create result // Create result
VectorValues g; VectorValues g;
// Sum up contributions for each clique // Sum up contributions for each clique
for (const ISAM2::sharedClique& root : this->roots()) for (const auto& root : this->roots()) root->addGradientAtZero(&g);
gradientAtZeroTreeAdder(root, &g);
return g; return g;
} }

82
gtsam/nonlinear/ISAM2.h
View File

@ -20,12 +20,12 @@
#pragma once #pragma once
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/nonlinear/ISAM2Clique.h>
#include <gtsam/nonlinear/ISAM2Params.h> #include <gtsam/nonlinear/ISAM2Params.h>
#include <gtsam/nonlinear/ISAM2Result.h> #include <gtsam/nonlinear/ISAM2Result.h>
#include <gtsam/nonlinear/ISAM2Clique.h>
#include <gtsam/nonlinear/ISAM2UpdateParams.h> #include <gtsam/nonlinear/ISAM2UpdateParams.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <vector> #include <vector>
@ -73,8 +73,8 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
* This is \c mutable because it is used internally to not update delta_ * This is \c mutable because it is used internally to not update delta_
* until it is needed. * until it is needed.
*/ */
mutable KeySet mutable KeySet deltaReplacedMask_; // TODO(dellaert): Make sure accessed in
deltaReplacedMask_; // TODO(dellaert): Make sure accessed in the right way // the right way
/** All original nonlinear factors are stored here to use during /** All original nonlinear factors are stored here to use during
* relinearization */ * relinearization */
@ -97,11 +97,11 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
///< periodic relinearization ///< periodic relinearization
public: public:
typedef ISAM2 This; ///< This class using This = ISAM2; ///< This class
typedef BayesTree<ISAM2Clique> Base; ///< The BayesTree base class using Base = BayesTree<ISAM2Clique>; ///< The BayesTree base class
typedef Base::Clique Clique; ///< A clique using Clique = Base::Clique; ///< A clique
typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique using sharedClique = Base::sharedClique; ///< Shared pointer to a clique
typedef Base::Cliques Cliques; ///< List of Clique typedef from base class using Cliques = Base::Cliques; ///< List of Cliques
/** Create an empty ISAM2 instance */ /** Create an empty ISAM2 instance */
explicit ISAM2(const ISAM2Params& params); explicit ISAM2(const ISAM2Params& params);
@ -175,8 +175,8 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
* @return An ISAM2Result struct containing information about the update * @return An ISAM2Result struct containing information about the update
* @note No default parameters to avoid ambiguous call errors. * @note No default parameters to avoid ambiguous call errors.
*/ */
virtual ISAM2Result update( virtual ISAM2Result update(const NonlinearFactorGraph& newFactors,
const NonlinearFactorGraph& newFactors, const Values& newTheta, const Values& newTheta,
const ISAM2UpdateParams& updateParams); const ISAM2UpdateParams& updateParams);
/** Marginalize out variables listed in leafKeys. These keys must be leaves /** Marginalize out variables listed in leafKeys. These keys must be leaves
@ -226,7 +226,6 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta); return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta);
} }
/** Compute an estimate for a single variable using its incomplete linear /** Compute an estimate for a single variable using its incomplete linear
* delta computed during the last update. This is faster than calling the * delta computed during the last update. This is faster than calling the
* no-argument version of calculateEstimate, which operates on all variables. * no-argument version of calculateEstimate, which operates on all variables.
@ -243,9 +242,6 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
/// @name Public members for non-typical usage /// @name Public members for non-typical usage
/// @{ /// @{
/** Internal implementation functions */
struct Impl;
/** Compute an estimate using a complete delta computed by a full /** Compute an estimate using a complete delta computed by a full
* back-substitution. * back-substitution.
*/ */
@ -268,13 +264,6 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
/** Access the nonlinear variable index */ /** Access the nonlinear variable index */
const KeySet& getFixedVariables() const { return fixedVariables_; } const KeySet& getFixedVariables() const { return fixedVariables_; }
size_t lastAffectedVariableCount;
size_t lastAffectedFactorCount;
size_t lastAffectedCliqueCount;
size_t lastAffectedMarkedCount;
mutable size_t lastBacksubVariableCount;
size_t lastNnzTop;
const ISAM2Params& params() const { return params_; } const ISAM2Params& params() const { return params_; }
/** prints out clique statistics */ /** prints out clique statistics */
@ -292,40 +281,39 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
/// @} /// @}
protected: protected:
/// Remove marked top and either recalculate in batch or incrementally.
void recalculate(const ISAM2UpdateParams& updateParams,
const KeySet& relinKeys, ISAM2Result* result);
// Do a batch step - reorder and relinearize all variables
void recalculateBatch(const ISAM2UpdateParams& updateParams,
KeySet* affectedKeysSet, ISAM2Result* result);
// retrieve all factors that ONLY contain the affected variables
// (note that the remaining stuff is summarized in the cached factors)
GaussianFactorGraph relinearizeAffectedFactors(
const ISAM2UpdateParams& updateParams, const FastList<Key>& affectedKeys,
const KeySet& relinKeys);
void recalculateIncremental(const ISAM2UpdateParams& updateParams,
const KeySet& relinKeys,
const FastList<Key>& affectedKeys,
KeySet* affectedKeysSet, Cliques* orphans,
ISAM2Result* result);
/** /**
* Add new variables to the ISAM2 system. * Add new variables to the ISAM2 system.
* @param newTheta Initial values for new variables * @param newTheta Initial values for new variables
* @param theta Current solution to be augmented with new initialization * @param variableStatus optional detailed result structure
* @param delta Current linear delta to be augmented with zeros
* @param deltaNewton
* @param RgProd
* @param keyFormatter Formatter for printing nonlinear keys during debugging
*/ */
void addVariables(const Values& newTheta); void addVariables(const Values& newTheta,
ISAM2Result::DetailedResults* detail = 0);
/** /**
* Remove variables from the ISAM2 system. * Remove variables from the ISAM2 system.
*/ */
void removeVariables(const KeySet& unusedKeys); void removeVariables(const KeySet& unusedKeys);
/**
* Apply expmap to the given values, but only for indices appearing in
* \c mask. Values are expmapped in-place.
* \param mask Mask on linear indices, only \c true entries are expmapped
*/
void expmapMasked(const KeySet& mask);
FactorIndexSet getAffectedFactors(const FastList<Key>& keys) const;
GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(
const FastList<Key>& affectedKeys, const KeySet& relinKeys) const;
GaussianFactorGraph getCachedBoundaryFactors(const Cliques& orphans);
virtual boost::shared_ptr<KeySet> recalculate(
const KeySet& markedKeys, const KeySet& relinKeys,
const KeyVector& observedKeys, const KeySet& unusedIndices,
const boost::optional<FastMap<Key, int> >& constrainKeys,
ISAM2Result* result);
void updateDelta(bool forceFullSolve = false) const; void updateDelta(bool forceFullSolve = false) const;
}; // ISAM2 }; // ISAM2
@ -334,5 +322,3 @@ template <>
struct traits<ISAM2> : public Testable<ISAM2> {}; struct traits<ISAM2> : public Testable<ISAM2> {};
} // namespace gtsam } // namespace gtsam
#include <gtsam/nonlinear/ISAM2-impl.h>

28
gtsam/nonlinear/ISAM2Clique.cpp
View File

@ -19,7 +19,9 @@
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/linearAlgorithms-inst.h> #include <gtsam/linear/linearAlgorithms-inst.h>
#include <gtsam/nonlinear/ISAM2Clique.h> #include <gtsam/nonlinear/ISAM2Clique.h>
#include <stack> #include <stack>
#include <utility>
using namespace std; using namespace std;
@ -304,7 +306,7 @@ void ISAM2Clique::findAll(const KeySet& markedMask, KeySet* keys) const {
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 : conditional()->parents()) { for (Key key : conditional_->parents()) {
if (markedMask.exists(key)) { if (markedMask.exists(key)) {
found = true; found = true;
break; break;
@ -312,14 +314,34 @@ void ISAM2Clique::findAll(const KeySet& markedMask, KeySet* keys) const {
} }
if (found) { if (found) {
// then add this clique // then add this clique
keys->insert(conditional()->beginFrontals(), conditional()->endFrontals()); keys->insert(conditional_->beginFrontals(), conditional_->endFrontals());
if (debug) print("Key(s) marked in clique "); if (debug) print("Key(s) marked in clique ");
if (debug) cout << "so marking key " << conditional()->front() << endl; if (debug) cout << "so marking key " << conditional_->front() << endl;
} }
for (const auto& child : children) { for (const auto& child : children) {
child->findAll(markedMask, keys); child->findAll(markedMask, keys);
} }
} }
/* ************************************************************************* */
void ISAM2Clique::addGradientAtZero(VectorValues* g) const {
// Loop through variables in each clique, adding contributions
DenseIndex position = 0;
for (auto it = conditional_->begin(); it != conditional_->end(); ++it) {
const DenseIndex dim = conditional_->getDim(it);
const Vector contribution = gradientContribution_.segment(position, dim);
VectorValues::iterator values_it;
bool success;
std::tie(values_it, success) = g->tryInsert(*it, contribution);
if (!success) values_it->second += contribution;
position += dim;
}
// Recursively add contributions from children
for (const auto& child : children) {
child->addGradientAtZero(g);
}
}
/* ************************************************************************* */ /* ************************************************************************* */
} // namespace gtsam } // namespace gtsam

5
gtsam/nonlinear/ISAM2Clique.h
View File

@ -75,9 +75,12 @@ class GTSAM_EXPORT ISAM2Clique
/** Access the cached factor */ /** Access the cached factor */
Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
/** Access the gradient contribution */ /// Access the gradient contribution
const Vector& gradientContribution() const { return gradientContribution_; } const Vector& gradientContribution() const { return gradientContribution_; }
/// Recursively add gradient at zero to g
void addGradientAtZero(VectorValues* g) const;
bool equals(const This& other, double tol = 1e-9) const; bool equals(const This& other, double tol = 1e-9) const;
/** print this node */ /** print this node */

6
gtsam/nonlinear/ISAM2Params.h
View File

@ -234,8 +234,8 @@ struct GTSAM_EXPORT ISAM2Params {
Factorization _factorization = ISAM2Params::CHOLESKY, Factorization _factorization = ISAM2Params::CHOLESKY,
bool _cacheLinearizedFactors = true, bool _cacheLinearizedFactors = true,
const KeyFormatter& _keyFormatter = const KeyFormatter& _keyFormatter =
DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter DefaultKeyFormatter, ///< see ISAM2::Params::keyFormatter,
) bool _enableDetailedResults = false)
: optimizationParams(_optimizationParams), : optimizationParams(_optimizationParams),
relinearizeThreshold(_relinearizeThreshold), relinearizeThreshold(_relinearizeThreshold),
relinearizeSkip(_relinearizeSkip), relinearizeSkip(_relinearizeSkip),
@ -244,7 +244,7 @@ struct GTSAM_EXPORT ISAM2Params {
factorization(_factorization), factorization(_factorization),
cacheLinearizedFactors(_cacheLinearizedFactors), cacheLinearizedFactors(_cacheLinearizedFactors),
keyFormatter(_keyFormatter), keyFormatter(_keyFormatter),
enableDetailedResults(false), enableDetailedResults(_enableDetailedResults),
enablePartialRelinearizationCheck(false), enablePartialRelinearizationCheck(false),
findUnusedFactorSlots(false) {} findUnusedFactorSlots(false) {}

34
gtsam/nonlinear/ISAM2Result.h
View File

@ -24,8 +24,8 @@
#include <gtsam/linear/GaussianBayesTree.h> #include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h> #include <gtsam/nonlinear/DoglegOptimizerImpl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ISAM2Params.h> #include <gtsam/nonlinear/ISAM2Params.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <boost/variant.hpp> #include <boost/variant.hpp>
@ -96,7 +96,22 @@ struct GTSAM_EXPORT ISAM2Result {
*/ */
FactorIndices newFactorsIndices; FactorIndices newFactorsIndices;
/** A struct holding detailed results, which must be enabled with /** Unused keys, and indices for unused keys,
* i.e., keys that are empty now and do not appear in the new factors.
*/
KeySet unusedKeys;
/** keys for variables that were observed, i.e., not unused. */
KeyVector observedKeys;
/** Keys of variables that had factors removed. */
KeySet keysWithRemovedFactors;
/** All keys that were marked during the update process. */
KeySet markedKeys;
/**
* A struct holding detailed results, which must be enabled with
* ISAM2Params::enableDetailedResults. * ISAM2Params::enableDetailedResults.
*/ */
struct DetailedResults { struct DetailedResults {
@ -132,15 +147,24 @@ struct GTSAM_EXPORT ISAM2Result {
inRootClique(false) {} inRootClique(false) {}
}; };
/** The status of each variable during this update, see VariableStatus. using StatusMap = FastMap<Key, VariableStatus>;
*/
FastMap<Key, VariableStatus> variableStatus; /// The status of each variable during this update, see VariableStatus.
StatusMap variableStatus;
}; };
/** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See /** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See
* Detail for information about the results data stored here. */ * Detail for information about the results data stored here. */
boost::optional<DetailedResults> detail; boost::optional<DetailedResults> detail;
explicit ISAM2Result(bool enableDetailedResults = false) {
if (enableDetailedResults) detail.reset(DetailedResults());
}
/// Return pointer to detail, 0 if no detail requested
DetailedResults* details() { return detail.get_ptr(); }
/// Print results
void print(const std::string str = "") const { void print(const std::string str = "") const {
using std::cout; using std::cout;
cout << str << " Reelimintated: " << variablesReeliminated cout << str << " Reelimintated: " << variablesReeliminated

6
gtsam/nonlinear/ISAM2UpdateParams.h
View File

@ -16,11 +16,11 @@
#pragma once #pragma once
#include <boost/optional.hpp>
#include <gtsam/base/FastList.h> #include <gtsam/base/FastList.h>
#include <gtsam/dllexport.h> // GTSAM_EXPORT #include <gtsam/dllexport.h> // GTSAM_EXPORT
#include <gtsam/inference/Key.h> // Key, KeySet #include <gtsam/inference/Key.h> // Key, KeySet
#include <gtsam/nonlinear/ISAM2Result.h> //FactorIndices #include <gtsam/nonlinear/ISAM2Result.h> //FactorIndices
#include <boost/optional.hpp>
namespace gtsam { namespace gtsam {
@ -65,6 +65,10 @@ struct GTSAM_EXPORT ISAM2UpdateParams {
*/ */
boost::optional<FastMap<FactorIndex, KeySet>> newAffectedKeys{boost::none}; boost::optional<FastMap<FactorIndex, KeySet>> newAffectedKeys{boost::none};
/** By default, iSAM2 uses a wildfire update scheme that stops updating when
* the deltas become too small down in the tree. This flagg forces a full
* solve instead. */
bool forceFullSolve{false};
}; };
} // namespace gtsam } // namespace gtsam

8
gtsam/slam/PriorFactor.h
View File

@ -18,6 +18,8 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <string>
namespace gtsam { namespace gtsam {
/** /**
@ -70,10 +72,14 @@ namespace gtsam {
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
traits<T>::Print(prior_, " prior mean: "); traits<T>::Print(prior_, " prior mean: ");
if (this->noiseModel_)
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
else
std::cout << "no noise model" << std::endl;
} }
/** equals */ /** equals */

2
gtsam_unstable/discrete/CSP.cpp
View File

@ -43,7 +43,7 @@ namespace gtsam {
// keep track of which domains changed // keep track of which domains changed
changed[v] = false; changed[v] = false;
// loop over all factors/constraints for variable v // loop over all factors/constraints for variable v
const VariableIndex::Factors& factors = index[v]; const FactorIndices& factors = index[v];
for(size_t f: factors) { for(size_t f: factors) {
// if not already a singleton // if not already a singleton
if (!domains[v].isSingleton()) { if (!domains[v].isSingleton()) {

128
tests/testGaussianISAM2.cpp
View File

@ -4,7 +4,7 @@
* @author Michael Kaess * @author Michael Kaess
*/ */
#include <CppUnitLite/TestHarness.h> #include <gtsam/nonlinear/ISAM2.h>
#include <tests/smallExample.h> #include <tests/smallExample.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
@ -14,7 +14,6 @@
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/Marginals.h> #include <gtsam/nonlinear/Marginals.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianBayesTree.h> #include <gtsam/linear/GaussianBayesTree.h>
@ -23,10 +22,12 @@
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/treeTraversal-inst.h> #include <gtsam/base/treeTraversal-inst.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/list_of.hpp> #include <boost/assign/list_of.hpp>
#include <gtsam/base/deprecated/LieScalar.h>
using namespace boost::assign;
#include <boost/range/adaptor/map.hpp> #include <boost/range/adaptor/map.hpp>
using namespace boost::assign;
namespace br { using namespace boost::adaptors; using namespace boost::range; } namespace br { using namespace boost::adaptors; using namespace boost::range; }
using namespace std; using namespace std;
@ -34,7 +35,6 @@ using namespace gtsam;
using boost::shared_ptr; using boost::shared_ptr;
static const SharedNoiseModel model; static const SharedNoiseModel model;
static const LieScalar Zero(0);
// SETDEBUG("ISAM2 update", true); // SETDEBUG("ISAM2 update", true);
// SETDEBUG("ISAM2 update verbose", true); // SETDEBUG("ISAM2 update verbose", true);
@ -47,9 +47,11 @@ SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0,
ISAM2 createSlamlikeISAM2( ISAM2 createSlamlikeISAM2(
boost::optional<Values&> init_values = boost::none, boost::optional<Values&> init_values = boost::none,
boost::optional<NonlinearFactorGraph&> full_graph = boost::none, boost::optional<NonlinearFactorGraph&> full_graph = boost::none,
const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true), const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0,
0, false, true,
ISAM2Params::CHOLESKY, true,
DefaultKeyFormatter, true),
size_t maxPoses = 10) { size_t maxPoses = 10) {
// These variables will be reused and accumulate factors and values // These variables will be reused and accumulate factors and values
ISAM2 isam(params); ISAM2 isam(params);
Values fullinit; Values fullinit;
@ -282,34 +284,6 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c
return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual && isamTreeEqual && consistent; return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual && isamTreeEqual && consistent;
} }
/* ************************************************************************* */
TEST(ISAM2, AddFactorsStep1)
{
NonlinearFactorGraph nonlinearFactors;
nonlinearFactors += PriorFactor<LieScalar>(10, Zero, model);
nonlinearFactors += NonlinearFactor::shared_ptr();
nonlinearFactors += PriorFactor<LieScalar>(11, Zero, model);
NonlinearFactorGraph newFactors;
newFactors += PriorFactor<LieScalar>(1, Zero, model);
newFactors += PriorFactor<LieScalar>(2, Zero, model);
NonlinearFactorGraph expectedNonlinearFactors;
expectedNonlinearFactors += PriorFactor<LieScalar>(10, Zero, model);
expectedNonlinearFactors += PriorFactor<LieScalar>(1, Zero, model);
expectedNonlinearFactors += PriorFactor<LieScalar>(11, Zero, model);
expectedNonlinearFactors += PriorFactor<LieScalar>(2, Zero, model);
const FactorIndices expectedNewFactorIndices = list_of(1)(3);
FactorIndices actualNewFactorIndices;
ISAM2::Impl::AddFactorsStep1(newFactors, true, &nonlinearFactors, &actualNewFactorIndices);
EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors));
EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ISAM2, simple) TEST(ISAM2, simple)
{ {
@ -692,20 +666,19 @@ namespace {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ISAM2, marginalizeLeaves1) TEST(ISAM2, marginalizeLeaves1) {
{
ISAM2 isam; ISAM2 isam;
NonlinearFactorGraph factors; NonlinearFactorGraph factors;
factors += PriorFactor<LieScalar>(0, Zero, model); factors += PriorFactor<double>(0, 0.0, model);
factors += BetweenFactor<LieScalar>(0, 1, Zero, model); factors += BetweenFactor<double>(0, 1, 0.0, model);
factors += BetweenFactor<LieScalar>(1, 2, Zero, model); factors += BetweenFactor<double>(1, 2, 0.0, model);
factors += BetweenFactor<LieScalar>(0, 2, Zero, model); factors += BetweenFactor<double>(0, 2, 0.0, model);
Values values; Values values;
values.insert(0, Zero); values.insert(0, 0.0);
values.insert(1, Zero); values.insert(1, 0.0);
values.insert(2, Zero); values.insert(2, 0.0);
FastMap<Key, int> constrainedKeys; FastMap<Key, int> constrainedKeys;
constrainedKeys.insert(make_pair(0, 0)); constrainedKeys.insert(make_pair(0, 0));
@ -719,23 +692,22 @@ TEST(ISAM2, marginalizeLeaves1)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ISAM2, marginalizeLeaves2) TEST(ISAM2, marginalizeLeaves2) {
{
ISAM2 isam; ISAM2 isam;
NonlinearFactorGraph factors; NonlinearFactorGraph factors;
factors += PriorFactor<LieScalar>(0, Zero, model); factors += PriorFactor<double>(0, 0.0, model);
factors += BetweenFactor<LieScalar>(0, 1, Zero, model); factors += BetweenFactor<double>(0, 1, 0.0, model);
factors += BetweenFactor<LieScalar>(1, 2, Zero, model); factors += BetweenFactor<double>(1, 2, 0.0, model);
factors += BetweenFactor<LieScalar>(0, 2, Zero, model); factors += BetweenFactor<double>(0, 2, 0.0, model);
factors += BetweenFactor<LieScalar>(2, 3, Zero, model); factors += BetweenFactor<double>(2, 3, 0.0, model);
Values values; Values values;
values.insert(0, Zero); values.insert(0, 0.0);
values.insert(1, Zero); values.insert(1, 0.0);
values.insert(2, Zero); values.insert(2, 0.0);
values.insert(3, Zero); values.insert(3, 0.0);
FastMap<Key, int> constrainedKeys; FastMap<Key, int> constrainedKeys;
constrainedKeys.insert(make_pair(0, 0)); constrainedKeys.insert(make_pair(0, 0));
@ -750,30 +722,29 @@ TEST(ISAM2, marginalizeLeaves2)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ISAM2, marginalizeLeaves3) TEST(ISAM2, marginalizeLeaves3) {
{
ISAM2 isam; ISAM2 isam;
NonlinearFactorGraph factors; NonlinearFactorGraph factors;
factors += PriorFactor<LieScalar>(0, Zero, model); factors += PriorFactor<double>(0, 0.0, model);
factors += BetweenFactor<LieScalar>(0, 1, Zero, model); factors += BetweenFactor<double>(0, 1, 0.0, model);
factors += BetweenFactor<LieScalar>(1, 2, Zero, model); factors += BetweenFactor<double>(1, 2, 0.0, model);
factors += BetweenFactor<LieScalar>(0, 2, Zero, model); factors += BetweenFactor<double>(0, 2, 0.0, model);
factors += BetweenFactor<LieScalar>(2, 3, Zero, model); factors += BetweenFactor<double>(2, 3, 0.0, model);
factors += BetweenFactor<LieScalar>(3, 4, Zero, model); factors += BetweenFactor<double>(3, 4, 0.0, model);
factors += BetweenFactor<LieScalar>(4, 5, Zero, model); factors += BetweenFactor<double>(4, 5, 0.0, model);
factors += BetweenFactor<LieScalar>(3, 5, Zero, model); factors += BetweenFactor<double>(3, 5, 0.0, model);
Values values; Values values;
values.insert(0, Zero); values.insert(0, 0.0);
values.insert(1, Zero); values.insert(1, 0.0);
values.insert(2, Zero); values.insert(2, 0.0);
values.insert(3, Zero); values.insert(3, 0.0);
values.insert(4, Zero); values.insert(4, 0.0);
values.insert(5, Zero); values.insert(5, 0.0);
FastMap<Key, int> constrainedKeys; FastMap<Key, int> constrainedKeys;
constrainedKeys.insert(make_pair(0, 0)); constrainedKeys.insert(make_pair(0, 0));
@ -790,19 +761,18 @@ TEST(ISAM2, marginalizeLeaves3)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ISAM2, marginalizeLeaves4) TEST(ISAM2, marginalizeLeaves4) {
{
ISAM2 isam; ISAM2 isam;
NonlinearFactorGraph factors; NonlinearFactorGraph factors;
factors += PriorFactor<LieScalar>(0, Zero, model); factors += PriorFactor<double>(0, 0.0, model);
factors += BetweenFactor<LieScalar>(0, 2, Zero, model); factors += BetweenFactor<double>(0, 2, 0.0, model);
factors += BetweenFactor<LieScalar>(1, 2, Zero, model); factors += BetweenFactor<double>(1, 2, 0.0, model);
Values values; Values values;
values.insert(0, Zero); values.insert(0, 0.0);
values.insert(1, Zero); values.insert(1, 0.0);
values.insert(2, Zero); values.insert(2, 0.0);
FastMap<Key, int> constrainedKeys; FastMap<Key, int> constrainedKeys;
constrainedKeys.insert(make_pair(0, 0)); constrainedKeys.insert(make_pair(0, 0));