Loading...
Searching...
No Matches
MixtureFactor.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Symbol.h>
#include <algorithm>
#include <boost/format.hpp>
#include <cmath>
#include <limits>
#include <vector>
namespace gtsam {
class MixtureFactor : public HybridFactor {
public:
using Base = HybridFactor;
using This = MixtureFactor;
using shared_ptr = boost::shared_ptr<MixtureFactor>;
using sharedFactor = boost::shared_ptr<NonlinearFactor>;
using Factors = DecisionTree<Key, sharedFactor>;
private:
Factors factors_;
bool normalized_;
public:
MixtureFactor() = default;
MixtureFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
const Factors& factors, bool normalized = false)
: Base(keys, discreteKeys), factors_(factors), normalized_(normalized) {}
template <typename FACTOR>
MixtureFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
const std::vector<boost::shared_ptr<FACTOR>>& factors,
bool normalized = false)
: Base(keys, discreteKeys), normalized_(normalized) {
std::vector<NonlinearFactor::shared_ptr> nonlinear_factors;
KeySet continuous_keys_set(keys.begin(), keys.end());
KeySet factor_keys_set;
for (auto&& f : factors) {
// Insert all factor continuous keys in the continuous keys set.
std::copy(f->keys().begin(), f->keys().end(),
std::inserter(factor_keys_set, factor_keys_set.end()));
if (auto nf = boost::dynamic_pointer_cast<NonlinearFactor>(f)) {
nonlinear_factors.push_back(nf);
} else {
throw std::runtime_error(
"Factors passed into MixtureFactor need to be nonlinear!");
}
}
factors_ = Factors(discreteKeys, nonlinear_factors);
if (continuous_keys_set != factor_keys_set) {
throw std::runtime_error(
"The specified continuous keys and the keys in the factors don't "
"match!");
}
}
~MixtureFactor() = default;
AlgebraicDecisionTree<Key> error(const Values& continuousValues) const {
// functor to convert from sharedFactor to double error value.
auto errorFunc = [continuousValues](const sharedFactor& factor) {
return factor->error(continuousValues);
};
DecisionTree<Key, double> errorTree(factors_, errorFunc);
return errorTree;
}
double error(const Values& continuousValues,
const DiscreteValues& discreteValues) const {
// Retrieve the factor corresponding to the assignment in discreteValues.
auto factor = factors_(discreteValues);
// Compute the error for the selected factor
const double factorError = factor->error(continuousValues);
if (normalized_) return factorError;
return factorError + this->nonlinearFactorLogNormalizingConstant(
factor, continuousValues);
}
double error(const HybridValues& values) const override {
return error(values.nonlinear(), values.discrete());
}
size_t dim() const {
const auto assignments = DiscreteValues::CartesianProduct(discreteKeys_);
auto factor = factors_(assignments.at(0));
return factor->dim();
}
void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << (s.empty() ? "" : s + " ");
Base::print("", keyFormatter);
std::cout << "\nMixtureFactor\n";
auto valueFormatter = [](const sharedFactor& v) {
if (v) {
return (boost::format("Nonlinear factor on %d keys") % v->size()).str();
} else {
return std::string("nullptr");
}
};
factors_.print("", keyFormatter, valueFormatter);
}
bool equals(const HybridFactor& other, double tol = 1e-9) const override {
// We attempt a dynamic cast from HybridFactor to MixtureFactor. If it
// fails, return false.
if (!dynamic_cast<const MixtureFactor*>(&other)) return false;
// If the cast is successful, we'll properly construct a MixtureFactor
// object from `other`
const MixtureFactor& f(static_cast<const MixtureFactor&>(other));
// Ensure that this MixtureFactor and `f` have the same `factors_`.
auto compare = [tol](const sharedFactor& a, const sharedFactor& b) {
return traits<NonlinearFactor>::Equals(*a, *b, tol);
};
if (!factors_.equals(f.factors_, compare)) return false;
// If everything above passes, and the keys_, discreteKeys_ and normalized_
// member variables are identical, return true.
return (std::equal(keys_.begin(), keys_.end(), f.keys().begin()) &&
(discreteKeys_ == f.discreteKeys_) &&
(normalized_ == f.normalized_));
}
GaussianFactor::shared_ptr linearize(
const Values& continuousValues,
const DiscreteValues& discreteValues) const {
auto factor = factors_(discreteValues);
return factor->linearize(continuousValues);
}
boost::shared_ptr<GaussianMixtureFactor> linearize(
const Values& continuousValues) const {
// functional to linearize each factor in the decision tree
auto linearizeDT = [continuousValues](const sharedFactor& factor) {
return factor->linearize(continuousValues);
};
DecisionTree<Key, GaussianFactor::shared_ptr> linearized_factors(
factors_, linearizeDT);
return boost::make_shared<GaussianMixtureFactor>(
continuousKeys_, discreteKeys_, linearized_factors);
}
double nonlinearFactorLogNormalizingConstant(const sharedFactor& factor,
const Values& values) const {
// Information matrix (inverse covariance matrix) for the factor.
Matrix infoMat;
// If this is a NoiseModelFactor, we'll use its noiseModel to
// otherwise noiseModelFactor will be nullptr
if (auto noiseModelFactor =
boost::dynamic_pointer_cast<NoiseModelFactor>(factor)) {
// If dynamic cast to NoiseModelFactor succeeded, see if the noise model
// is Gaussian
auto noiseModel = noiseModelFactor->noiseModel();
auto gaussianNoiseModel =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(noiseModel);
if (gaussianNoiseModel) {
// If the noise model is Gaussian, retrieve the information matrix
infoMat = gaussianNoiseModel->information();
} else {
// If the factor is not a Gaussian factor, we'll linearize it to get
// something with a normalized noise model
// TODO(kevin): does this make sense to do? I think maybe not in
// general? Should we just yell at the user?
auto gaussianFactor = factor->linearize(values);
infoMat = gaussianFactor->information();
}
}
// Compute the (negative) log of the normalizing constant
return -(factor->dim() * log(2.0 * M_PI) / 2.0) -
(log(infoMat.determinant()) / 2.0);
}
};
} // namespace gtsam
DiscreteValues.h
HybridValues.h
GaussianMixtureFactor.h
A set of GaussianFactors, indexed by a set of discrete keys.
NonlinearFactor.h
Non-linear factor base classes.
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
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
gtsam::noiseModel
All noise models live in the noiseModel namespace.
Definition LossFunctions.cpp:27
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::AlgebraicDecisionTree
An algebraic decision tree fixes the range of a DecisionTree to double.
Definition AlgebraicDecisionTree.h:38
gtsam::DecisionTree
a decision tree is a function from assignments to values.
Definition DecisionTree.h:61
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
gtsam::DiscreteValues::CartesianProduct
static std::vector< DiscreteValues > CartesianProduct(const DiscreteKeys &keys)
Return a vector of DiscreteValues, one for each possible combination of values.
Definition DiscreteValues.h:85
gtsam::HybridFactor::HybridFactor
HybridFactor()=default
Default constructor creates empty factor.
gtsam::HybridFactor::print
void print(const std::string &s="HybridFactor\n", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition HybridFactor.cpp:82
gtsam::HybridFactor::continuousKeys_
KeyVector continuousKeys_
Record continuous keys for book-keeping.
Definition HybridFactor.h:62
gtsam::HybridFactor::discreteKeys
const DiscreteKeys & discreteKeys() const
Return the discrete keys for this factor.
Definition HybridFactor.h:132
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
gtsam::HybridValues::discrete
const DiscreteValues & discrete() const
Return the discrete values.
Definition HybridValues.h:92
gtsam::HybridValues::nonlinear
const Values & nonlinear() const
Return the nonlinear values.
Definition HybridValues.h:95
gtsam::MixtureFactor
Implementation of a discrete conditional mixture factor.
Definition MixtureFactor.h:48
gtsam::MixtureFactor::MixtureFactor
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
gtsam::MixtureFactor::error
double error(const Values &continuousValues, const DiscreteValues &discreteValues) const
Compute error of factor given both continuous and discrete values.
Definition MixtureFactor.h:153
gtsam::MixtureFactor::error
AlgebraicDecisionTree< Key > error(const Values &continuousValues) const
Compute error of the MixtureFactor as a tree.
Definition MixtureFactor.h:137
gtsam::MixtureFactor::MixtureFactor
MixtureFactor(const KeyVector &keys, const DiscreteKeys &discreteKeys, const Factors &factors, bool normalized=false)
Construct from Decision tree.
Definition MixtureFactor.h:78
gtsam::MixtureFactor::equals
bool equals(const HybridFactor &other, double tol=1e-9) const override
Check equality.
Definition MixtureFactor.h:206
gtsam::MixtureFactor::linearize
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
gtsam::MixtureFactor::nonlinearFactorLogNormalizingConstant
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
gtsam::MixtureFactor::error
double error(const HybridValues &values) const override
Compute error of factor given hybrid values.
Definition MixtureFactor.h:170
gtsam::MixtureFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Testable.
Definition MixtureFactor.h:189
gtsam::MixtureFactor::Factors
DecisionTree< Key, sharedFactor > Factors
typedef for DecisionTree which has Keys as node labels and NonlinearFactor as leaf nodes.
Definition MixtureFactor.h:59
gtsam::MixtureFactor::dim
size_t dim() const
Get the dimension of the factor (number of rows on linearization).
Definition MixtureFactor.h:179
gtsam::MixtureFactor::linearize
boost::shared_ptr< GaussianMixtureFactor > linearize(const Values &continuousValues) const
Linearize all the continuous factors to get a GaussianMixtureFactor.
Definition MixtureFactor.h:240
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
Symbol.h
Symbol.h was moved to inference directory, this header was retained for compatibility.