diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index e40713f46..f52022348 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -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; /** diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 1bd25a6b1..c3f127f4a 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -138,7 +138,7 @@ class HybridNonlinearFactor : public HybridFactor { auto errorFunc = [continuousValues](const std::pair& f) { auto [factor, val] = f; - return factor->error(continuousValues) + val; + return factor->error(continuousValues) + (0.5 * val * val); }; DecisionTree 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); } /**