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

Changed several Impl functions to methods in ISAM2 and ISAM2Clique

release/4.3a0
Frank Dellaert 2018-09-30 14:11:00 -04:00
parent 63b7d64fea
commit d6edc217ab
7 changed files with 154 additions and 193 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -31,27 +31,12 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
void ISAM2::Impl::AddVariables(const Values& newTheta, Values& theta,
VectorValues& delta, VectorValues& deltaNewton,
VectorValues& RgProd,
const KeyFormatter& keyFormatter) {
const bool debug = ISDEBUG("ISAM2 AddVariables");
theta.insert(newTheta);
if (debug) newTheta.print("The new variables are: ");
// Add zeros into the VectorValues
delta.insert(newTheta.zeroVectors());
deltaNewton.insert(newTheta.zeroVectors());
RgProd.insert(newTheta.zeroVectors());
}
/* ************************************************************************* */
void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors,
bool useUnusedSlots,
NonlinearFactorGraph& nonlinearFactors,
FactorIndices& newFactorIndices) {
newFactorIndices.resize(newFactors.size());
NonlinearFactorGraph* nonlinearFactors,
FactorIndices* newFactorIndices) {
newFactorIndices->resize(newFactors.size());
if (useUnusedSlots) {
size_t globalFactorIndex = 0;
@ -65,44 +50,24 @@ void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors,
// 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])
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;
(*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);
}
}
/* ************************************************************************* */
void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys,
const FastVector<ISAM2::sharedClique>& roots,
Values& theta, VariableIndex& variableIndex,
VectorValues& delta,
VectorValues& deltaNewton,
VectorValues& RgProd, KeySet& replacedKeys,
Base::Nodes& nodes, KeySet& fixedVariables) {
variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end());
for (Key key : unusedKeys) {
delta.erase(key);
deltaNewton.erase(key);
RgProd.erase(key);
replacedKeys.erase(key);
nodes.unsafe_erase(key);
theta.erase(key);
fixedVariables.erase(key);
(*newFactorIndices)[i] = i + nonlinearFactors->size();
nonlinearFactors->push_back(newFactors);
}
}
@ -195,7 +160,7 @@ static void CheckRelinearizationRecursiveMap(
/* ************************************************************************* */
KeySet ISAM2::Impl::CheckRelinearizationPartial(
const FastVector<ISAM2::sharedClique>& roots, const VectorValues& delta,
const ISAM2::Roots& roots, const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
KeySet relinKeys;
for (const ISAM2::sharedClique& root : roots) {
@ -210,71 +175,6 @@ KeySet ISAM2::Impl::CheckRelinearizationPartial(
return relinKeys;
}
/* ************************************************************************* */
void ISAM2::Impl::FindAll(ISAM2::sharedClique clique, KeySet& keys,
const KeySet& markedMask) {
static const bool debug = false;
// does the separator contain any of the variables?
bool found = false;
for (Key key : clique->conditional()->parents()) {
if (markedMask.exists(key)) {
found = true;
break;
}
}
if (found) {
// then add this clique
keys.insert(clique->conditional()->beginFrontals(),
clique->conditional()->endFrontals());
if (debug) clique->print("Key(s) marked in clique ");
if (debug)
cout << "so marking key " << clique->conditional()->front() << endl;
}
for (const ISAM2::sharedClique& child : clique->children) {
FindAll(child, keys, markedMask);
}
}
/* ************************************************************************* */
void ISAM2::Impl::ExpmapMasked(const VectorValues& delta, const KeySet& mask,
Values* values,
boost::optional<VectorValues&> invalidateIfDebug,
const KeyFormatter& keyFormatter) {
// If debugging, invalidate if requested, otherwise do not invalidate.
// Invalidating means setting expmapped entries to Inf, to trigger assertions
// if we try to re-use them.
#ifdef NDEBUG
invalidateIfDebug = boost::none;
#endif
assert(values->size() == delta.size());
Values::iterator key_value;
VectorValues::const_iterator key_delta;
#ifdef GTSAM_USE_TBB
for (key_value = values->begin(); key_value != values->end(); ++key_value) {
key_delta = delta.find(key_value->key);
#else
for (key_value = values->begin(), key_delta = delta.begin();
key_value != values->end(); ++key_value, ++key_delta) {
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_();
if (invalidateIfDebug)
(*invalidateIfDebug)[var].operator=(Vector::Constant(
delta[var].rows(),
numeric_limits<double>::infinity())); // Strange syntax to work
// with clang++ (bug in
// clang?)
}
}
}
/* ************************************************************************* */
namespace internal {
inline static void optimizeInPlace(const ISAM2::sharedClique& clique,
@ -289,9 +189,10 @@ inline static void optimizeInPlace(const ISAM2::sharedClique& clique,
} // namespace internal
/* ************************************************************************* */
size_t ISAM2::Impl::UpdateGaussNewtonDelta(
const FastVector<ISAM2::sharedClique>& roots, const KeySet& replacedKeys,
double wildfireThreshold, VectorValues* delta) {
size_t ISAM2::Impl::UpdateGaussNewtonDelta(const ISAM2::Roots& roots,
const KeySet& replacedKeys,
double wildfireThreshold,
VectorValues* delta) {
size_t lastBacksubVariableCount;
if (wildfireThreshold <= 0.0) {

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

@ -23,7 +23,6 @@
namespace gtsam {
struct GTSAM_EXPORT ISAM2::Impl {
struct GTSAM_EXPORT PartialSolveResult {
ISAM2::sharedClique bayesTree;
};
@ -32,35 +31,14 @@ struct GTSAM_EXPORT ISAM2::Impl {
size_t nFullSystemVars;
enum { /*AS_ADDED,*/ COLAMD } algorithm;
enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain;
boost::optional<FastMap<Key,int> > constrainedKeys;
boost::optional<FastMap<Key, int> > constrainedKeys;
};
/**
* Add new variables to the ISAM2 system.
* @param newTheta Initial values for new variables
* @param theta Current solution to be augmented with new initialization
* @param delta Current linear delta to be augmented with zeros
* @param ordering Current ordering to be augmented with new variables
* @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
* @param keyFormatter Formatter for printing nonlinear keys during debugging
*/
static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta,
VectorValues& deltaNewton, VectorValues& RgProd,
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/// 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);
/**
* Remove variables from the ISAM2 system.
*/
static void RemoveVariables(const KeySet& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton,
VectorValues& RgProd, KeySet& replacedKeys, Base::Nodes& nodes,
KeySet& fixedVariables);
NonlinearFactorGraph* nonlinearFactors, FactorIndices* newFactorIndices);
/**
* Find the set of variables to be relinearized according to relinearizeThreshold.
@ -85,47 +63,13 @@ struct GTSAM_EXPORT ISAM2::Impl {
* @return The set of variable indices in delta whose magnitude is greater than or
* equal to relinearizeThreshold
*/
static KeySet CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
static KeySet CheckRelinearizationPartial(const ISAM2::Roots& roots,
const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
/**
* Recursively search this clique and its children for marked keys appearing
* in the separator, and add the *frontal* keys of any cliques whose
* separator contains any marked keys to the set \c keys. The purpose of
* this is to discover the cliques that need to be redone due to information
* propagating to them from cliques that directly contain factors being
* relinearized.
*
* The original comment says this finds all variables directly connected to
* the marked ones by measurements. Is this true, because it seems like this
* would also pull in variables indirectly connected through other frontal or
* separator variables?
*
* Alternatively could we trace up towards the root for each variable here?
*/
static void FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask);
/**
* Apply expmap to the given values, but only for indices appearing in
* \c markedRelinMask. Values are expmapped in-place.
* \param delta The linear delta with which to expmap
* \param mask Mask on linear indices, only \c true entries are expmapped
* \param [in, out] values The value to expmap in-place
* \param invalidateIfDebug If this is true, *and* NDEBUG is not defined,
* expmapped deltas will be set to an invalid value (infinity) to catch bugs
* where we might expmap something twice, or expmap it but then not
* recalculate its delta.
* @param keyFormatter Formatter for printing nonlinear keys during debugging
*/
static void ExpmapMasked(
const VectorValues& delta, const KeySet& mask, Values* values,
boost::optional<VectorValues&> invalidateIfDebug = boost::none,
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
* Update the Newton's method step point, using wildfire
*/
static size_t UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots,
static size_t UpdateGaussNewtonDelta(const ISAM2::Roots& roots,
const KeySet& replacedKeys, double wildfireThreshold, VectorValues* delta);
/**
@ -134,7 +78,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
*/
static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys,
const VectorValues& gradAtZero, VectorValues* RgProd);
/**
* Compute the gradient-search point. Only used in Dogleg.
*/

76
gtsam/nonlinear/ISAM2.cpp
View File

@ -33,6 +33,7 @@ using namespace boost::adaptors;
} // namespace br
#include <algorithm>
#include <limits>
#include <map>
#include <utility>
@ -484,6 +485,62 @@ boost::shared_ptr<KeySet> ISAM2::recalculate(
return affectedKeysSet;
}
/* ************************************************************************* */
void ISAM2::addVariables(const Values& newTheta) {
const bool debug = ISDEBUG("ISAM2 AddVariables");
theta_.insert(newTheta);
if (debug) newTheta.print("The new variables are: ");
// Add zeros into the VectorValues
delta_.insert(newTheta.zeroVectors());
deltaNewton_.insert(newTheta.zeroVectors());
RgProd_.insert(newTheta.zeroVectors());
}
/* ************************************************************************* */
void ISAM2::removeVariables(const KeySet& unusedKeys) {
variableIndex_.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end());
for (Key key : unusedKeys) {
delta_.erase(key);
deltaNewton_.erase(key);
RgProd_.erase(key);
deltaReplacedMask_.erase(key);
Base::nodes_.unsafe_erase(key);
theta_.erase(key);
fixedVariables_.erase(key);
}
}
/* ************************************************************************* */
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(
const NonlinearFactorGraph& newFactors, const Values& newTheta,
@ -529,7 +586,7 @@ ISAM2Result ISAM2::update(
// Add the new factor indices to the result struct
if (debug || verbose) newFactors.print("The new factors are: ");
Impl::AddFactorsStep1(newFactors, params_.findUnusedFactorSlots,
nonlinearFactors_, result.newFactorsIndices);
&nonlinearFactors_, &result.newFactorsIndices);
// Remove the removed factors
NonlinearFactorGraph removeFactors;
@ -571,7 +628,7 @@ ISAM2Result ISAM2::update(
gttic(add_new_variables);
// 2. Initialize any new variables \Theta_{new} and add
// \Theta:=\Theta\cup\Theta_{new}.
Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_);
addVariables(newTheta);
// New keys for detailed results
if (params_.enableDetailedResults) {
for (Key key : newTheta.keys()) {
@ -669,13 +726,13 @@ ISAM2Result ISAM2::update(
if (!relinKeys.empty()) {
for (const sharedClique& root : roots_)
// add other cliques that have the marked ones in the separator
Impl::FindAll(root, markedKeys, markedRelinMask);
root->findAll(markedRelinMask, &markedKeys);
// Relin involved keys for detailed results
if (params_.enableDetailedResults) {
KeySet involvedRelinKeys;
for (const sharedClique& root : roots_)
Impl::FindAll(root, involvedRelinKeys, markedRelinMask);
root->findAll(markedRelinMask, &involvedRelinKeys);
for (Key key : involvedRelinKeys) {
if (!result.detail->variableStatus[key].isAboveRelinThreshold) {
result.detail->variableStatus[key].isRelinearizeInvolved = true;
@ -689,8 +746,7 @@ ISAM2Result ISAM2::update(
gttic(expmap);
// 6. Update linearization point for marked variables:
// \Theta_{J}:=\Theta_{J}+\Delta_{J}.
if (!relinKeys.empty())
Impl::ExpmapMasked(delta_, markedRelinMask, &theta_, delta_);
if (!relinKeys.empty()) expmapMasked(markedRelinMask);
gttoc(expmap);
result.variablesRelinearized = markedKeys.size();
@ -740,9 +796,7 @@ ISAM2Result ISAM2::update(
// Update data structures to remove unused keys
if (!unusedKeys.empty()) {
gttic(remove_variables);
Impl::RemoveVariables(unusedKeys, roots_, theta_, variableIndex_, delta_,
deltaNewton_, RgProd_, deltaReplacedMask_,
Base::nodes_, fixedVariables_);
removeVariables(unusedKeys);
gttoc(remove_variables);
}
result.cliques = this->nodes().size();
@ -998,9 +1052,7 @@ void ISAM2::marginalizeLeaves(
factorIndicesToRemove.end());
// Remove the marginalized variables
Impl::RemoveVariables(KeySet(leafKeys.begin(), leafKeys.end()), roots_,
theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
deltaReplacedMask_, nodes_, fixedVariables_);
removeVariables(KeySet(leafKeys.begin(), leafKeys.end()));
}
/* ************************************************************************* */

24
gtsam/nonlinear/ISAM2.h
View File

@ -269,6 +269,29 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
/// @}
protected:
/**
* Add new variables to the ISAM2 system.
* @param newTheta Initial values for new variables
* @param theta Current solution to be augmented with new initialization
* @param delta Current linear delta to be augmented with zeros
* @param deltaNewton
* @param RgProd
* @param keyFormatter Formatter for printing nonlinear keys during debugging
*/
void addVariables(const Values& newTheta);
/**
* Remove variables from the ISAM2 system.
*/
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);
FastSet<Key> getAffectedFactors(const FastList<Key>& keys) const;
GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(
const FastList<Key>& affectedKeys, const KeySet& relinKeys) const;
@ -279,6 +302,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
const std::vector<Key>& observedKeys, const KeySet& unusedIndices,
const boost::optional<FastMap<Key, int> >& constrainKeys,
ISAM2Result& result);
void updateDelta(bool forceFullSolve = false) const;
}; // ISAM2

27
gtsam/nonlinear/ISAM2Clique.cpp
View File

@ -15,10 +15,10 @@
* @author Michael Kaess, Richard Roberts, Frank Dellaert
*/
#include <gtsam/nonlinear/ISAM2Clique.h>
#include <gtsam/inference/BayesTreeCliqueBase-inst.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/linearAlgorithms-inst.h>
#include <gtsam/inference/BayesTreeCliqueBase-inst.h>
#include <gtsam/nonlinear/ISAM2Clique.h>
#include <stack>
using namespace std;
@ -299,4 +299,27 @@ size_t ISAM2Clique::calculate_nnz() const {
return result;
}
/* ************************************************************************* */
void ISAM2Clique::findAll(const KeySet& markedMask, KeySet* keys) const {
static const bool debug = false;
// does the separator contain any of the variables?
bool found = false;
for (Key key : conditional()->parents()) {
if (markedMask.exists(key)) {
found = true;
break;
}
}
if (found) {
// then add this clique
keys->insert(conditional()->beginFrontals(), conditional()->endFrontals());
if (debug) print("Key(s) marked in clique ");
if (debug) cout << "so marking key " << conditional()->front() << endl;
}
for (const auto& child : children) {
child->findAll(markedMask, keys);
}
}
/* ************************************************************************* */
} // namespace gtsam

21
gtsam/nonlinear/ISAM2Clique.h
View File

@ -19,11 +19,11 @@
#pragma once
#include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/inference/Key.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/inference/Key.h>
#include <string>
namespace gtsam {
@ -99,6 +99,23 @@ class GTSAM_EXPORT ISAM2Clique
void nnz_internal(size_t* result) const;
size_t calculate_nnz() const;
/**
* Recursively search this clique and its children for marked keys appearing
* in the separator, and add the *frontal* keys of any cliques whose
* separator contains any marked keys to the set \c keys. The purpose of
* this is to discover the cliques that need to be redone due to information
* propagating to them from cliques that directly contain factors being
* relinearized.
*
* The original comment says this finds all variables directly connected to
* the marked ones by measurements. Is this true, because it seems like this
* would also pull in variables indirectly connected through other frontal or
* separator variables?
*
* Alternatively could we trace up towards the root for each variable here?
*/
void findAll(const KeySet& markedMask, KeySet* keys) const;
private:
/**
* Check if clique was replaced, or if any parents were changed above the

2
tests/testGaussianISAM2.cpp
View File

@ -304,7 +304,7 @@ TEST(ISAM2, AddFactorsStep1)
FactorIndices actualNewFactorIndices;
ISAM2::Impl::AddFactorsStep1(newFactors, true, nonlinearFactors, actualNewFactorIndices);
ISAM2::Impl::AddFactorsStep1(newFactors, true, &nonlinearFactors, &actualNewFactorIndices);
EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors));
EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices));