Skip to content

Commit

Permalink
perfect forwarding magic
Browse files Browse the repository at this point in the history
  • Loading branch information
dellaert committed Sep 29, 2024
1 parent 14d0f9f commit a25a0a9
Showing 1 changed file with 12 additions and 33 deletions.
45 changes: 12 additions & 33 deletions gtsam/hybrid/HybridGaussianConditional.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand All @@ -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),
Expand Down Expand Up @@ -124,22 +103,22 @@ 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, //
const Matrix &A1, Key parent1, const Matrix &A2, Key parent2,
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,
Expand Down

0 comments on commit a25a0a9

Please sign in to comment.