hide sqrt(2*value) so the user doesn't have to premultiply by 2

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -222,8 +222,7 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
         } else {
           // Add a constant to the likelihood in case the noise models
           // are not all equal.
-          double c = 2.0 * Cgm_Kgcm;
-          return {likelihood_m, c};
+          return {likelihood_m, Cgm_Kgcm};
         }
       });
   return std::make_shared<HybridGaussianFactor>(

gtsam/hybrid/HybridGaussianFactor.cpp

@@ -46,31 +46,34 @@ HybridGaussianFactor::Factors augment(
   AlgebraicDecisionTree<Key> valueTree;
   std::tie(gaussianFactors, valueTree) = unzip(factors);
 
-  // Normalize
+  // Compute minimum value for normalization.
   double min_value = valueTree.min();
-  AlgebraicDecisionTree<Key> values =
-      valueTree.apply([&min_value](double n) { return n - min_value; });
 
   // Finally, update the [A|b] matrices.
-  auto update = [&values](const Assignment<Key> &assignment,
-                          const HybridGaussianFactor::sharedFactor &gf) {
+  auto update = [&min_value](const GaussianFactorValuePair &gfv) {
+    auto [gf, value] = gfv;
+
     auto jf = std::dynamic_pointer_cast<JacobianFactor>(gf);
     if (!jf) return gf;
+
+    double normalized_value = value - min_value;
+
     // If the value is 0, do nothing
-    if (values(assignment) == 0.0) return gf;
+    if (normalized_value == 0.0) return gf;
 
     GaussianFactorGraph gfg;
     gfg.push_back(jf);
 
     Vector c(1);
-    c << std::sqrt(values(assignment));
+    // When hiding c inside the `b` vector, value == 0.5*c^2
+    c << std::sqrt(2.0 * normalized_value);
     auto constantFactor = std::make_shared<JacobianFactor>(c);
 
     gfg.push_back(constantFactor);
     return std::dynamic_pointer_cast<GaussianFactor>(
         std::make_shared<JacobianFactor>(gfg));
   };
-  return gaussianFactors.apply(update);
+  return HybridGaussianFactor::Factors(factors, update);
 }
 
 /* *******************************************************************************/

gtsam/hybrid/tests/testHybridGaussianFactor.cpp

@@ -770,10 +770,11 @@ static HybridGaussianFactorGraph CreateFactorGraph(
           ->linearize(values);
 
   // Create HybridGaussianFactor
-  // We multiply by -2 since the we want the underlying scalar to be log(|2πΣ|)
+  // We take negative since we want
+  // the underlying scalar to be log(\sqrt(|2πΣ|))
   std::vector<GaussianFactorValuePair> factors{
-      {f0, -2 * model0->logNormalizationConstant()},
-      {f1, -2 * model1->logNormalizationConstant()}};
+      {f0, -model0->logNormalizationConstant()},
+      {f1, -model1->logNormalizationConstant()}};
   HybridGaussianFactor motionFactor({X(0), X(1)}, m1, factors);
 
   HybridGaussianFactorGraph hfg;

gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp

@@ -868,10 +868,11 @@ static HybridNonlinearFactorGraph CreateFactorGraph(
       std::make_shared<BetweenFactor<double>>(X(0), X(1), means[1], model1);
 
   // Create HybridNonlinearFactor
-  // We multiply by -2 since the we want the underlying scalar to be log(|2πΣ|)
+  // We take negative since we want
+  // the underlying scalar to be log(\sqrt(|2πΣ|))
   std::vector<NonlinearFactorValuePair> factors{
-      {f0, -2 * model0->logNormalizationConstant()},
-      {f1, -2 * model1->logNormalizationConstant()}};
+      {f0, -model0->logNormalizationConstant()},
+      {f1, -model1->logNormalizationConstant()}};
 
   HybridNonlinearFactor mixtureFactor({X(0), X(1)}, m1, factors);