gtsam  4.1.0
gtsam
SymbolicFactor-inst.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 #pragma once
19 
22 #include <gtsam/inference/Factor.h>
23 #include <gtsam/inference/Key.h>
24 #include <gtsam/base/timing.h>
25 
26 #include <boost/shared_ptr.hpp>
27 #include <boost/make_shared.hpp>
28 
29 #include <utility>
30 
31 namespace gtsam
32 {
33  namespace internal
34  {
37  template<class FACTOR>
38  std::pair<boost::shared_ptr<SymbolicConditional>, boost::shared_ptr<SymbolicFactor> >
39  EliminateSymbolic(const FactorGraph<FACTOR>& factors, const Ordering& keys)
40  {
41  gttic(EliminateSymbolic);
42 
43  // Gather all keys
44  KeySet allKeys;
45  for(const boost::shared_ptr<FACTOR>& factor: factors) {
46  allKeys.insert(factor->begin(), factor->end());
47  }
48 
49  // Check keys
50  for(Key key: keys) {
51  if(allKeys.find(key) == allKeys.end())
52  throw std::runtime_error("Requested to eliminate a key that is not in the factors");
53  }
54 
55  // Sort frontal keys
56  KeySet frontals(keys);
57  const size_t nFrontals = keys.size();
58 
59  // Build a key vector with the frontals followed by the separator
60  KeyVector orderedKeys(allKeys.size());
61  std::copy(keys.begin(), keys.end(), orderedKeys.begin());
62  std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals);
63 
64  // Return resulting conditional and factor
65  return std::make_pair(
66  SymbolicConditional::FromKeysShared(orderedKeys, nFrontals),
67  SymbolicFactor::FromIteratorsShared(orderedKeys.begin() + nFrontals, orderedKeys.end()));
68  }
69  }
70 }
timing.h
Timing utilities.
gtsam::Ordering
Definition: Ordering.h:34
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::EliminateSymbolic
std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< SymbolicFactor > > EliminateSymbolic(const SymbolicFactorGraph &factors, const Ordering &keys)
Dense elimination function for symbolic factors.
Definition: SymbolicFactor.cpp:31
SymbolicFactor.h
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Factor.h
The base class for all factors.
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition: FactorGraph.h:94
Key.h
gtsam::FastSet< Key >
gtsam::SymbolicFactor::FromIteratorsShared
static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, KEYITERATOR endKey)
Constructor from a collection of keys.
Definition: SymbolicFactor.h:108
SymbolicConditional.h
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::SymbolicConditional::FromKeysShared
static SymbolicConditional::shared_ptr FromKeysShared(const CONTAINER &keys, size_t nrFrontals)
Named constructor from an arbitrary number of keys and frontals.
Definition: SymbolicConditional.h:94