HybridFactor.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 
20 #include <gtsam/base/Testable.h>
23 #include <gtsam/inference/Factor.h>
25 #include <gtsam/nonlinear/Values.h>
26 
27 #include <cstddef>
28 #include <string>
29 namespace gtsam {
30 
31 class HybridValues;
32 
35 
36 KeyVector CollectKeys(const KeyVector &continuousKeys,
37  const DiscreteKeys &discreteKeys);
38 KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2);
40  const DiscreteKeys &key2);
41 
52 class GTSAM_EXPORT HybridFactor : public Factor {
53  private:
54  bool isDiscrete_ = false;
55  bool isContinuous_ = false;
56  bool isHybrid_ = false;
57 
58  protected:
59  // Set of DiscreteKeys for this factor.
63 
64  public:
65  // typedefs needed to play nice with gtsam
66  typedef HybridFactor This;
67  typedef std::shared_ptr<HybridFactor>
69  typedef Factor Base;
70 
73 
75  HybridFactor() = default;
76 
82  explicit HybridFactor(const KeyVector &keys);
83 
89  explicit HybridFactor(const DiscreteKeys &discreteKeys);
90 
97  HybridFactor(const KeyVector &continuousKeys,
98  const DiscreteKeys &discreteKeys);
99 
103 
105  virtual bool equals(const HybridFactor &lf, double tol = 1e-9) const;
106 
108  void print(
109  const std::string &s = "HybridFactor\n",
110  const KeyFormatter &formatter = DefaultKeyFormatter) const override;
111 
115 
117  bool isDiscrete() const { return isDiscrete_; }
118 
120  bool isContinuous() const { return isContinuous_; }
121 
123  bool isHybrid() const { return isHybrid_; }
124 
126  size_t nrContinuous() const { return continuousKeys_.size(); }
127 
129  const DiscreteKeys &discreteKeys() const { return discreteKeys_; }
130 
132  const KeyVector &continuousKeys() const { return continuousKeys_; }
133 
135 
136  private:
137 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
138 
139  friend class boost::serialization::access;
140  template <class ARCHIVE>
141  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
142  ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
143  ar &BOOST_SERIALIZATION_NVP(isDiscrete_);
144  ar &BOOST_SERIALIZATION_NVP(isContinuous_);
145  ar &BOOST_SERIALIZATION_NVP(isHybrid_);
146  ar &BOOST_SERIALIZATION_NVP(discreteKeys_);
147  ar &BOOST_SERIALIZATION_NVP(continuousKeys_);
148  }
149 #endif
150 };
151 // HybridFactor
152 
153 // traits
154 template <>
155 struct traits<HybridFactor> : public Testable<HybridFactor> {};
156 
157 } // namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
Decision Tree for use in DiscreteFactors.
Concept check for values that can be used in unit tests.
A non-templated config holding any types of Manifold-group elements.
std::string serialize(const T &input)
serializes to a string
const DiscreteKeys & discreteKeys() const
Return the discrete keys for this factor.
Definition: HybridFactor.h:129
const KeyVector & continuousKeys() const
Return only the continuous keys for this factor.
Definition: HybridFactor.h:132
HybridFactor This
This class.
Definition: HybridFactor.h:66
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
Factor Base
Our base class.
Definition: HybridFactor.h:69
const KeyFormatter & formatter
std::shared_ptr< HybridFactor > shared_ptr
shared_ptr to this class
Definition: HybridFactor.h:68
size_t nrContinuous() const
Return the number of continuous variables in this factor.
Definition: HybridFactor.h:126
const Symbol key1('v', 1)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
Linear Factor Graph where all factors are Gaussians.
bool isContinuous() const
True if this is a factor of continuous variables only.
Definition: HybridFactor.h:120
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
KeyVector continuousKeys_
Record continuous keys for book-keeping.
Definition: HybridFactor.h:62
traits
Definition: chartTesting.h:28
specialized key for discrete variables
bool isHybrid() const
True is this is a Discrete-Continuous factor.
Definition: HybridFactor.h:123
const G double tol
Definition: Group.h:86
const KeyVector keys
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
DiscreteKeys discreteKeys_
Definition: HybridFactor.h:60
bool isDiscrete() const
True if this is a factor of discrete variables only.
Definition: HybridFactor.h:117
DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &key2)
KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys)
const Symbol key2('v', 2)
The base class for all factors.
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition: DiscreteKey.h:41


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:20