ISAM2-impl.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#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 <algorithm>
#include <limits>
#include <string>
#include <utility>
#include <variant>
#include <cassert>
 
namespace gtsam {
 
/* ************************************************************************* */
// 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 std::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 std::shared_ptr<This> shared_ptr;
 
  explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree)
      : Base(eliminationTree) {}
};
 
/* ************************************************************************* */
struct GTSAM_EXPORT DeltaImpl {
  struct GTSAM_EXPORT PartialSolveResult {
    ISAM2::sharedClique bayesTree;
  };
 
  struct GTSAM_EXPORT ReorderingMode {
    size_t nFullSystemVars;
    enum { /*AS_ADDED,*/ COLAMD } algorithm;
    enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain;
    std::optional<FastMap<Key, int> > constrainedKeys;
  };

  static size_t UpdateGaussNewtonDelta(const ISAM2::Roots& roots,
                                       const KeySet& replacedKeys,
                                       double wildfireThreshold,
                                       VectorValues* delta);

  static size_t UpdateRgProd(const ISAM2::Roots& roots,
                             const KeySet& replacedKeys,
                             const VectorValues& gradAtZero,
                             VectorValues* RgProd);

  static VectorValues ComputeGradientSearch(const VectorValues& gradAtZero,
                                            const VectorValues& RgProd);
};
 
/* ************************************************************************* */
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);
    // Most incremental updates remove no factors, so no key can become unused.
    if (keysWithRemovedFactors.empty()) return;
    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, std::optional<double>* result) const {
    gttic(error);
    *result = 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);
      }
    }
  }

  static KeySet CheckRelinearizationPartial(
      const ISAM2::Roots& roots, const VectorValues& delta,
      const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
    KeySet relinKeys;
    for (const ISAM2::sharedClique& root : roots) {
      if (std::holds_alternative<double>(relinearizeThreshold)) {
        CheckRelinearizationRecursiveDouble(
            std::get<double>(relinearizeThreshold), delta, root, &relinKeys);
      } else if (std::holds_alternative<FastMap<char, Vector>>(relinearizeThreshold)) {
        CheckRelinearizationRecursiveMap(
            std::get<FastMap<char, Vector> >(relinearizeThreshold), delta,
            root, &relinKeys);
      }
    }
    return relinKeys;
  }

  static KeySet CheckRelinearizationFull(
      const VectorValues& delta,
      const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
    KeySet relinKeys;
 
    if (const double* threshold = std::get_if<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 =
                   std::get_if<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;
        }
      }
    }
  }
 
  // 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