IndexedJunctionTree.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/base/types.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/symbolic/SymbolicEliminationTree.h>
#include <gtsam/symbolic/SymbolicFactor.h>
#include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <gtsam/symbolic/SymbolicJunctionTree.h>
 
#include <cstddef>
#include <unordered_set>
 
namespace gtsam {
 
namespace internal {
class IndexedSymbolicFactor : public SymbolicFactor {
 public:
  size_t index_;
  IndexedSymbolicFactor(const KeyVector& keys, size_t index)
      : SymbolicFactor(), index_(index) {
    keys_ = keys;
  }
};
}  // namespace internal

class GTSAM_EXPORT IndexedJunctionTree : public SymbolicJunctionTree {
 public:
  using SymbolicJunctionTree::SymbolicJunctionTree;

  template <typename GRAPH>
  IndexedJunctionTree(const GRAPH& graph, const Ordering& ordering,
                      const std::unordered_set<Key>& fixedKeys = {})
      : SymbolicJunctionTree(makeIndexedEliminationTree(graph, ordering, fixedKeys)) {}
 
 private:
  template <typename GRAPH>
  static SymbolicFactorGraph buildIndexedSymbolicFactorGraph(
      const GRAPH& graph, const std::unordered_set<Key>& fixedKeys) {
    SymbolicFactorGraph symbolicGraph;
    symbolicGraph.reserve(graph.size());
    for (size_t i = 0; i < graph.size(); ++i) {
      if (!graph.at(i)) continue;
      KeyVector keys;
      keys.reserve(graph[i]->size());
      for (Key key : graph[i]->keys()) {
        if (!fixedKeys.count(key)) keys.push_back(key);
      }
      // Skip factors that are fully constrained away.
      if (keys.empty()) continue;
      symbolicGraph.emplace_shared<internal::IndexedSymbolicFactor>(keys, i);
    }
    return symbolicGraph;
  }
 
  template <typename GRAPH>
  static SymbolicEliminationTree makeIndexedEliminationTree(
      const GRAPH& graph, const Ordering& ordering,
      const std::unordered_set<Key>& fixedKeys) {
    SymbolicFactorGraph symbolicGraph =
        buildIndexedSymbolicFactorGraph(graph, fixedKeys);
    return SymbolicEliminationTree(std::move(symbolicGraph), ordering);
  }
};
}  // namespace gtsam