31#include <boost/range/adaptors.hpp>
32#include <boost/range/algorithm/copy.hpp>
34using namespace boost::range;
35using namespace boost::adaptors;
52 typedef boost::shared_ptr<This> shared_ptr;
61 :
public JunctionTree<ISAM2BayesTree, GaussianFactorGraph> {
65 typedef boost::shared_ptr<This> shared_ptr;
68 :
Base(eliminationTree) {}
78 size_t nFullSystemVars;
79 enum { COLAMD } algorithm;
80 enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain;
81 boost::optional<FastMap<Key, int> > constrainedKeys;
87 static size_t UpdateGaussNewtonDelta(
const ISAM2::Roots& roots,
88 const KeySet& replacedKeys,
89 double wildfireThreshold,
97 const KeySet& replacedKeys,
118 : params_(params), updateParams_(updateParams) {}
122 const ISAM2& isam2) {
123 gttic(pushBackFactors);
124 const bool debug = ISDEBUG(
"ISAM2 update");
125 const bool verbose = ISDEBUG(
"ISAM2 update verbose");
128 std::cout <<
"ISAM2::update\n";
129 isam2.
print(
"ISAM2: ");
132 if (debug || verbose) {
133 newFactors.
print(
"The new factors are: ");
139 bool relinarizationNeeded(
size_t update_count)
const {
141 (params_.enableRelinearization &&
142 update_count % params_.relinearizeSkip == 0);
151 KeySet* keysWithRemovedFactors)
const {
152 gttic(pushBackFactors);
158 *newFactorsIndices = nonlinearFactors->
add_factors(
159 newFactors, params_.findUnusedFactorSlots);
165 removedFactors.
push_back(nonlinearFactors->
at(index));
166 nonlinearFactors->
remove(index);
167 if (params_.cacheLinearizedFactors) linearFactors->
remove(index);
175 *keysWithRemovedFactors = removedFactors.
keys();
182 const KeySet& keysWithRemovedFactors,
183 KeySet* unusedKeys)
const {
184 gttic(computeUnusedKeys);
186 for (
Key key : keysWithRemovedFactors) {
187 if (variableIndex.
empty(key))
188 removedAndEmpty.insert(removedAndEmpty.end(), key);
191 std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(),
192 newFactorSymbKeys.begin(), newFactorSymbKeys.end(),
193 std::inserter(*unusedKeys, unusedKeys->end()));
198 const Values& estimate, boost::optional<double>* result)
const {
200 result->reset(nonlinearFactors.
error(estimate));
206 const KeySet& keysWithRemovedFactors,
207 KeySet* markedKeys)
const {
208 gttic(gatherInvolvedKeys);
209 *markedKeys = newFactors.
keys();
211 markedKeys->insert(keysWithRemovedFactors.begin(),
212 keysWithRemovedFactors.end());
217 markedKeys->insert(key);
225 const auto factorIdx = factorAddedKeys.first;
226 const auto& affectedKeys = nonlinearFactors.
at(factorIdx)->keys();
227 markedKeys->insert(affectedKeys.begin(), affectedKeys.end());
236 if (result->detail && params_.enableDetailedResults) {
237 for (
Key key : markedKeys) {
238 result->detail->variableStatus[key].isObserved =
true;
242 for (
Key index : markedKeys) {
244 if (result->unusedKeys.find(index) == result->unusedKeys.end())
246 result->observedKeys.push_back(index);
250 static void CheckRelinearizationRecursiveMap(
254 bool relinearize =
false;
255 for (
Key var : *clique->conditional()) {
257 const Vector& threshold = thresholds.find(
Symbol(var).chr())->second;
259 const Vector& deltaVar = delta[var];
262 if (threshold.rows() != deltaVar.rows())
263 throw std::invalid_argument(
264 "Relinearization threshold vector dimensionality for '" +
265 std::string(1,
Symbol(var).chr()) +
266 "' passed into iSAM2 parameters does not match actual variable "
270 if ((deltaVar.array().abs() > threshold.array()).any()) {
271 relinKeys->insert(var);
279 CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys);
284 static void CheckRelinearizationRecursiveDouble(
288 bool relinearize =
false;
289 for (
Key var : *clique->conditional()) {
290 double maxDelta = delta[var].lpNorm<Eigen::Infinity>();
291 if (maxDelta >= threshold) {
292 relinKeys->insert(var);
300 CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys);
323 if (relinearizeThreshold.type() ==
typeid(
double))
324 CheckRelinearizationRecursiveDouble(
325 boost::get<double>(relinearizeThreshold), delta, root, &relinKeys);
327 CheckRelinearizationRecursiveMap(
350 if (
const double* threshold = boost::get<double>(&relinearizeThreshold)) {
352 double maxDelta = key_delta.second.lpNorm<Eigen::Infinity>();
353 if (maxDelta >= *threshold) relinKeys.insert(key_delta.first);
358 const Vector& threshold =
359 thresholds->find(
Symbol(key_delta.first).
chr())->second;
360 if (threshold.rows() != key_delta.second.rows())
361 throw std::invalid_argument(
362 "Relinearization threshold vector dimensionality for '" +
363 std::string(1,
Symbol(key_delta.first).
chr()) +
364 "' passed into iSAM2 parameters does not match actual variable "
366 if ((key_delta.second.array().abs() > threshold.array()).any())
367 relinKeys.insert(key_delta.first);
377 const KeySet& fixedVariables,
378 KeySet* markedKeys)
const {
379 gttic(gatherRelinearizeKeys);
382 params_.enablePartialRelinearizationCheck
383 ? CheckRelinearizationPartial(roots, delta,
384 params_.relinearizeThreshold)
385 : CheckRelinearizationFull(delta, params_.relinearizeThreshold);
387 relinKeys = CheckRelinearizationFull(delta, 0.0);
390 for (
Key key : fixedVariables) {
391 relinKeys.erase(key);
395 relinKeys.erase(key);
400 markedKeys->insert(relinKeys.begin(), relinKeys.end());
405 void recordRelinearizeDetail(
const KeySet& relinKeys,
406 ISAM2Result::DetailedResults* detail)
const {
407 if (detail && params_.enableDetailedResults) {
408 for (
Key key : relinKeys) {
409 detail->variableStatus[key].isAboveRelinThreshold =
true;
410 detail->variableStatus[key].isRelinearized =
true;
417 void findFluid(
const ISAM2::Roots& roots,
const KeySet& relinKeys,
419 ISAM2Result::DetailedResults* detail)
const {
421 for (
const auto& root : roots)
423 root->findAll(relinKeys, markedKeys);
426 if (detail && params_.enableDetailedResults) {
427 KeySet involvedRelinKeys;
428 for (
const auto& root : roots)
429 root->findAll(relinKeys, &involvedRelinKeys);
430 for (
Key key : involvedRelinKeys) {
431 if (!detail->variableStatus[key].isAboveRelinThreshold) {
432 detail->variableStatus[key].isRelinearizeInvolved =
true;
433 detail->variableStatus[key].isRelinearized =
true;
440 void linearizeNewFactors(
const NonlinearFactorGraph& newFactors,
441 const Values& theta,
size_t numNonlinearFactors,
443 GaussianFactorGraph* linearFactors)
const {
444 gttic(linearizeNewFactors);
445 auto linearized = newFactors.linearize(theta);
446 if (params_.findUnusedFactorSlots) {
447 linearFactors->resize(numNonlinearFactors);
448 for (
size_t i = 0; i < newFactors.size(); ++i)
449 (*linearFactors)[newFactorsIndices[i]] = (*linearized)[i];
451 linearFactors->push_back(*linearized);
453 assert(linearFactors->size() == numNonlinearFactors);
456 void augmentVariableIndex(
const NonlinearFactorGraph& newFactors,
458 VariableIndex* variableIndex)
const {
459 gttic(augmentVariableIndex);
461 if (params_.findUnusedFactorSlots)
462 variableIndex->augment(newFactors, newFactorsIndices);
464 variableIndex->augment(newFactors);
469 const auto factorIdx = factorAddedKeys.first;
470 variableIndex->augmentExistingFactor(factorIdx, factorAddedKeys.second);
475 static void LogRecalculateKeys(
const ISAM2Result& result) {
476 const bool debug = ISDEBUG(
"ISAM2 recalculate");
479 std::cout <<
"markedKeys: ";
480 for (
const Key key : result.markedKeys) {
481 std::cout << key <<
" ";
483 std::cout << std::endl;
484 std::cout <<
"observedKeys: ";
485 for (
const Key key : result.observedKeys) {
486 std::cout << key <<
" ";
488 std::cout << std::endl;
492 static FactorIndexSet GetAffectedFactors(
const KeyList& keys,
493 const VariableIndex& variableIndex) {
494 gttic(GetAffectedFactors);
495 FactorIndexSet indices;
496 for (
const Key key : keys) {
498 indices.insert(factors.begin(), factors.end());
505 static GaussianFactorGraph GetCachedBoundaryFactors(
507 GaussianFactorGraph cachedBoundary;
509 for (
const auto& orphan : orphans) {
511 cachedBoundary.push_back(orphan->cachedFactor());
514 return cachedBoundary;
The junction tree, template bodies.
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
Class that stores detailed iSAM2 result.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:34
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition: FastMap.h:38
KeySet keys() const
Potentially slow function to return all keys involved, sorted, as a set.
Definition: FactorGraph-inst.h:85
FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false)
Add new factors to a factor graph and returns a list of new factor indices, optionally finding and re...
Definition: FactorGraph-inst.h:109
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:186
void remove(size_t i)
delete factor without re-arranging indexes by inserting a nullptr pointer
Definition: FactorGraph.h:385
const sharedFactor at(size_t i) const
Get a specific factor by index (this checks array bounds and may throw an exception,...
Definition: FactorGraph.h:335
void reserve(size_t size)
Reserve space for the specified number of factors if you know in advance how many there will be (work...
Definition: FactorGraph.h:182
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print
Definition: BayesTree-inst.h:212
FastVector< sharedClique > Roots
Root cliques.
Definition: BayesTree.h:95
A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
Definition: JunctionTree.h:50
Character and index key used to refer to variables.
Definition: Symbol.h:35
unsigned char chr() const
Retrieve key character.
Definition: Symbol.h:73
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition: VariableIndex.h:43
void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG &factors)
Remove entries corresponding to the specified factors.
Definition: VariableIndex-inl.h:54
bool empty(Key variable) const
Return true if no factors associated with a variable.
Definition: VariableIndex.h:98
Definition: GaussianEliminationTree.h:29
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:75
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition: VectorValues.h:74
Definition: ISAM2-impl.h:48
Definition: ISAM2-impl.h:61
Definition: ISAM2-impl.h:72
Definition: ISAM2-impl.h:73
Definition: ISAM2-impl.h:77
Implementation functions for update method All of the methods below have clear inputs and outputs,...
Definition: ISAM2-impl.h:114
static KeySet CheckRelinearizationFull(const VectorValues &delta, const ISAM2Params::RelinearizationThreshold &relinearizeThreshold)
Find the set of variables to be relinearized according to relinearizeThreshold.
Definition: ISAM2-impl.h:345
static KeySet CheckRelinearizationPartial(const ISAM2::Roots &roots, const VectorValues &delta, const ISAM2Params::RelinearizationThreshold &relinearizeThreshold)
Find the set of variables to be relinearized according to relinearizeThreshold.
Definition: ISAM2-impl.h:318
Implementation of the full ISAM2 algorithm for incremental nonlinear optimization.
Definition: ISAM2.h:45
Base::Cliques Cliques
List of Cliques.
Definition: ISAM2.h:104
Base::sharedClique sharedClique
Shared pointer to a clique.
Definition: ISAM2.h:103
Definition: ISAM2Params.h:135
boost::variant< double, FastMap< char, Vector > > RelinearizationThreshold
Either a constant relinearization threshold or a per-variable-type set of thresholds.
Definition: ISAM2Params.h:140
This struct is returned from ISAM2::update() and contains information about the update that is useful...
Definition: ISAM2Result.h:41
This struct is used by ISAM2::update() to pass additional parameters to give the user a fine-grained ...
Definition: ISAM2UpdateParams.h:32
bool force_relinearize
Relinearize any variables whose delta magnitude is sufficiently large (Params::relinearizeThreshold),...
Definition: ISAM2UpdateParams.h:54
FactorIndices removeFactorIndices
Indices of factors to remove from system (default: empty)
Definition: ISAM2UpdateParams.h:36
bool forceFullSolve
By default, iSAM2 uses a wildfire update scheme that stops updating when the deltas become too small ...
Definition: ISAM2UpdateParams.h:71
boost::optional< FastList< Key > > extraReelimKeys
An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless of the size of the linear ...
Definition: ISAM2UpdateParams.h:49
boost::optional< FastList< Key > > noRelinKeys
An optional set of nonlinear keys that iSAM2 will hold at a constant linearization point,...
Definition: ISAM2UpdateParams.h:44
boost::optional< FastMap< FactorIndex, KeySet > > newAffectedKeys
An optional set of new Keys that are now affected by factors, indexed by factor indices (as returned ...
Definition: ISAM2UpdateParams.h:66
Definition: NonlinearFactorGraph.h:55
void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: NonlinearFactorGraph.cpp:55
double error(const Values &values) const
unnormalized error, in the most common case
Definition: NonlinearFactorGraph.cpp:170
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:65
A key-value pair, which you get by dereferencing iterators.
Definition: Values.h:93
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...