EliminateableFactorGraph-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/EliminateableFactorGraph.h>
#include <gtsam/inference/inferenceExceptions.h>
#include <boost/tuple/tuple.hpp>

namespace gtsam {

  /* ************************************************************************* */
  template<class FACTORGRAPH>
  boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
    EliminateableFactorGraph<FACTORGRAPH>::eliminateSequential(
    OptionalOrdering ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
  {
    if(ordering && variableIndex) {
      gttic(eliminateSequential);
      // Do elimination
      EliminationTreeType etree(asDerived(), *variableIndex, *ordering);
      boost::shared_ptr<BayesNetType> bayesNet;
      boost::shared_ptr<FactorGraphType> factorGraph;
      boost::tie(bayesNet,factorGraph) = etree.eliminate(function);
      // If any factors are remaining, the ordering was incomplete
      if(!factorGraph->empty())
        throw InconsistentEliminationRequested();
      // Return the Bayes net
      return bayesNet;
    }
    else if(!variableIndex) {
      // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
      // for no variable index first so that it's always computed if we need to call COLAMD because
      // no Ordering is provided.
      return eliminateSequential(ordering, function, VariableIndex(asDerived()));
    }
    else /*if(!ordering)*/ {
      // If no Ordering provided, compute one and call this function again.  We are guaranteed to
      // have a VariableIndex already here because we computed one if needed in the previous 'else'
      // block.
      return eliminateSequential(Ordering::COLAMD(*variableIndex), function);
    }
  }

  /* ************************************************************************* */
  template<class FACTORGRAPH>
  boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
    EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
    OptionalOrdering ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
  {
    if(ordering && variableIndex) {
      gttic(eliminateMultifrontal);
      // Do elimination with given ordering
      EliminationTreeType etree(asDerived(), *variableIndex, *ordering);
      JunctionTreeType junctionTree(etree);
      boost::shared_ptr<BayesTreeType> bayesTree;
      boost::shared_ptr<FactorGraphType> factorGraph;
      boost::tie(bayesTree,factorGraph) = junctionTree.eliminate(function);
      // If any factors are remaining, the ordering was incomplete
      if(!factorGraph->empty())
        throw InconsistentEliminationRequested();
      // Return the Bayes tree
      return bayesTree;
    }
    else if(!variableIndex) {
      // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
      // for no variable index first so that it's always computed if we need to call COLAMD because
      // no Ordering is provided.
      return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()));
    }
    else /*if(!ordering)*/ {
      // If no Ordering provided, compute one and call this function again.  We are guaranteed to
      // have a VariableIndex already here because we computed one if needed in the previous 'else'
      // block.
      return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function);
    }
  }

  /* ************************************************************************* */
  template<class FACTORGRAPH>
  std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
    EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential(
    const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
  {
    if(variableIndex) {
      gttic(eliminatePartialSequential);
      // Do elimination
      EliminationTreeType etree(asDerived(), *variableIndex, ordering);
      return etree.eliminate(function);
    } else {
      // If no variable index is provided, compute one and call this function again
      return eliminatePartialSequential(ordering, function, VariableIndex(asDerived()));
    }
  }

  /* ************************************************************************* */
  template<class FACTORGRAPH>
  std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
    EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential(
    const std::vector<Key>& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
  {
    if(variableIndex) {
      gttic(eliminatePartialSequential);
      // Compute full ordering
      Ordering fullOrdering = Ordering::COLAMDConstrainedFirst(*variableIndex, variables);

      // Split off the part of the ordering for the variables being eliminated
      Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
      return eliminatePartialSequential(ordering, function, variableIndex);
    } else {
      // If no variable index is provided, compute one and call this function again
      return eliminatePartialSequential(variables, function, VariableIndex(asDerived()));
    }
  }

  /* ************************************************************************* */
  template<class FACTORGRAPH>
  std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
    EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal(
    const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
  {
    if(variableIndex) {
      gttic(eliminatePartialMultifrontal);
      // Do elimination
      EliminationTreeType etree(asDerived(), *variableIndex, ordering);
      JunctionTreeType junctionTree(etree);
      return junctionTree.eliminate(function);
    } else {
      // If no variable index is provided, compute one and call this function again
      return eliminatePartialMultifrontal(ordering, function, VariableIndex(asDerived()));
    }
  }

  /* ************************************************************************* */
  template<class FACTORGRAPH>
  std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
    EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal(
    const std::vector<Key>& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
  {
    if(variableIndex) {
      gttic(eliminatePartialMultifrontal);
      // Compute full ordering
      Ordering fullOrdering = Ordering::COLAMDConstrainedFirst(*variableIndex, variables);

      // Split off the part of the ordering for the variables being eliminated
      Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
      return eliminatePartialMultifrontal(ordering, function, variableIndex);
    } else {
      // If no variable index is provided, compute one and call this function again
      return eliminatePartialMultifrontal(variables, function, VariableIndex(asDerived()));
    }
  }

  /* ************************************************************************* */
  template<class FACTORGRAPH>
  boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
    EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
    boost::variant<const Ordering&, const std::vector<Key>&> variables,
    OptionalOrdering marginalizedVariableOrdering,
    const Eliminate& function, OptionalVariableIndex variableIndex) const
  {
    if(variableIndex)
    {
      if(marginalizedVariableOrdering)
      {
        gttic(marginalMultifrontalBayesNet);
        // An ordering was provided for the marginalized variables, so we can first eliminate them
        // in the order requested.
        boost::shared_ptr<BayesTreeType> bayesTree;
        boost::shared_ptr<FactorGraphType> factorGraph;
        boost::tie(bayesTree,factorGraph) =
          eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex);

        if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
        {
          // An ordering was also provided for the unmarginalized variables, so we can also
          // eliminate them in the order requested.
          return factorGraph->eliminateSequential(*varsAsOrdering, function);
        }
        else
        {
          // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
          return factorGraph->eliminateSequential(boost::none, function);
        }
      }
      else
      {
        // No ordering was provided for the marginalized variables, so order them using constrained
        // COLAMD.
        bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
        const std::vector<Key>* variablesOrOrdering =
          unmarginalizedAreOrdered ?
          boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables);

        Ordering totalOrdering =
          Ordering::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);

        // Split up ordering
        const size_t nVars = variablesOrOrdering->size();
        Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
        Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());

        // Call this function again with the computed orderings
        return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
      }
    } else {
      // If no variable index is provided, compute one and call this function again
      VariableIndex index(asDerived());
      return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, index);
    }
  }

  /* ************************************************************************* */
  template<class FACTORGRAPH>
  boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
    EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
    boost::variant<const Ordering&, const std::vector<Key>&> variables,
    OptionalOrdering marginalizedVariableOrdering,
    const Eliminate& function, OptionalVariableIndex variableIndex) const
  {
    if(variableIndex)
    {
      if(marginalizedVariableOrdering)
      {
        gttic(marginalMultifrontalBayesTree);
        // An ordering was provided for the marginalized variables, so we can first eliminate them
        // in the order requested.
        boost::shared_ptr<BayesTreeType> bayesTree;
        boost::shared_ptr<FactorGraphType> factorGraph;
        boost::tie(bayesTree,factorGraph) =
          eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex);

        if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
        {
          // An ordering was also provided for the unmarginalized variables, so we can also
          // eliminate them in the order requested.
          return factorGraph->eliminateMultifrontal(*varsAsOrdering, function);
        }
        else
        {
          // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
          return factorGraph->eliminateMultifrontal(boost::none, function);
        }
      }
      else
      {
        // No ordering was provided for the marginalized variables, so order them using constrained
        // COLAMD.
        bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
        const std::vector<Key>* variablesOrOrdering =
          unmarginalizedAreOrdered ?
          boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables);

        Ordering totalOrdering =
          Ordering::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);

        // Split up ordering
        const size_t nVars = variablesOrOrdering->size();
        Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
        Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());

        // Call this function again with the computed orderings
        return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
      }
    } else {
      // If no variable index is provided, compute one and call this function again
      return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, VariableIndex(asDerived()));
    }
  }

  /* ************************************************************************* */
  template<class FACTORGRAPH>
  boost::shared_ptr<FACTORGRAPH>
    EliminateableFactorGraph<FACTORGRAPH>::marginal(
    const std::vector<Key>& variables,
    const Eliminate& function, OptionalVariableIndex variableIndex) const
  {
    if(variableIndex)
    {
      // Compute a total ordering for all variables
      Ordering totalOrdering = Ordering::COLAMDConstrainedLast(*variableIndex, variables);

      // Split out the part for the marginalized variables
      Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size());

      // Eliminate and return the remaining factor graph
      return eliminatePartialMultifrontal(marginalizationOrdering, function, *variableIndex).second;
    }
    else
    {
      // If no variable index is provided, compute one and call this function again
      return marginal(variables, function, VariableIndex(asDerived()));
    }
  }


}