HybridGaussianFactor.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 
21 #pragma once
22 
30 
31 namespace gtsam {
32 
33 class HybridValues;
34 class DiscreteValues;
35 class VectorValues;
36 
38 using GaussianFactorValuePair = std::pair<GaussianFactor::shared_ptr, double>;
39 
60 class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
61  public:
62  using Base = HybridFactor;
64  using shared_ptr = std::shared_ptr<This>;
65 
66  using sharedFactor = std::shared_ptr<GaussianFactor>;
67 
70 
71  private:
74 
75  public:
78 
80  HybridGaussianFactor() = default;
81 
90  HybridGaussianFactor(const DiscreteKey &discreteKey,
91  const std::vector<GaussianFactor::shared_ptr> &factors);
92 
102  HybridGaussianFactor(const DiscreteKey &discreteKey,
103  const std::vector<GaussianFactorValuePair> &factorPairs);
104 
114  HybridGaussianFactor(const DiscreteKeys &discreteKeys,
115  const FactorValuePairs &factorPairs);
116 
120 
121  bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
122 
123  void print(const std::string &s = "", const KeyFormatter &formatter =
124  DefaultKeyFormatter) const override;
125 
129 
131  GaussianFactorValuePair operator()(const DiscreteValues &assignment) const;
132 
140  AlgebraicDecisionTree<Key> errorTree(
141  const VectorValues &continuousValues) const override;
142 
147  double error(const HybridValues &values) const override;
148 
150  const FactorValuePairs &factors() const { return factors_; }
157  virtual HybridGaussianProductFactor asProductFactor() const;
158 
160 
161  private:
170  static FactorValuePairs augment(const FactorValuePairs &factors);
171 
173  struct ConstructorHelper;
174 
175  // Private constructor using ConstructorHelper above.
176  HybridGaussianFactor(const ConstructorHelper &helper);
177 
178 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
179 
180  friend class boost::serialization::access;
181  template <class ARCHIVE>
182  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
183  ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
184  ar &BOOST_SERIALIZATION_NVP(factors_);
185  }
186 #endif
187 };
188 
189 // traits
190 template <>
191 struct traits<HybridGaussianFactor> : public Testable<HybridGaussianFactor> {};
192 
193 } // namespace gtsam
VectorValues
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::HybridValues
Definition: HybridValues.h:37
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
gtsam::HybridFactor
Definition: HybridFactor.h:51
gtsam::HybridFactor::shared_ptr
std::shared_ptr< HybridFactor > shared_ptr
shared_ptr to this class
Definition: HybridFactor.h:70
formatter
const KeyFormatter & formatter
Definition: treeTraversal-inst.h:204
gtsam::HybridGaussianProductFactor
Alias for DecisionTree of GaussianFactorGraphs and their scalar sums.
Definition: HybridGaussianProductFactor.h:34
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
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
HybridValues
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::AlgebraicDecisionTree< Key >
AlgebraicDecisionTree.h
Algebraic Decision Trees.
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
HybridFactor.h
gtsam::GaussianFactorValuePair
std::pair< GaussianFactor::shared_ptr, double > GaussianFactorValuePair
Alias for pair of GaussianFactor::shared_pointer and a double value.
Definition: HybridGaussianFactor.h:38
gtsam::VectorValues
Definition: VectorValues.h:74
operator()
internal::enable_if< internal::valid_indexed_view_overload< RowIndices, ColIndices >::value &&internal::traits< typename EIGEN_INDEXED_VIEW_METHOD_TYPE< RowIndices, ColIndices >::type >::ReturnAsIndexedView, typename EIGEN_INDEXED_VIEW_METHOD_TYPE< RowIndices, ColIndices >::type >::type operator()(const RowIndices &rowIndices, const ColIndices &colIndices) EIGEN_INDEXED_VIEW_METHOD_CONST
Definition: IndexedViewMethods.h:73
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
HybridGaussianProductFactor.h
DecisionTree.h
Decision Tree for use in DiscreteFactors.
DiscreteValues
gtsam::HybridGaussianFactor::factors
const FactorValuePairs & factors() const
Getter for GaussianFactor decision tree.
Definition: HybridGaussianFactor.h:150
gtsam::equals
Definition: Testable.h:112
DiscreteKey.h
specialized key for discrete variables
gtsam::DecisionTree< Key, GaussianFactorValuePair >
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::HybridGaussianFactor::factors_
FactorValuePairs factors_
Decision tree of Gaussian factors indexed by discrete keys.
Definition: HybridGaussianFactor.h:73
error
static double error
Definition: testRot3.cpp:37
gtsam::traits
Definition: Group.h:36
gtsam::HybridGaussianFactor::sharedFactor
std::shared_ptr< GaussianFactor > sharedFactor
Definition: HybridGaussianFactor.h:66
gtsam::DiscreteValues
Definition: DiscreteValues.h:34
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Definition: DiscreteKey.h:38
gtsam::HybridGaussianFactor
Implementation of a discrete-conditioned hybrid factor. Implements a joint discrete-continuous factor...
Definition: HybridGaussianFactor.h:60
gtsam::tol
const G double tol
Definition: Group.h:79
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
Base
Definition: test_virtual_functions.cpp:156


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