Re-ordered functions to minimize diff
Frank Dellaert
parent
d385c1e698
commit
ebae189c7b
|
|
@ -58,6 +58,118 @@ bool ISAM2::equals(const ISAM2& other, double tol) const {
|
||||||
fixedVariables_ == other.fixedVariables_;
|
fixedVariables_ == other.fixedVariables_;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
GaussianFactorGraph ISAM2::relinearizeAffectedFactors(
|
||||||
|
const ISAM2UpdateParams& updateParams, const FastList<Key>& affectedKeys,
|
||||||
|
const KeySet& relinKeys) {
|
||||||
|
gttic(relinearizeAffectedFactors);
|
||||||
|
FactorIndexSet candidates =
|
||||||
|
UpdateImpl::GetAffectedFactors(affectedKeys, variableIndex_);
|
||||||
|
|
||||||
|
gttic(affectedKeysSet);
|
||||||
|
// for fast lookup below
|
||||||
|
KeySet affectedKeysSet;
|
||||||
|
affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end());
|
||||||
|
gttoc(affectedKeysSet);
|
||||||
|
|
||||||
|
gttic(check_candidates_and_linearize);
|
||||||
|
GaussianFactorGraph linearized;
|
||||||
|
for (const FactorIndex idx : candidates) {
|
||||||
|
bool inside = true;
|
||||||
|
bool useCachedLinear = params_.cacheLinearizedFactors;
|
||||||
|
for (Key key : nonlinearFactors_[idx]->keys()) {
|
||||||
|
if (affectedKeysSet.find(key) == affectedKeysSet.end()) {
|
||||||
|
inside = false;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
if (useCachedLinear && relinKeys.find(key) != relinKeys.end())
|
||||||
|
useCachedLinear = false;
|
||||||
|
}
|
||||||
|
if (inside) {
|
||||||
|
if (useCachedLinear) {
|
||||||
|
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||||
|
assert(linearFactors_[idx]);
|
||||||
|
assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->keys());
|
||||||
|
#endif
|
||||||
|
linearized.push_back(linearFactors_[idx]);
|
||||||
|
} else {
|
||||||
|
auto linearFactor = nonlinearFactors_[idx]->linearize(theta_);
|
||||||
|
linearized.push_back(linearFactor);
|
||||||
|
if (params_.cacheLinearizedFactors) {
|
||||||
|
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||||
|
assert(linearFactors_[idx]->keys() == linearFactor->keys());
|
||||||
|
#endif
|
||||||
|
linearFactors_[idx] = linearFactor;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
gttoc(check_candidates_and_linearize);
|
||||||
|
|
||||||
|
return linearized;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
void ISAM2::recalculate(const ISAM2UpdateParams& updateParams,
|
||||||
|
const KeySet& relinKeys, ISAM2Result* result) {
|
||||||
|
gttic(recalculate);
|
||||||
|
UpdateImpl::LogRecalculateKeys(*result);
|
||||||
|
|
||||||
|
if (!result->markedKeys.empty() || !result->observedKeys.empty()) {
|
||||||
|
// Remove top of Bayes tree and convert to a factor graph:
|
||||||
|
// (a) For each affected variable, remove the corresponding clique and all
|
||||||
|
// parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of
|
||||||
|
// removed cliques.
|
||||||
|
GaussianBayesNet affectedBayesNet;
|
||||||
|
Cliques orphans;
|
||||||
|
this->removeTop(
|
||||||
|
KeyVector(result->markedKeys.begin(), result->markedKeys.end()),
|
||||||
|
&affectedBayesNet, &orphans);
|
||||||
|
|
||||||
|
// FactorGraph<GaussianFactor> factors(affectedBayesNet);
|
||||||
|
// bug was here: we cannot reuse the original factors, because then the
|
||||||
|
// cached factors get messed up [all the necessary data is actually
|
||||||
|
// contained in the affectedBayesNet, including what was passed in from the
|
||||||
|
// boundaries, so this would be correct; however, in the process we also
|
||||||
|
// generate new cached_ entries that will be wrong (ie. they don't contain
|
||||||
|
// what would be passed up at a certain point if batch elimination was done,
|
||||||
|
// but that's what we need); we could choose not to update cached_ from
|
||||||
|
// here, but then the new information (and potentially different variable
|
||||||
|
// ordering) is not reflected in the cached_ values which again will be
|
||||||
|
// wrong] so instead we have to retrieve the original linearized factors AND
|
||||||
|
// add the cached factors from the boundary
|
||||||
|
|
||||||
|
// ordering provides all keys in conditionals, there cannot be others
|
||||||
|
// because path to root included
|
||||||
|
gttic(affectedKeys);
|
||||||
|
FastList<Key> affectedKeys;
|
||||||
|
for (const auto& conditional : affectedBayesNet)
|
||||||
|
affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(),
|
||||||
|
conditional->endFrontals());
|
||||||
|
gttoc(affectedKeys);
|
||||||
|
|
||||||
|
KeySet affectedKeysSet;
|
||||||
|
static const double kBatchThreshold = 0.65;
|
||||||
|
if (affectedKeys.size() >= theta_.size() * kBatchThreshold) {
|
||||||
|
// Do a batch step - reorder and relinearize all variables
|
||||||
|
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,
|
void ISAM2::recalculateBatch(const ISAM2UpdateParams& updateParams,
|
||||||
KeySet* affectedKeysSet, ISAM2Result* result) {
|
KeySet* affectedKeysSet, ISAM2Result* result) {
|
||||||
|
|
@ -128,57 +240,6 @@ void ISAM2::recalculateBatch(const ISAM2UpdateParams& updateParams,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
GaussianFactorGraph ISAM2::relinearizeAffectedFactors(
|
|
||||||
const ISAM2UpdateParams& updateParams, const FastList<Key>& affectedKeys,
|
|
||||||
const KeySet& relinKeys) {
|
|
||||||
gttic(relinearizeAffectedFactors);
|
|
||||||
FactorIndexSet candidates =
|
|
||||||
UpdateImpl::GetAffectedFactors(affectedKeys, variableIndex_);
|
|
||||||
|
|
||||||
gttic(affectedKeysSet);
|
|
||||||
// for fast lookup below
|
|
||||||
KeySet affectedKeysSet;
|
|
||||||
affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end());
|
|
||||||
gttoc(affectedKeysSet);
|
|
||||||
|
|
||||||
gttic(check_candidates_and_linearize);
|
|
||||||
GaussianFactorGraph linearized;
|
|
||||||
for (const FactorIndex idx : candidates) {
|
|
||||||
bool inside = true;
|
|
||||||
bool useCachedLinear = params_.cacheLinearizedFactors;
|
|
||||||
for (Key key : nonlinearFactors_[idx]->keys()) {
|
|
||||||
if (affectedKeysSet.find(key) == affectedKeysSet.end()) {
|
|
||||||
inside = false;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
if (useCachedLinear && relinKeys.find(key) != relinKeys.end())
|
|
||||||
useCachedLinear = false;
|
|
||||||
}
|
|
||||||
if (inside) {
|
|
||||||
if (useCachedLinear) {
|
|
||||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
|
||||||
assert(linearFactors_[idx]);
|
|
||||||
assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->keys());
|
|
||||||
#endif
|
|
||||||
linearized.push_back(linearFactors_[idx]);
|
|
||||||
} else {
|
|
||||||
auto linearFactor = nonlinearFactors_[idx]->linearize(theta_);
|
|
||||||
linearized.push_back(linearFactor);
|
|
||||||
if (params_.cacheLinearizedFactors) {
|
|
||||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
|
||||||
assert(linearFactors_[idx]->keys() == linearFactor->keys());
|
|
||||||
#endif
|
|
||||||
linearFactors_[idx] = linearFactor;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
gttoc(check_candidates_and_linearize);
|
|
||||||
|
|
||||||
return linearized;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
void ISAM2::recalculateIncremental(const ISAM2UpdateParams& updateParams,
|
void ISAM2::recalculateIncremental(const ISAM2UpdateParams& updateParams,
|
||||||
const KeySet& relinKeys,
|
const KeySet& relinKeys,
|
||||||
|
|
@ -295,66 +356,6 @@ void ISAM2::recalculateIncremental(const ISAM2UpdateParams& updateParams,
|
||||||
// 4. The orphans have already been inserted during elimination
|
// 4. The orphans have already been inserted during elimination
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
void ISAM2::recalculate(const ISAM2UpdateParams& updateParams,
|
|
||||||
const KeySet& relinKeys, ISAM2Result* result) {
|
|
||||||
gttic(recalculate);
|
|
||||||
UpdateImpl::LogRecalculateKeys(*result);
|
|
||||||
|
|
||||||
if (!result->markedKeys.empty() || !result->observedKeys.empty()) {
|
|
||||||
// Remove top of Bayes tree and convert to a factor graph:
|
|
||||||
// (a) For each affected variable, remove the corresponding clique and all
|
|
||||||
// parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of
|
|
||||||
// removed cliques.
|
|
||||||
GaussianBayesNet affectedBayesNet;
|
|
||||||
Cliques orphans;
|
|
||||||
this->removeTop(
|
|
||||||
KeyVector(result->markedKeys.begin(), result->markedKeys.end()),
|
|
||||||
&affectedBayesNet, &orphans);
|
|
||||||
|
|
||||||
// FactorGraph<GaussianFactor> factors(affectedBayesNet);
|
|
||||||
// bug was here: we cannot reuse the original factors, because then the
|
|
||||||
// cached factors get messed up [all the necessary data is actually
|
|
||||||
// contained in the affectedBayesNet, including what was passed in from the
|
|
||||||
// boundaries, so this would be correct; however, in the process we also
|
|
||||||
// generate new cached_ entries that will be wrong (ie. they don't contain
|
|
||||||
// what would be passed up at a certain point if batch elimination was done,
|
|
||||||
// but that's what we need); we could choose not to update cached_ from
|
|
||||||
// here, but then the new information (and potentially different variable
|
|
||||||
// ordering) is not reflected in the cached_ values which again will be
|
|
||||||
// wrong] so instead we have to retrieve the original linearized factors AND
|
|
||||||
// add the cached factors from the boundary
|
|
||||||
|
|
||||||
// ordering provides all keys in conditionals, there cannot be others
|
|
||||||
// because path to root included
|
|
||||||
gttic(affectedKeys);
|
|
||||||
FastList<Key> affectedKeys;
|
|
||||||
for (const auto& conditional : affectedBayesNet)
|
|
||||||
affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(),
|
|
||||||
conditional->endFrontals());
|
|
||||||
gttoc(affectedKeys);
|
|
||||||
|
|
||||||
KeySet affectedKeysSet;
|
|
||||||
static const double kBatchThreshold = 0.65;
|
|
||||||
if (affectedKeys.size() >= theta_.size() * kBatchThreshold) {
|
|
||||||
// Do a batch step - reorder and relinearize all variables
|
|
||||||
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::addVariables(const Values& newTheta,
|
void ISAM2::addVariables(const Values& newTheta,
|
||||||
ISAM2Result::DetailedResults* detail) {
|
ISAM2Result::DetailedResults* detail) {
|
||||||
|
|
|
||||||
|
|
@ -281,6 +281,10 @@ 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
|
// Do a batch step - reorder and relinearize all variables
|
||||||
void recalculateBatch(const ISAM2UpdateParams& updateParams,
|
void recalculateBatch(const ISAM2UpdateParams& updateParams,
|
||||||
KeySet* affectedKeysSet, ISAM2Result* result);
|
KeySet* affectedKeysSet, ISAM2Result* result);
|
||||||
|
|
@ -296,11 +300,6 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
|
||||||
const FastList<Key>& affectedKeys,
|
const FastList<Key>& affectedKeys,
|
||||||
KeySet* affectedKeysSet, Cliques* orphans,
|
KeySet* affectedKeysSet, Cliques* orphans,
|
||||||
ISAM2Result* result);
|
ISAM2Result* result);
|
||||||
/**
|
|
||||||
* Remove marked top and either recalculate in batch or incrementally.
|
|
||||||
*/
|
|
||||||
void recalculate(const ISAM2UpdateParams& updateParams,
|
|
||||||
const KeySet& relinKeys, ISAM2Result* result);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Add new variables to the ISAM2 system.
|
* Add new variables to the ISAM2 system.
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue