HybridFactor.cpp
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 
19 
20 namespace gtsam {
21 
22 /* ************************************************************************ */
23 KeyVector CollectKeys(const KeyVector &continuousKeys,
24  const DiscreteKeys &discreteKeys) {
25  KeyVector allKeys;
26  std::copy(continuousKeys.begin(), continuousKeys.end(),
27  std::back_inserter(allKeys));
28  std::transform(discreteKeys.begin(), discreteKeys.end(),
29  std::back_inserter(allKeys),
30  [](const DiscreteKey &k) { return k.first; });
31  return allKeys;
32 }
33 
34 /* ************************************************************************ */
35 KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2) {
36  KeyVector allKeys;
37  std::copy(keys1.begin(), keys1.end(), std::back_inserter(allKeys));
38  std::copy(keys2.begin(), keys2.end(), std::back_inserter(allKeys));
39  return allKeys;
40 }
41 
42 /* ************************************************************************ */
44  const DiscreteKeys &key2) {
45  DiscreteKeys allKeys;
46  std::copy(key1.begin(), key1.end(), std::back_inserter(allKeys));
47  std::copy(key2.begin(), key2.end(), std::back_inserter(allKeys));
48  return allKeys;
49 }
50 
51 /* ************************************************************************ */
53  : Base(keys), category_(Category::Continuous), continuousKeys_(keys) {}
54 
55 /* ************************************************************************ */
57  const DiscreteKeys &discreteKeys) {
58  if ((continuousKeys.size() == 0) && (discreteKeys.size() != 0)) {
60  } else if ((continuousKeys.size() != 0) && (discreteKeys.size() == 0)) {
62  } else if ((continuousKeys.size() != 0) && (discreteKeys.size() != 0)) {
64  } else {
65  // Case where we have no keys. Should never happen.
67  }
68 }
69 
70 /* ************************************************************************ */
71 HybridFactor::HybridFactor(const KeyVector &continuousKeys,
72  const DiscreteKeys &discreteKeys)
73  : Base(CollectKeys(continuousKeys, discreteKeys)),
74  category_(GetCategory(continuousKeys, discreteKeys)),
75  discreteKeys_(discreteKeys),
76  continuousKeys_(continuousKeys) {}
77 
78 /* ************************************************************************ */
80  : Base(CollectKeys({}, discreteKeys)),
81  category_(Category::Discrete),
82  discreteKeys_(discreteKeys),
83  continuousKeys_({}) {}
84 
85 /* ************************************************************************ */
86 bool HybridFactor::equals(const HybridFactor &lf, double tol) const {
87  const This *e = dynamic_cast<const This *>(&lf);
88  return e != nullptr && Base::equals(*e, tol) && category_ == e->category_ &&
89  continuousKeys_ == e->continuousKeys_ &&
90  discreteKeys_ == e->discreteKeys_;
91 }
92 
93 /* ************************************************************************ */
94 void HybridFactor::print(const std::string &s,
95  const KeyFormatter &formatter) const {
96  std::cout << (s.empty() ? "" : s + "\n");
97  switch (category_) {
99  std::cout << "Continuous ";
100  break;
101  case Category::Discrete:
102  std::cout << "Discrete ";
103  break;
104  case Category::Hybrid:
105  std::cout << "Hybrid ";
106  break;
107  case Category::None:
108  std::cout << "None ";
109  break;
110  }
111 
112  std::cout << "[";
113  for (size_t c = 0; c < continuousKeys_.size(); c++) {
114  std::cout << formatter(continuousKeys_.at(c));
115  if (c < continuousKeys_.size() - 1) {
116  std::cout << " ";
117  } else {
118  std::cout << (discreteKeys_.size() > 0 ? "; " : "");
119  }
120  }
121  for (size_t d = 0; d < discreteKeys_.size(); d++) {
122  std::cout << formatter(discreteKeys_.at(d).first);
123  if (d < discreteKeys_.size() - 1) {
124  std::cout << " ";
125  }
126  }
127  std::cout << "]";
128 }
129 
130 } // namespace gtsam
key1
const Symbol key1('v', 1)
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
gtsam::HybridFactor::equals
virtual bool equals(const HybridFactor &lf, double tol=1e-9) const
equals
Definition: HybridFactor.cpp:86
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::HybridFactor::category_
Category category_
Record what category of HybridFactor this is.
Definition: HybridFactor.h:58
gtsam::HybridFactor::Category::Discrete
@ Discrete
gtsam::HybridFactor
Definition: HybridFactor.h:51
formatter
const KeyFormatter & formatter
Definition: treeTraversal-inst.h:204
gtsam::HybridFactor::continuousKeys_
KeyVector continuousKeys_
Record continuous keys for book-keeping.
Definition: HybridFactor.h:64
gtsam::Factor
Definition: Factor.h:70
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition: DiscreteKey.h:41
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
HybridFactor.h
gtsam::HybridFactor::Category
Category
Enum to help with categorizing hybrid factors.
Definition: HybridFactor.h:54
key2
const Symbol key2('v', 2)
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
gtsam::CollectKeys
KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys)
Definition: HybridFactor.cpp:23
gtsam::HybridFactor::discreteKeys_
DiscreteKeys discreteKeys_
Definition: HybridFactor.h:62
transform
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
gtsam::HybridFactor::Category::None
@ None
gtsam::HybridFactor::print
void print(const std::string &s="HybridFactor\n", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition: HybridFactor.cpp:94
gtsam::GetCategory
HybridFactor::Category GetCategory(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys)
Definition: HybridFactor.cpp:56
gtsam::HybridFactor::HybridFactor
HybridFactor()=default
gtsam
traits
Definition: SFMdata.h:40
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Definition: DiscreteKey.h:38
gtsam::HybridFactor::Category::Hybrid
@ Hybrid
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::HybridFactor::Category::Continuous
@ Continuous
gtsam::CollectDiscreteKeys
DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &key2)
Definition: HybridFactor.cpp:43


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:43