fix error using value for HybridNonlinearFactor
parent
cfe2ad56bf
commit
48e087e083
|
@ -119,7 +119,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
|
|||
/// @name Standard API
|
||||
/// @{
|
||||
|
||||
/// Get the factor and scalar at a given discrete assignment.
|
||||
/// Get factor at a given discrete assignment.
|
||||
sharedFactor operator()(const DiscreteValues &assignment) const;
|
||||
|
||||
/**
|
||||
|
|
|
@ -138,7 +138,7 @@ class HybridNonlinearFactor : public HybridFactor {
|
|||
auto errorFunc =
|
||||
[continuousValues](const std::pair<sharedFactor, double>& f) {
|
||||
auto [factor, val] = f;
|
||||
return factor->error(continuousValues) + val;
|
||||
return factor->error(continuousValues) + (0.5 * val * val);
|
||||
};
|
||||
DecisionTree<Key, double> result(factors_, errorFunc);
|
||||
return result;
|
||||
|
@ -157,7 +157,7 @@ class HybridNonlinearFactor : public HybridFactor {
|
|||
auto [factor, val] = factors_(discreteValues);
|
||||
// Compute the error for the selected factor
|
||||
const double factorError = factor->error(continuousValues);
|
||||
return factorError + val;
|
||||
return factorError + (0.5 * val * val);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
Loading…
Reference in New Issue