ISAM-inst.h

/* ----------------------------------------------------------------------------

 * 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/inference/ISAM.h>
#include <gtsam/inference/VariableIndex.h>

namespace gtsam {

/* ************************************************************************* */
template<class BAYESTREE>
void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors,
    Cliques& orphans, const Eliminate& function) {
  // Remove the contaminated part of the Bayes tree
  BayesNetType bn;
  const KeySet newFactorKeys = newFactors.keys();
  if (!this->empty()) {
    KeyVector keyVector(newFactorKeys.begin(), newFactorKeys.end());
    this->removeTop(keyVector, bn, orphans);
  }

  // Add the removed top and the new factors
  FactorGraphType factors;
  factors += bn;
  factors += newFactors;

  // Add the orphaned subtrees
  for (const sharedClique& orphan : orphans)
    factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);

  // Get an ordering where the new keys are eliminated last
  const VariableIndex index(factors);
  const Ordering ordering = Ordering::ColamdConstrainedLast(index,
      KeyVector(newFactorKeys.begin(), newFactorKeys.end()));

  // eliminate all factors (top, added, orphans) into a new Bayes tree
  auto bayesTree = factors.eliminateMultifrontal(ordering, function, index);

  // Re-add into Bayes tree data structures
  this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(),
      bayesTree->roots().end());
  this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end());
}

/* ************************************************************************* */
template<class BAYESTREE>
void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors,
    const Eliminate& function) {
  Cliques orphans;
  this->update_internal(newFactors, orphans, function);
}

}