HybridGaussianFactor.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 
21 #include <gtsam/base/types.h>
22 #include <gtsam/base/utilities.h>
31 
32 namespace gtsam {
33 
34 /* *******************************************************************************/
36  KeyVector continuousKeys; // Continuous keys extracted from factors
37  DiscreteKeys discreteKeys; // Discrete keys provided to the constructors
38  FactorValuePairs pairs; // The decision tree with factors and scalars
39 
41  ConstructorHelper(const DiscreteKey& discreteKey,
42  const std::vector<GaussianFactor::shared_ptr>& factors)
43  : discreteKeys({discreteKey}) {
44  // Extract continuous keys from the first non-null factor
45  for (const auto& factor : factors) {
46  if (factor && continuousKeys.empty()) {
47  continuousKeys = factor->keys();
48  break;
49  }
50  }
51  // Build the FactorValuePairs DecisionTree
53  DecisionTree<Key, GaussianFactor::shared_ptr>(discreteKeys, factors),
54  [](const sharedFactor& f) {
55  return std::pair{f,
56  f ? 0.0 : std::numeric_limits<double>::infinity()};
57  });
58  }
59 
62  ConstructorHelper(const DiscreteKey& discreteKey,
63  const std::vector<GaussianFactorValuePair>& factorPairs)
64  : discreteKeys({discreteKey}) {
65  // Extract continuous keys from the first non-null factor
66  for (const GaussianFactorValuePair& pair : factorPairs) {
67  if (pair.first && continuousKeys.empty()) {
68  continuousKeys = pair.first->keys();
69  break;
70  }
71  }
72 
73  // Build the FactorValuePairs DecisionTree
74  pairs = FactorValuePairs(discreteKeys, factorPairs);
75  }
76 
80  const FactorValuePairs& factorPairs)
82  // Extract continuous keys from the first non-null factor
83  // TODO: just stop after first non-null factor
84  factorPairs.visit([&](const GaussianFactorValuePair& pair) {
85  if (pair.first && continuousKeys.empty()) {
86  continuousKeys = pair.first->keys();
87  }
88  });
89 
90  // Build the FactorValuePairs DecisionTree
91  pairs = factorPairs;
92  }
93 };
94 
95 /* *******************************************************************************/
97  : Base(helper.continuousKeys, helper.discreteKeys),
98  factors_(helper.pairs) {}
99 
101  const DiscreteKey& discreteKey,
102  const std::vector<GaussianFactor::shared_ptr>& factors)
104 
106  const DiscreteKey& discreteKey,
107  const std::vector<GaussianFactorValuePair>& factorPairs)
108  : HybridGaussianFactor(ConstructorHelper(discreteKey, factorPairs)) {}
109 
111  const FactorValuePairs& factors)
112  : HybridGaussianFactor(ConstructorHelper(discreteKeys, factors)) {}
113 
114 /* *******************************************************************************/
115 bool HybridGaussianFactor::equals(const HybridFactor& lf, double tol) const {
116  const This* e = dynamic_cast<const This*>(&lf);
117  if (e == nullptr) return false;
118 
119  // This will return false if either factors_ is empty or e->factors_ is
120  // empty, but not if both are empty or both are not empty:
121  if (factors_.empty() ^ e->factors_.empty()) return false;
122 
123  // Check the base and the factors:
124  auto compareFunc = [tol](const GaussianFactorValuePair& pair1,
125  const GaussianFactorValuePair& pair2) {
126  auto f1 = pair1.first, f2 = pair2.first;
127  bool match = (!f1 && !f2) || (f1 && f2 && f1->equals(*f2, tol));
128  return match && gtsam::equal(pair1.second, pair2.second, tol);
129  };
130  return Base::equals(*e, tol) && factors_.equals(e->factors_, compareFunc);
131 }
132 
133 /* *******************************************************************************/
134 void HybridGaussianFactor::print(const std::string& s,
135  const KeyFormatter& formatter) const {
136  std::cout << (s.empty() ? "" : s + "\n");
138  std::cout << "{\n";
139  if (factors_.empty()) {
140  std::cout << " empty" << std::endl;
141  } else {
142  factors_.print(
143  "", [&](Key k) { return formatter(k); },
144  [&](const GaussianFactorValuePair& pair) -> std::string {
145  RedirectCout rd;
146  std::cout << ":\n";
147  if (pair.first) {
148  pair.first->print("", formatter);
149  std::cout << "scalar: " << pair.second << "\n";
150  return rd.str();
151  } else {
152  return "nullptr";
153  }
154  });
155  }
156  std::cout << "}" << std::endl;
157 }
158 
159 /* *******************************************************************************/
161  const DiscreteValues& assignment) const {
162  return factors_(assignment);
163 }
164 
165 /* *******************************************************************************/
167  // Implemented by creating a new DecisionTree where:
168  // - The structure (keys and assignments) is preserved from factors_
169  // - Each leaf converted to a GaussianFactorGraph with just the factor and its
170  // scalar.
171  return {{factors_,
172  [](const GaussianFactorValuePair& pair)
173  -> std::pair<GaussianFactorGraph, double> {
174  return {GaussianFactorGraph{pair.first}, pair.second};
175  }}};
176 }
177 
178 /* *******************************************************************************/
179 inline static double PotentiallyPrunedComponentError(
180  const GaussianFactorValuePair& pair, const VectorValues& continuousValues) {
181  return pair.first ? pair.first->error(continuousValues) + pair.second
182  : std::numeric_limits<double>::infinity();
183 }
184 
185 /* *******************************************************************************/
187  const VectorValues& continuousValues) const {
188  // functor to convert from sharedFactor to double error value.
189  auto errorFunc = [&continuousValues](const GaussianFactorValuePair& pair) {
190  return PotentiallyPrunedComponentError(pair, continuousValues);
191  };
192  return {factors_, errorFunc};
193 }
194 
195 /* *******************************************************************************/
197  // Directly index to get the component, no need to build the whole tree.
198  const GaussianFactorValuePair pair = factors_(values.discrete());
199  return PotentiallyPrunedComponentError(pair, values.continuous());
200 }
201 
202 } // namespace gtsam
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::HybridValues
Definition: HybridValues.h:37
test_constructor::f1
auto f1
Definition: testHybridNonlinearFactor.cpp:56
gtsam::HybridGaussianFactor::ConstructorHelper::continuousKeys
KeyVector continuousKeys
Definition: HybridGaussianFactor.cpp:36
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::HybridGaussianFactor::errorTree
AlgebraicDecisionTree< Key > errorTree(const VectorValues &continuousValues) const override
Compute error of the HybridGaussianFactor as a tree.
Definition: HybridGaussianFactor.cpp:186
gtsam::HybridGaussianFactor::operator()
GaussianFactorValuePair operator()(const DiscreteValues &assignment) const
Get factor at a given discrete assignment.
Definition: HybridGaussianFactor.cpp:160
types.h
Typedefs for easier changing of types.
gtsam::HybridFactor::equals
virtual bool equals(const HybridFactor &lf, double tol=1e-9) const
equals
Definition: HybridFactor.cpp:86
gtsam::HybridGaussianFactor::HybridGaussianFactor
HybridGaussianFactor()=default
Default constructor, mainly for serialization.
gtsam::HybridGaussianFactor::ConstructorHelper::ConstructorHelper
ConstructorHelper(const DiscreteKey &discreteKey, const std::vector< GaussianFactorValuePair > &factorPairs)
Definition: HybridGaussianFactor.cpp:62
gtsam::DecisionTree::empty
bool empty() const
Check if tree is empty.
Definition: DecisionTree.h:290
gtsam::DecisionTree::equals
bool equals(const DecisionTree &other, const CompareFunc &compare=&DefaultCompare) const
Definition: DecisionTree-inl.h:972
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
gtsam::RedirectCout
Definition: base/utilities.h:16
gtsam::HybridFactor
Definition: HybridFactor.h:51
gtsam::HybridGaussianFactor::ConstructorHelper::pairs
FactorValuePairs pairs
Definition: HybridGaussianFactor.cpp:38
formatter
const KeyFormatter & formatter
Definition: treeTraversal-inst.h:204
f2
double f2(const Vector2 &x)
Definition: testNumericalDerivative.cpp:56
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
gtsam::DecisionTree::print
void print(const std::string &s, const LabelFormatter &labelFormatter, const ValueFormatter &valueFormatter) const
GTSAM-style print.
Definition: DecisionTree-inl.h:978
gtsam::HybridGaussianFactor::print
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition: HybridGaussianFactor.cpp:134
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
HybridGaussianFactor.h
A set of GaussianFactors, indexed by a set of discrete keys.
utilities.h
gtsam::AlgebraicDecisionTree< Key >
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
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
match
bool match(const T &xpr, std::string ref, std::string str_xpr="")
Definition: indexed_view.cpp:54
gtsam::VectorValues
Definition: VectorValues.h:74
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.
gtsam::DecisionTree::visit
void visit(Func f) const
Visit all leaves in depth-first fashion.
Definition: DecisionTree-inl.h:842
gtsam::HybridGaussianFactor::factors
const FactorValuePairs & factors() const
Getter for GaussianFactor decision tree.
Definition: HybridGaussianFactor.h:150
gtsam::HybridGaussianFactor::equals
bool equals(const HybridFactor &lf, double tol=1e-9) const override
equals
Definition: HybridGaussianFactor.cpp:115
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::DecisionTree< Key, GaussianFactorValuePair >
gtsam::HybridGaussianFactor::ConstructorHelper::ConstructorHelper
ConstructorHelper(const DiscreteKeys &discreteKeys, const FactorValuePairs &factorPairs)
Definition: HybridGaussianFactor.cpp:79
gtsam::HybridFactor::print
void print(const std::string &s="HybridFactor\n", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition: HybridFactor.cpp:94
gtsam
traits
Definition: SFMdata.h:40
gtsam::HybridGaussianFactor::asProductFactor
virtual HybridGaussianProductFactor asProductFactor() const
Helper function to return factors and functional to create a DecisionTree of Gaussian Factor Graphs.
Definition: HybridGaussianFactor.cpp:166
gtsam::HybridGaussianFactor::factors_
FactorValuePairs factors_
Decision tree of Gaussian factors indexed by discrete keys.
Definition: HybridGaussianFactor.h:73
gtsam::HybridGaussianFactor::FactorValuePairs
DecisionTree< Key, GaussianFactorValuePair > FactorValuePairs
typedef for Decision Tree of Gaussian factors and arbitrary value.
Definition: HybridGaussianFactor.h:69
DecisionTree-inl.h
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::PotentiallyPrunedComponentError
static double PotentiallyPrunedComponentError(const GaussianFactorValuePair &pair, const VectorValues &continuousValues)
Definition: HybridGaussianFactor.cpp:179
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::HybridGaussianFactor::error
double error(const HybridValues &values) const override
Compute the log-likelihood, including the log-normalizing constant.
Definition: HybridGaussianFactor.cpp:196
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
gtsam::HybridGaussianFactor::ConstructorHelper::discreteKeys
DiscreteKeys discreteKeys
Definition: HybridGaussianFactor.cpp:37
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
HybridValues.h
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Definition: Testable.h:85
gtsam::RedirectCout::str
std::string str() const
return the string
Definition: utilities.cpp:5
gtsam::HybridGaussianFactor::ConstructorHelper::ConstructorHelper
ConstructorHelper(const DiscreteKey &discreteKey, const std::vector< GaussianFactor::shared_ptr > &factors)
Constructor for a single discrete key and a vector of Gaussian factors.
Definition: HybridGaussianFactor.cpp:41
gtsam::HybridGaussianFactor::ConstructorHelper
Definition: HybridGaussianFactor.cpp:35


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