Go to the documentation of this file.
38 throw std::runtime_error(
39 "HybridNonlinearFactor: all factors should have the same keys!");
44 const std::vector<NoiseModelFactor::shared_ptr>&
factors)
46 std::vector<NonlinearFactorValuePair> pairs;
48 for (
const auto& factor :
factors) {
49 pairs.emplace_back(factor, 0.0);
56 const std::vector<NonlinearFactorValuePair>& pairs)
59 for (
const auto& pair : pairs) {
77 :
Base(helper.continuousKeys, helper.discreteKeys),
78 factors_(helper.factorTree) {}
82 const std::vector<NoiseModelFactor::shared_ptr>&
factors)
87 const std::vector<NonlinearFactorValuePair>& pairs)
96 const Values& continuousValues)
const {
99 [continuousValues](
const std::pair<sharedFactor, double>&
f) {
100 auto [factor, val] =
f;
101 return factor->error(continuousValues) + val;
109 const Values& continuousValues,
112 auto [factor, val] =
factors_(discreteValues);
114 const double factorError = factor->error(continuousValues);
126 auto [factor, val] =
factors_(assignments.at(0));
127 return factor->dim();
133 std::cout << (
s.empty() ?
"" :
s +
" ");
135 std::cout <<
"\nHybridNonlinearFactor\n";
137 auto [factor, val] =
v;
139 return "Nonlinear factor on " + std::to_string(factor->size()) +
" keys";
141 return std::string(
"nullptr");
160 auto compare = [
tol](
const std::pair<sharedFactor, double>&
a,
161 const std::pair<sharedFactor, double>&
b) {
162 return a.first->equals(*
b.first,
tol) && (
a.second ==
b.second);
174 const Values& continuousValues,
176 auto factor =
factors_(discreteValues).first;
177 return factor->linearize(continuousValues);
182 const Values& continuousValues)
const {
187 auto [factor, val] =
f;
188 if (
auto gaussian = std::dynamic_pointer_cast<noiseModel::Gaussian>(
189 factor->noiseModel())) {
190 return {factor->linearize(continuousValues),
191 val + gaussian->negLogConstant()};
193 throw std::runtime_error(
194 "HybridNonlinearFactor: linearize() only supports NoiseModelFactors "
195 "with Gaussian (or derived) noise models.");
200 linearized_factors(
factors_, linearizeDT);
HybridNonlinearFactor()=default
Default constructor, mainly for serialization.
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print to stdout
ConstructorHelper(const DiscreteKeys &discreteKeys, const FactorValuePairs &factorPairs)
GaussianFactor::shared_ptr linearize(const Values &continuousValues, const DiscreteValues &discreteValues) const
bool equals(const DecisionTree &other, const CompareFunc &compare=&DefaultCompare) const
const GaussianFactorGraph factors
double error(const Values &continuousValues, const DiscreteValues &discreteValues) const
Compute error of factor given both continuous and discrete values.
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.
std::shared_ptr< This > shared_ptr
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
ConstructorHelper(const DiscreteKey &discreteKey, const std::vector< NoiseModelFactor::shared_ptr > &factors)
DiscreteKeys discreteKeys
std::pair< GaussianFactor::shared_ptr, double > GaussianFactorValuePair
Alias for pair of GaussianFactor::shared_pointer and a double value.
static std::vector< DiscreteValues > CartesianProduct(const DiscreteKeys &keys)
Return a vector of DiscreteValues, one for each possible combination of values.
bool equals(const HybridFactor &other, double tol=1e-9) const override
Check equality.
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
void visit(Func f) const
Visit all leaves in depth-first fashion.
DiscreteKeys discreteKeys_
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Non-linear factor base classes.
void print(const std::string &s="HybridFactor\n", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
A set of nonlinear factors indexed by a set of discrete keys.
KeyVector keys_
The keys involved in this factor.
std::pair< Key, size_t > DiscreteKey
ConstructorHelper(const DiscreteKey &discreteKey, const std::vector< NonlinearFactorValuePair > &pairs)
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...
void copyOrCheckContinuousKeys(const NoiseModelFactor::shared_ptr &factor)
FactorValuePairs factors_
Decision tree of nonlinear factors indexed by discrete keys.
std::pair< NoiseModelFactor::shared_ptr, double > NonlinearFactorValuePair
Alias for a NoiseModelFactor shared pointer and double scalar pair.
static std::string valueFormatter(const double &v)
AlgebraicDecisionTree< Key > errorTree(const VectorValues &continuousValues) const override
HybridFactor method implementation. Should not be used.
DecisionTree< Key, NonlinearFactorValuePair > FactorValuePairs
typedef for DecisionTree which has Keys as node labels and pairs of NoiseModelFactor & an arbitrary s...
bool equal(const T &obj1, const T &obj2, double tol)
Implementation of a discrete-conditioned hybrid factor.
FactorValuePairs factorTree
Vector factorError(const Point3 &T1, const Point3 &T2, const TranslationFactor &factor)
gtsam
Author(s):
autogenerated on Fri Oct 4 2024 03:02:14