From a25a0a923bebb50c531f725df0ff732d8c9db244 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 29 Sep 2024 00:13:08 -0700
Subject: [PATCH] perfect forwarding magic

---
 gtsam/hybrid/HybridGaussianConditional.cpp | 45 ++++++----------------
 1 file changed, 12 insertions(+), 33 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp
index 068bd2e5d4..c348b85f0a 100644
--- a/gtsam/hybrid/HybridGaussianConditional.cpp
+++ b/gtsam/hybrid/HybridGaussianConditional.cpp
@@ -26,11 +26,10 @@
 #include <gtsam/inference/Conditional-inst.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/JacobianFactor.h>
 
 #include <cstddef>
 
-#include "gtsam/linear/JacobianFactor.h"
-
 namespace gtsam {
 /* *******************************************************************************/
 struct HybridGaussianConditional::Helper {
@@ -42,47 +41,27 @@ struct HybridGaussianConditional::Helper {
   using GC = GaussianConditional;
   using P = std::vector<std::pair<Vector, double>>;
 
-  // Common code for three constructors below:
-  template <typename Create>
-  void initialize(const DiscreteKey &mode, const P &p, Create create) {
+  /// Construct from a vector of mean and sigma pairs, plus extra args.
+  template <typename... Args>
+  Helper(const DiscreteKey &mode, const P &p, Args &&...args) {
     nrFrontals = 1;
     minNegLogConstant = std::numeric_limits<double>::infinity();
 
     std::vector<GaussianFactorValuePair> fvs;
     std::vector<GC::shared_ptr> gcs;
     for (const auto &[mean, sigma] : p) {
-      auto c = create(mean, sigma);
-      double value = c->negLogConstant();
+      auto gaussianConditional =
+          GC::sharedMeanAndStddev(std::forward<Args>(args)..., mean, sigma);
+      double value = gaussianConditional->negLogConstant();
       minNegLogConstant = std::min(minNegLogConstant, value);
-      fvs.push_back({c, value});
-      gcs.push_back(c);
+      fvs.push_back({gaussianConditional, value});
+      gcs.push_back(gaussianConditional);
     }
 
     conditionals = Conditionals({mode}, gcs);
     pairs = FactorValuePairs({mode}, fvs);
   }
 
-  // Constructors for different types of GaussianConditionals:
-
-  Helper(const DiscreteKey &mode, Key x0, const P &p) {
-    initialize(mode, p, [x0](const Vector &mean, double sigma) {
-      return GC::sharedMeanAndStddev(x0, mean, sigma);
-    });
-  }
-
-  Helper(const DiscreteKey &mode, Key x0, const Matrix &A, Key x1, const P &p) {
-    initialize(mode, p, [x0, A, x1](const Vector &mean, double sigma) {
-      return GC::sharedMeanAndStddev(x0, A, x1, mean, sigma);
-    });
-  }
-
-  Helper(const DiscreteKey &mode, Key x0,  //
-         const Matrix &A1, Key x1, const Matrix &A2, Key x2, const P &p) {
-    initialize(mode, p, [x0, A1, x1, A2, x2](const Vector &mean, double sigma) {
-      return GC::sharedMeanAndStddev(x0, A1, x1, A2, x2, mean, sigma);
-    });
-  }
-
   /// Construct from tree of GaussianConditionals.
   Helper(const Conditionals &conditionals)
       : conditionals(conditionals),
@@ -124,14 +103,14 @@ HybridGaussianConditional::HybridGaussianConditional(
     const DiscreteKey mode, Key key,  //
     const std::vector<std::pair<Vector, double>> &parameters)
     : HybridGaussianConditional(DiscreteKeys{mode},
-                                Helper(mode, key, parameters)) {}
+                                Helper(mode, parameters, key)) {}
 
 HybridGaussianConditional::HybridGaussianConditional(
     const DiscreteKey mode, Key key,  //
     const Matrix &A, Key parent,
     const std::vector<std::pair<Vector, double>> &parameters)
     : HybridGaussianConditional(DiscreteKeys{mode},
-                                Helper(mode, key, A, parent, parameters)) {}
+                                Helper(mode, parameters, key, A, parent)) {}
 
 HybridGaussianConditional::HybridGaussianConditional(
     const DiscreteKey mode, Key key,  //
@@ -139,7 +118,7 @@ HybridGaussianConditional::HybridGaussianConditional(
     const std::vector<std::pair<Vector, double>> &parameters)
     : HybridGaussianConditional(
           DiscreteKeys{mode},
-          Helper(mode, key, A1, parent1, A2, parent2, parameters)) {}
+          Helper(mode, parameters, key, A1, parent1, A2, parent2)) {}
 
 HybridGaussianConditional::HybridGaussianConditional(
     const DiscreteKeys &discreteParents,