Go to the documentation of this file.
97 template <
typename FACTOR>
99 const std::vector<std::shared_ptr<FACTOR>>&
factors,
100 bool normalized =
false)
102 std::vector<NonlinearFactor::shared_ptr> nonlinear_factors;
107 std::copy(
f->keys().begin(),
f->keys().end(),
108 std::inserter(factor_keys_set, factor_keys_set.end()));
110 if (
auto nf = std::dynamic_pointer_cast<NonlinearFactor>(
f)) {
111 nonlinear_factors.push_back(nf);
113 throw std::runtime_error(
114 "Factors passed into MixtureFactor need to be nonlinear!");
119 if (continuous_keys_set != factor_keys_set) {
120 throw std::runtime_error(
121 "The specified continuous keys and the keys in the factors don't "
136 auto errorFunc = [continuousValues](
const sharedFactor& factor) {
137 return factor->error(continuousValues);
153 auto factor =
factors_(discreteValues);
155 const double factorError = factor->error(continuousValues);
158 factor, continuousValues);
178 auto factor =
factors_(assignments.at(0));
179 return factor->dim();
187 const std::string&
s =
"",
189 std::cout << (
s.empty() ?
"" :
s +
" ");
191 std::cout <<
"\nMixtureFactor\n";
194 return "Nonlinear factor on " + std::to_string(
v->size()) +
" keys";
196 return std::string(
"nullptr");
230 const Values& continuousValues,
232 auto factor =
factors_(discreteValues);
233 return factor->linearize(continuousValues);
238 const Values& continuousValues)
const {
240 auto linearizeDT = [continuousValues](
const sharedFactor& factor) {
241 return factor->linearize(continuousValues);
247 return std::make_shared<GaussianMixtureFactor>(
265 if (
auto noiseModelFactor =
266 std::dynamic_pointer_cast<NoiseModelFactor>(factor)) {
269 auto noiseModel = noiseModelFactor->noiseModel();
271 auto gaussianNoiseModel =
272 std::dynamic_pointer_cast<noiseModel::Gaussian>(noiseModel);
273 if (gaussianNoiseModel) {
275 infoMat = gaussianNoiseModel->information();
281 auto gaussianFactor = factor->linearize(
values);
282 infoMat = gaussianFactor->information();
287 return -(factor->dim() *
log(2.0 *
M_PI) / 2.0) -
288 (
log(infoMat.determinant()) / 2.0);
MixtureFactor(const KeyVector &keys, const DiscreteKeys &discreteKeys, const std::vector< std::shared_ptr< FACTOR >> &factors, bool normalized=false)
Convenience constructor that generates the underlying factor decision tree for us.
MixtureFactor(const KeyVector &keys, const DiscreteKeys &discreteKeys, const Factors &factors, bool normalized=false)
Construct from Decision tree.
Factors factors_
Decision tree of Gaussian factors indexed by discrete keys.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
DecisionTree< Key, sharedFactor > Factors
typedef for DecisionTree which has Keys as node labels and NonlinearFactor as leaf nodes.
bool equals(const DecisionTree &other, const CompareFunc &compare=&DefaultCompare) const
const GaussianFactorGraph factors
std::shared_ptr< HybridFactor > shared_ptr
shared_ptr to this class
Symbol.h was moved to inference directory, this header was retained for compatibility.
AlgebraicDecisionTree< Key > errorTree(const Values &continuousValues) const
Compute error of the MixtureFactor as a tree.
KeyVector continuousKeys_
Record continuous keys for book-keeping.
bool equals(const HybridFactor &other, double tol=1e-9) const override
Check equality.
const EIGEN_DEVICE_FUNC LogReturnType log() const
DiscreteKeys is a set of keys that can be assembled using the & operator.
void print(const std::string &s, const LabelFormatter &labelFormatter, const ValueFormatter &valueFormatter) const
GTSAM-style print.
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
std::shared_ptr< NonlinearFactor > sharedFactor
double nonlinearFactorLogNormalizingConstant(const sharedFactor &factor, const Values &values) const
static std::vector< DiscreteValues > CartesianProduct(const DiscreteKeys &keys)
Return a vector of DiscreteValues, one for each possible combination of values.
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
std::shared_ptr< This > shared_ptr
shared_ptr to this class
DiscreteKeys discreteKeys_
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print to stdout
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
GaussianFactor::shared_ptr linearize(const Values &continuousValues, const DiscreteValues &discreteValues) const
Non-linear factor base classes.
void print(const std::string &s="HybridFactor\n", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
KeyVector keys_
The keys involved in this factor.
Factor Graph consisting of non-linear factors.
const KeyVector & keys() const
Access the factor's involved variable keys.
std::shared_ptr< GaussianMixtureFactor > linearize(const Values &continuousValues) const
Linearize all the continuous factors to get a GaussianMixtureFactor.
Array< int, Dynamic, 1 > v
size_t dim() const
Get the dimension of the factor (number of rows on linearization). Returns the dimension of the first...
Implementation of a discrete conditional mixture factor.
double error(const Values &continuousValues, const DiscreteValues &discreteValues) const
Compute error of factor given both continuous and discrete values.
static std::string valueFormatter(const double &v)
double error(const HybridValues &values) const override
Compute error of factor given hybrid values.
bool equal(const T &obj1, const T &obj2, double tol)
const DiscreteKeys & discreteKeys() const
Return the discrete keys for this factor.
A set of GaussianFactors, indexed by a set of discrete keys.
Vector factorError(const Point3 &T1, const Point3 &T2, const TranslationFactor &factor)
gtsam
Author(s):
autogenerated on Tue Jun 25 2024 03:01:44