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);
394 for (Key key : *updateParams_.noRelinKeys) {
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,
442 const FactorIndices& newFactorsIndices,
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,
457 const FactorIndices& newFactorsIndices,
458 VariableIndex* variableIndex)
const {
459 gttic(augmentVariableIndex);
461 if (params_.findUnusedFactorSlots)
462 variableIndex->augment(newFactors, newFactorsIndices);
464 variableIndex->augment(newFactors);
468 for (
const auto& factorAddedKeys : *updateParams_.newAffectedKeys) {
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(
506 const ISAM2::Cliques& orphans) {
507 GaussianFactorGraph cachedBoundary;
509 for (
const auto& orphan : orphans) {
511 cachedBoundary.push_back(orphan->cachedFactor());
514 return cachedBoundary;