diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 742b4c162..096c14f77 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -140,7 +140,7 @@ class HybridNonlinearFactor : public HybridFactor { auto errorFunc = [continuousValues](const std::pair& f) { auto [factor, val] = f; - return factor->error(continuousValues) + (0.5 * val * val); + return factor->error(continuousValues) + (0.5 * val); }; DecisionTree result(factors_, errorFunc); return result; @@ -159,7 +159,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 + (0.5 * val * val); + return factorError + (0.5 * val); } /**