diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 8c73b7952..d724bdff3 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -45,6 +45,15 @@ using GaussianFactorValuePair = std::pair; * where the set of discrete variables indexes to * the continuous gaussian distribution. * + * In factor graphs the error function typically returns 0.5*|h(x)-z|^2, i.e., + * the negative log-likelihood for a Gaussian noise model. + * In hybrid factor graphs we allow *adding* an arbitrary scalar dependent on + * the discrete assignment. + * For example, adding a 70/30 mode probability is supported by providing the + * scalars $-log(.7)$ and $-log(.3)$. + * Note that adding a common constant will not make any difference in the + * optimization, so $-log(70)$ and $-log(30)$ work just as well. + * * @ingroup hybrid */ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 3c0e2ab91..a0c7af92b 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -45,6 +45,17 @@ using NonlinearFactorValuePair = std::pair; * This class stores all factors as HybridFactors which can then be typecast to * one of (NonlinearFactor, GaussianFactor) which can then be checked to perform * the correct operation. + * + * In factor graphs the error function typically returns 0.5*|h(x)-z|^2, i.e., + * the negative log-likelihood for a Gaussian noise model. + * In hybrid factor graphs we allow *adding* an arbitrary scalar dependent on + * the discrete assignment. + * For example, adding a 70/30 mode probability is supported by providing the + * scalars $-log(.7)$ and $-log(.3)$. + * Note that adding a common constant will not make any difference in the + * optimization, so $-log(70)$ and $-log(30)$ work just as well. + * + * @ingroup hybrid */ class HybridNonlinearFactor : public HybridFactor { public: @@ -134,7 +145,7 @@ class HybridNonlinearFactor : public HybridFactor { auto errorFunc = [continuousValues](const std::pair& f) { auto [factor, val] = f; - return factor->error(continuousValues) + (0.5 * val); + return factor->error(continuousValues) + val; }; DecisionTree result(factors_, errorFunc); return result; @@ -153,7 +164,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); + return factorError + val; } /**