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 <memory>
27 
28 #include <utility>
29 
30 namespace gtsam
31 {
32  namespace internal
33  {
36  template<class FACTOR>
37  std::pair<std::shared_ptr<SymbolicConditional>, std::shared_ptr<SymbolicFactor> >
39  {
41 
42  // Gather all keys
43  KeySet allKeys;
44  for(const std::shared_ptr<FACTOR>& factor: factors) {
45  // Non-active factors are nullptr
46  if (factor)
47  allKeys.insert(factor->begin(), factor->end());
48  }
49 
50  // Check keys
51  for(Key key: keys) {
52  if(allKeys.find(key) == allKeys.end())
53  throw std::runtime_error("Requested to eliminate a key that is not in the factors");
54  }
55 
56  // Sort frontal keys
57  KeySet frontals(keys);
58  const size_t nFrontals = keys.size();
59 
60  // Build a key vector with the frontals followed by the separator
61  KeyVector orderedKeys(allKeys.size());
62  std::copy(keys.begin(), keys.end(), orderedKeys.begin());
63  std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals);
64 
65  // Return resulting conditional and factor
66  return {
67  SymbolicConditional::FromKeysShared(orderedKeys, nFrontals),
68  SymbolicFactor::FromIteratorsShared(orderedKeys.begin() + nFrontals, orderedKeys.end())
69  };
70  }
71  }
72 }
timing.h
Timing utilities.
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
gtsam::SymbolicFactor::FromIteratorsShared
static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, KEYITERATOR endKey)
Definition: SymbolicFactor.h:118
gtsam::FastSet< Key >
SymbolicFactor.h
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
SymbolicConditional.h
Factor.h
The base class for all factors.
gtsam::FactorGraph
Definition: BayesTree.h:34
Key.h
gtsam::internal::EliminateSymbolic
std::pair< std::shared_ptr< SymbolicConditional >, std::shared_ptr< SymbolicFactor > > EliminateSymbolic(const FactorGraph< FACTOR > &factors, const Ordering &keys)
Definition: SymbolicFactor-inst.h:38
key
const gtsam::Symbol key('X', 0)
gtsam::SymbolicConditional::FromKeysShared
static SymbolicConditional::shared_ptr FromKeysShared(const CONTAINER &keys, size_t nrFrontals)
Definition: SymbolicConditional.h:94
gtsam
traits
Definition: SFMdata.h:40
internal
Definition: BandTriangularSolver.h:13
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::Ordering
Definition: inference/Ordering.h:33
gttic
#define gttic(label)
Definition: timing.h:295


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:05:21