HybridFactorGraph.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2022, 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 
22 
23 namespace gtsam {
24 
25 /* ************************************************************************* */
26 std::set<DiscreteKey> HybridFactorGraph::discreteKeys() const {
27  std::set<DiscreteKey> keys;
28  for (auto& factor : factors_) {
29  if (auto p = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
30  for (const DiscreteKey& key : p->discreteKeys()) {
31  keys.insert(key);
32  }
33  } else if (auto p = std::dynamic_pointer_cast<HybridFactor>(factor)) {
34  for (const DiscreteKey& key : p->discreteKeys()) {
35  keys.insert(key);
36  }
37  }
38  }
39  return keys;
40 }
41 
42 /* ************************************************************************* */
44  KeySet keys;
45  std::set<DiscreteKey> key_set = discreteKeys();
46  std::transform(key_set.begin(), key_set.end(),
47  std::inserter(keys, keys.begin()),
48  [](const DiscreteKey& k) { return k.first; });
49  return keys;
50 }
51 
52 /* ************************************************************************* */
54  KeySet keys;
55  for (auto& factor : factors_) {
56  if (auto p = std::dynamic_pointer_cast<HybridFactor>(factor)) {
57  for (const Key& key : p->continuousKeys()) {
58  keys.insert(key);
59  }
60  } else if (auto p = std::dynamic_pointer_cast<GaussianFactor>(factor)) {
61  keys.insert(p->keys().begin(), p->keys().end());
62  } else if (auto p = std::dynamic_pointer_cast<NonlinearFactor>(factor)) {
63  keys.insert(p->keys().begin(), p->keys().end());
64  }
65  }
66  return keys;
67 }
68 
69 /* ************************************************************************* */
70 
71 } // namespace gtsam
gtsam::HybridFactorGraph::discreteKeys
std::set< DiscreteKey > discreteKeys() const
Get all the discrete keys in the factor graph.
Definition: HybridFactorGraph.cpp:26
gtsam::FactorGraph< Factor >::factors_
FastVector< sharedFactor > factors_
Definition: FactorGraph.h:92
gtsam::HybridFactorGraph::continuousKeySet
const KeySet continuousKeySet() const
Get all the continuous keys in the factor graph.
Definition: HybridFactorGraph.cpp:53
gtsam::FastSet< Key >
gtsam::FactorGraph< Factor >::keys
KeySet keys() const
Definition: FactorGraph-inst.h:85
gtsam::HybridFactorGraph::discreteKeySet
KeySet discreteKeySet() const
Get all the discrete keys in the factor graph, as a set of Keys.
Definition: HybridFactorGraph.cpp:43
transform
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
key
const gtsam::Symbol key('X', 0)
NonlinearFactor.h
Non-linear factor base classes.
HybridFactorGraph.h
Factor graph with utilities for hybrid factors.
gtsam
traits
Definition: SFMdata.h:40
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Definition: DiscreteKey.h:38
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:26