30#include <boost/format.hpp>
52 using shared_ptr = boost::shared_ptr<MixtureFactor>;
53 using sharedFactor = boost::shared_ptr<NonlinearFactor>;
79 const Factors& factors,
bool normalized =
false)
98 template <
typename FACTOR>
100 const std::vector<boost::shared_ptr<FACTOR>>& factors,
101 bool normalized =
false)
103 std::vector<NonlinearFactor::shared_ptr> nonlinear_factors;
106 for (
auto&& f : factors) {
108 std::copy(f->keys().begin(), f->keys().end(),
109 std::inserter(factor_keys_set, factor_keys_set.end()));
111 if (
auto nf = boost::dynamic_pointer_cast<NonlinearFactor>(f)) {
112 nonlinear_factors.push_back(nf);
114 throw std::runtime_error(
115 "Factors passed into MixtureFactor need to be nonlinear!");
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 "
139 auto errorFunc = [continuousValues](
const sharedFactor& factor) {
140 return factor->error(continuousValues);
156 auto factor = factors_(discreteValues);
158 const double factorError = factor->error(continuousValues);
159 if (normalized_)
return factorError;
161 factor, continuousValues);
181 auto factor = factors_(assignments.at(0));
182 return factor->dim();
190 const std::string& s =
"",
191 const KeyFormatter& keyFormatter = DefaultKeyFormatter)
const override {
192 std::cout << (s.empty() ?
"" : s +
" ");
194 std::cout <<
"\nMixtureFactor\n";
195 auto valueFormatter = [](
const sharedFactor& v) {
197 return (boost::format(
"Nonlinear factor on %d keys") % v->size()).str();
199 return std::string(
"nullptr");
202 factors_.
print(
"", keyFormatter, valueFormatter);
209 if (!
dynamic_cast<const MixtureFactor*
>(&other))
return false;
216 auto compare = [tol](
const sharedFactor& a,
const sharedFactor& b) {
219 if (!factors_.equals(f.factors_, compare))
return false;
223 return (std::equal(
keys_.begin(),
keys_.end(), f.
keys().begin()) &&
224 (discreteKeys_ == f.discreteKeys_) &&
225 (normalized_ == f.normalized_));
233 const Values& continuousValues,
235 auto factor = factors_(discreteValues);
236 return factor->linearize(continuousValues);
241 const Values& continuousValues)
const {
243 auto linearizeDT = [continuousValues](
const sharedFactor& factor) {
244 return factor->linearize(continuousValues);
248 factors_, linearizeDT);
250 return boost::make_shared<GaussianMixtureFactor>(
262 const Values& values)
const {
268 if (
auto noiseModelFactor =
269 boost::dynamic_pointer_cast<NoiseModelFactor>(factor)) {
272 auto noiseModel = noiseModelFactor->noiseModel();
274 auto gaussianNoiseModel =
275 boost::dynamic_pointer_cast<noiseModel::Gaussian>(noiseModel);
276 if (gaussianNoiseModel) {
278 infoMat = gaussianNoiseModel->information();
284 auto gaussianFactor = factor->linearize(values);
285 infoMat = gaussianFactor->information();
290 return -(factor->dim() * log(2.0 * M_PI) / 2.0) -
291 (log(infoMat.determinant()) / 2.0);
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
An algebraic decision tree fixes the range of a DecisionTree to double.
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.