ConcurrentFilteringAndSmoothing.cpp

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

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

// \callgraph

#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>

namespace gtsam {

/* ************************************************************************* */
void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother) {

  NonlinearFactorGraph smootherFactors, filterSumarization, smootherSummarization;
  Values smootherValues, filterSeparatorValues, smootherSeparatorValues;

  // Call the pre-sync functions of the filter and smoother
  filter.presync();
  smoother.presync();

  // Get the updates from the smoother and apply them to the filter
  smoother.getSummarizedFactors(smootherSummarization, smootherSeparatorValues);
  filter.synchronize(smootherSummarization, smootherSeparatorValues);

  // Get the updates from the filter and apply them to the smoother
  filter.getSmootherFactors(smootherFactors, smootherValues);
  filter.getSummarizedFactors(filterSumarization, filterSeparatorValues);
  smoother.synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);

  // Call the post-sync functions of the filter and smoother
  filter.postsync();
  smoother.postsync();
}

namespace internal {

/* ************************************************************************* */
NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta,
    const KeySet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) {


  // Calculate the set of RootKeys = AllKeys \Intersect RemainingKeys
  KeySet rootKeys;
  KeySet allKeys(graph.keys());
  std::set_intersection(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(rootKeys, rootKeys.end()));

  // Calculate the set of MarginalizeKeys = AllKeys - RemainingKeys
  KeySet marginalizeKeys;
  std::set_difference(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(marginalizeKeys, marginalizeKeys.end()));

  if(marginalizeKeys.size() == 0) {
    // There are no keys to marginalize. Simply return the input factors
    return graph;
  } else {
    // Create the linear factor graph
    GaussianFactorGraph linearFactorGraph = *graph.linearize(theta);
    // .first is the eliminated Bayes tree, while .second is the remaining factor graph
    GaussianFactorGraph marginalLinearFactors = *linearFactorGraph.eliminatePartialMultifrontal(
        KeyVector(marginalizeKeys.begin(), marginalizeKeys.end()), eliminateFunction).second;

    // Wrap in nonlinear container factors
    NonlinearFactorGraph marginalFactors;
    marginalFactors.reserve(marginalLinearFactors.size());
    for(const GaussianFactor::shared_ptr& gaussianFactor: marginalLinearFactors) {
      marginalFactors.emplace_shared<LinearContainerFactor>(gaussianFactor, theta);
    }

    return marginalFactors;
  }
}

/* ************************************************************************* */
}

/* ************************************************************************* */
}