Go to the documentation of this file.
42 double min_value = valueTree.
min();
46 auto [gf,
value] = gfv;
48 auto jf = std::dynamic_pointer_cast<JacobianFactor>(gf);
51 double normalized_value =
value - min_value;
54 if (normalized_value == 0.0)
return gf;
62 auto constantFactor = std::make_shared<JacobianFactor>(
c);
65 return std::dynamic_pointer_cast<GaussianFactor>(
66 std::make_shared<JacobianFactor>(gfg));
79 const std::vector<GaussianFactor::shared_ptr> &
factors)
82 for (
const auto &factor :
factors) {
94 const std::vector<GaussianFactorValuePair> &factorPairs)
97 for (
const auto &pair : factorPairs) {
114 continuousKeys = pair.first->keys();
125 :
Base(helper.continuousKeys, helper.discreteKeys),
126 factors_(helper.factorsTree.
empty() ? augment(helper.pairs)
127 : helper.factorsTree) {}
132 const std::vector<GaussianFactor::shared_ptr> &
factors)
138 const std::vector<GaussianFactorValuePair> &factorPairs)
148 const This *
e =
dynamic_cast<const This *
>(&lf);
149 if (
e ==
nullptr)
return false;
158 return (!f1 && !f2) || (f1 && f2 && f1->equals(*f2, tol));
165 std::cout << (
s.empty() ?
"" :
s +
"\n");
166 std::cout <<
"HybridGaussianFactor" << std::endl;
170 std::cout <<
" empty" << std::endl;
185 std::cout <<
"}" << std::endl;
232 auto errorFunc = [&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.
ConstructorHelper(const DiscreteKey &discreteKey, const std::vector< GaussianFactorValuePair > &factorPairs)
bool empty() const
Check if tree is empty.
bool equals(const DecisionTree &other, const CompareFunc &compare=&DefaultCompare) const
const GaussianFactorGraph factors
const Factors & factors() const
Getter for GaussianFactor decision tree.
mxArray * wrap(const Class &value)
NonlinearFactorGraph graph2()
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.
std::shared_ptr< This > shared_ptr
shared_ptr to this class
void visit(Func f) const
Visit all leaves in depth-first fashion.
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.
ConstructorHelper(const DiscreteKeys &discreteKeys, const FactorValuePairs &factorPairs)
void print(const std::string &s="HybridFactor\n", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
DecisionTree< Key, GaussianFactorValuePair > FactorValuePairs
typedef for Decision Tree of Gaussian factors and arbitrary value.
std::shared_ptr< GaussianFactor > sharedFactor
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
std::pair< Key, size_t > DiscreteKey
Implementation of a discrete-conditioned hybrid factor. Implements a joint discrete-continuous factor...
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.
static Factors augment(const FactorValuePairs &factors)
Helper function to augment the [A|b] matrices in the factor components with the additional scalar val...
std::pair< DecisionTree< L, T1 >, DecisionTree< L, T2 > > unzip(const DecisionTree< L, std::pair< T1, T2 > > &input)
unzip a DecisionTree with std::pair values.
DiscreteKeys discreteKeys
std::uint64_t Key
Integer nonlinear key type.
Jet< T, N > sqrt(const Jet< T, N > &f)
std::string str() const
return the string
static double PotentiallyPrunedComponentError(const GaussianFactor::shared_ptr &gf, const VectorValues &values)
Helper method to compute the error of a component.
ConstructorHelper(const DiscreteKey &discreteKey, const std::vector< GaussianFactor::shared_ptr > &factors)
gtsam
Author(s):
autogenerated on Fri Oct 4 2024 03:02:13