12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix error using value for HybridNonlinearFactor

release/4.3a0
Varun Agrawal 2024-09-14 15:14:02 -04:00
parent cfe2ad56bf
commit 48e087e083
2 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/hybrid/HybridGaussianFactor.h
View File

@ -119,7 +119,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
/// @name Standard API /// @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; sharedFactor operator()(const DiscreteValues &assignment) const;
/** /**

4
gtsam/hybrid/HybridNonlinearFactor.h
View File

@ -138,7 +138,7 @@ class HybridNonlinearFactor : public HybridFactor {
auto errorFunc = auto errorFunc =
[continuousValues](const std::pair<sharedFactor, double>& f) { [continuousValues](const std::pair<sharedFactor, double>& f) {
auto [factor, val] = f; auto [factor, val] = f;
return factor->error(continuousValues) + val; return factor->error(continuousValues) + (0.5 * val * val);
}; };
DecisionTree<Key, double> result(factors_, errorFunc); DecisionTree<Key, double> result(factors_, errorFunc);
return result; return result;
@ -157,7 +157,7 @@ class HybridNonlinearFactor : public HybridFactor {
auto [factor, val] = factors_(discreteValues); auto [factor, val] = factors_(discreteValues);
// Compute the error for the selected factor // Compute the error for the selected factor
const double factorError = factor->error(continuousValues); const double factorError = factor->error(continuousValues);
return factorError + val; return factorError + (0.5 * val * val);
} }
/** /**