gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
MixtureFactor.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
20#pragma once
21
28
29#include <algorithm>
30#include <boost/format.hpp>
31#include <cmath>
32#include <limits>
33#include <vector>
34
35namespace gtsam {
36
49 public:
50 using Base = HybridFactor;
51 using This = MixtureFactor;
52 using shared_ptr = boost::shared_ptr<MixtureFactor>;
53 using sharedFactor = boost::shared_ptr<NonlinearFactor>;
54
60
61 private:
63 Factors factors_;
64 bool normalized_;
65
66 public:
67 MixtureFactor() = default;
68
79 const Factors& factors, bool normalized = false)
80 : Base(keys, discreteKeys), factors_(factors), normalized_(normalized) {}
81
98 template <typename FACTOR>
100 const std::vector<boost::shared_ptr<FACTOR>>& factors,
101 bool normalized = false)
102 : Base(keys, discreteKeys), normalized_(normalized) {
103 std::vector<NonlinearFactor::shared_ptr> nonlinear_factors;
104 KeySet continuous_keys_set(keys.begin(), keys.end());
105 KeySet factor_keys_set;
106 for (auto&& f : factors) {
107 // Insert all factor continuous keys in the continuous keys set.
108 std::copy(f->keys().begin(), f->keys().end(),
109 std::inserter(factor_keys_set, factor_keys_set.end()));
110
111 if (auto nf = boost::dynamic_pointer_cast<NonlinearFactor>(f)) {
112 nonlinear_factors.push_back(nf);
113 } else {
114 throw std::runtime_error(
115 "Factors passed into MixtureFactor need to be nonlinear!");
116 }
117 }
118 factors_ = Factors(discreteKeys, nonlinear_factors);
119
120 if (continuous_keys_set != factor_keys_set) {
121 throw std::runtime_error(
122 "The specified continuous keys and the keys in the factors don't "
123 "match!");
124 }
125 }
126
127 ~MixtureFactor() = default;
128
137 AlgebraicDecisionTree<Key> error(const Values& continuousValues) const {
138 // functor to convert from sharedFactor to double error value.
139 auto errorFunc = [continuousValues](const sharedFactor& factor) {
140 return factor->error(continuousValues);
141 };
142 DecisionTree<Key, double> errorTree(factors_, errorFunc);
143 return errorTree;
144 }
145
153 double error(const Values& continuousValues,
154 const DiscreteValues& discreteValues) const {
155 // Retrieve the factor corresponding to the assignment in discreteValues.
156 auto factor = factors_(discreteValues);
157 // Compute the error for the selected factor
158 const double factorError = factor->error(continuousValues);
159 if (normalized_) return factorError;
160 return factorError + this->nonlinearFactorLogNormalizingConstant(
161 factor, continuousValues);
162 }
163
170 double error(const HybridValues& values) const override {
171 return error(values.nonlinear(), values.discrete());
172 }
173
179 size_t dim() const {
180 const auto assignments = DiscreteValues::CartesianProduct(discreteKeys_);
181 auto factor = factors_(assignments.at(0));
182 return factor->dim();
183 }
184
187
189 void print(
190 const std::string& s = "",
191 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
192 std::cout << (s.empty() ? "" : s + " ");
193 Base::print("", keyFormatter);
194 std::cout << "\nMixtureFactor\n";
195 auto valueFormatter = [](const sharedFactor& v) {
196 if (v) {
197 return (boost::format("Nonlinear factor on %d keys") % v->size()).str();
198 } else {
199 return std::string("nullptr");
200 }
201 };
202 factors_.print("", keyFormatter, valueFormatter);
203 }
204
206 bool equals(const HybridFactor& other, double tol = 1e-9) const override {
207 // We attempt a dynamic cast from HybridFactor to MixtureFactor. If it
208 // fails, return false.
209 if (!dynamic_cast<const MixtureFactor*>(&other)) return false;
210
211 // If the cast is successful, we'll properly construct a MixtureFactor
212 // object from `other`
213 const MixtureFactor& f(static_cast<const MixtureFactor&>(other));
214
215 // Ensure that this MixtureFactor and `f` have the same `factors_`.
216 auto compare = [tol](const sharedFactor& a, const sharedFactor& b) {
217 return traits<NonlinearFactor>::Equals(*a, *b, tol);
218 };
219 if (!factors_.equals(f.factors_, compare)) return false;
220
221 // If everything above passes, and the keys_, discreteKeys_ and normalized_
222 // member variables are identical, return true.
223 return (std::equal(keys_.begin(), keys_.end(), f.keys().begin()) &&
224 (discreteKeys_ == f.discreteKeys_) &&
225 (normalized_ == f.normalized_));
226 }
227
229
233 const Values& continuousValues,
234 const DiscreteValues& discreteValues) const {
235 auto factor = factors_(discreteValues);
236 return factor->linearize(continuousValues);
237 }
238
240 boost::shared_ptr<GaussianMixtureFactor> linearize(
241 const Values& continuousValues) const {
242 // functional to linearize each factor in the decision tree
243 auto linearizeDT = [continuousValues](const sharedFactor& factor) {
244 return factor->linearize(continuousValues);
245 };
246
248 factors_, linearizeDT);
249
250 return boost::make_shared<GaussianMixtureFactor>(
251 continuousKeys_, discreteKeys_, linearized_factors);
252 }
253
261 double nonlinearFactorLogNormalizingConstant(const sharedFactor& factor,
262 const Values& values) const {
263 // Information matrix (inverse covariance matrix) for the factor.
264 Matrix infoMat;
265
266 // If this is a NoiseModelFactor, we'll use its noiseModel to
267 // otherwise noiseModelFactor will be nullptr
268 if (auto noiseModelFactor =
269 boost::dynamic_pointer_cast<NoiseModelFactor>(factor)) {
270 // If dynamic cast to NoiseModelFactor succeeded, see if the noise model
271 // is Gaussian
272 auto noiseModel = noiseModelFactor->noiseModel();
273
274 auto gaussianNoiseModel =
275 boost::dynamic_pointer_cast<noiseModel::Gaussian>(noiseModel);
276 if (gaussianNoiseModel) {
277 // If the noise model is Gaussian, retrieve the information matrix
278 infoMat = gaussianNoiseModel->information();
279 } else {
280 // If the factor is not a Gaussian factor, we'll linearize it to get
281 // something with a normalized noise model
282 // TODO(kevin): does this make sense to do? I think maybe not in
283 // general? Should we just yell at the user?
284 auto gaussianFactor = factor->linearize(values);
285 infoMat = gaussianFactor->information();
286 }
287 }
288
289 // Compute the (negative) log of the normalizing constant
290 return -(factor->dim() * log(2.0 * M_PI) / 2.0) -
291 (log(infoMat.determinant()) / 2.0);
292 }
293};
294
295} // namespace gtsam
A set of GaussianFactors, indexed by a set of discrete keys.
Non-linear factor base classes.
Factor Graph consisting of non-linear factors.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
Algebraic Decision Trees fix the range to double Just has some nice constructors and some syntactic s...
Definition AlgebraicDecisionTree.h:38
void print(const std::string &s, const LabelFormatter &labelFormatter, const ValueFormatter &valueFormatter) const
GTSAM-style print.
Definition DecisionTree-inl.h:872
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
A map from keys to values.
Definition DiscreteValues.h:34
static std::vector< DiscreteValues > CartesianProduct(const DiscreteKeys &keys)
Return a vector of DiscreteValues, one for each possible combination of values.
Definition DiscreteValues.h:85
Base class for truly hybrid probabilistic factors.
Definition HybridFactor.h:52
HybridFactor()=default
Default constructor creates empty factor.
void print(const std::string &s="HybridFactor\n", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition HybridFactor.cpp:82
KeyVector continuousKeys_
Record continuous keys for book-keeping.
Definition HybridFactor.h:62
const DiscreteKeys & discreteKeys() const
Return the discrete keys for this factor.
Definition HybridFactor.h:132
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
const DiscreteValues & discrete() const
Return the discrete values.
Definition HybridValues.h:92
const Values & nonlinear() const
Return the nonlinear values.
Definition HybridValues.h:95
Implementation of a discrete conditional mixture factor.
Definition MixtureFactor.h:48
MixtureFactor(const KeyVector &keys, const DiscreteKeys &discreteKeys, const std::vector< boost::shared_ptr< FACTOR > > &factors, bool normalized=false)
Convenience constructor that generates the underlying factor decision tree for us.
Definition MixtureFactor.h:99
double error(const Values &continuousValues, const DiscreteValues &discreteValues) const
Compute error of factor given both continuous and discrete values.
Definition MixtureFactor.h:153
AlgebraicDecisionTree< Key > error(const Values &continuousValues) const
Compute error of the MixtureFactor as a tree.
Definition MixtureFactor.h:137
MixtureFactor(const KeyVector &keys, const DiscreteKeys &discreteKeys, const Factors &factors, bool normalized=false)
Construct from Decision tree.
Definition MixtureFactor.h:78
bool equals(const HybridFactor &other, double tol=1e-9) const override
Check equality.
Definition MixtureFactor.h:206
GaussianFactor::shared_ptr linearize(const Values &continuousValues, const DiscreteValues &discreteValues) const
Linearize specific nonlinear factors based on the assignment in discreteValues.
Definition MixtureFactor.h:232
double nonlinearFactorLogNormalizingConstant(const sharedFactor &factor, const Values &values) const
If the component factors are not already normalized, we want to compute their normalizing constants s...
Definition MixtureFactor.h:261
double error(const HybridValues &values) const override
Compute error of factor given hybrid values.
Definition MixtureFactor.h:170
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Testable.
Definition MixtureFactor.h:189
DecisionTree< Key, sharedFactor > Factors
typedef for DecisionTree which has Keys as node labels and NonlinearFactor as leaf nodes.
Definition MixtureFactor.h:59
size_t dim() const
Get the dimension of the factor (number of rows on linearization).
Definition MixtureFactor.h:179
boost::shared_ptr< GaussianMixtureFactor > linearize(const Values &continuousValues) const
Linearize all the continuous factors to get a GaussianMixtureFactor.
Definition MixtureFactor.h:240
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
Symbol.h was moved to inference directory, this header was retained for compatibility.