Go to the documentation of this file.
50 double min_value = valueTree.
min();
54 auto [gf,
value] = gfv;
56 auto jf = std::dynamic_pointer_cast<JacobianFactor>(gf);
59 double normalized_value =
value - min_value;
62 if (normalized_value == 0.0)
return gf;
70 auto constantFactor = std::make_shared<JacobianFactor>(
c);
73 return std::dynamic_pointer_cast<GaussianFactor>(
74 std::make_shared<JacobianFactor>(gfg));
87 const This *
e =
dynamic_cast<const This *
>(&lf);
88 if (
e ==
nullptr)
return false;
98 return f1->equals(*f2, tol);
105 std::cout << (
s.empty() ?
"" :
s +
"\n");
106 std::cout <<
"HybridGaussianFactor" << std::endl;
110 std::cout <<
" empty" << std::endl;
125 std::cout <<
"}" << std::endl;
172 auto errorFunc = [
this, &continuousValues](
const sharedFactor &gf) {
DecisionTree< Key, sharedFactor > Factors
typedef for Decision Tree of Gaussian factors.
sharedFactor operator()(const DiscreteValues &assignment) const
Get factor at a given discrete assignment.
Linear Factor Graph where all factors are Gaussians.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
AlgebraicDecisionTree< Key > errorTree(const VectorValues &continuousValues) const override
Compute error of the HybridGaussianFactor as a tree.
virtual bool equals(const HybridFactor &lf, double tol=1e-9) const
equals
HybridGaussianFactor()=default
Default constructor, mainly for serialization.
bool empty() const
Check if tree is empty.
bool equals(const DecisionTree &other, const CompareFunc &compare=&DefaultCompare) const
const GaussianFactorGraph factors
mxArray * wrap(const Class &value)
NonlinearFactorGraph graph2()
static HybridGaussianFactor::Factors augment(const HybridGaussianFactor::FactorValuePairs &factors)
Helper function to augment the [A|b] matrices in the factor components with the additional scalar val...
const KeyFormatter & formatter
double f2(const Vector2 &x)
GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const
Combine the Gaussian Factor Graphs in sum and this while maintaining the original tree structure.
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.
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
A set of GaussianFactors, indexed by a set of discrete keys.
std::pair< GaussianFactor::shared_ptr, double > GaussianFactorValuePair
Alias for pair of GaussianFactor::shared_pointer and a double value.
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Decision Tree for use in DiscreteFactors.
DecisionTree apply(const Unary &op) const
bool equals(const HybridFactor &lf, double tol=1e-9) const override
equals
GaussianFactorGraphTree asGaussianFactorGraphTree() const
Helper function to return factors and functional to create a DecisionTree of Gaussian Factor Graphs.
void print(const std::string &s="HybridFactor\n", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
std::shared_ptr< GaussianFactor > sharedFactor
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
double potentiallyPrunedComponentError(const sharedFactor &gf, const VectorValues &continuousValues) const
Helper method to compute the error of a component.
double min() const
Find the minimum values amongst all leaves.
Factors factors_
Decision tree of Gaussian factors indexed by discrete keys.
double error(const HybridValues &values) const override
Compute the log-likelihood, including the log-normalizing constant.
A factor with a quadratic error function - a Gaussian.
std::pair< DecisionTree< L, T1 >, DecisionTree< L, T2 > > unzip(const DecisionTree< L, std::pair< T1, T2 > > &input)
unzip a DecisionTree with std::pair values.
std::uint64_t Key
Integer nonlinear key type.
Jet< T, N > sqrt(const Jet< T, N > &f)
std::string str() const
return the string
gtsam
Author(s):
autogenerated on Wed Sep 25 2024 03:02:32