12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Address o1 review comments

release/4.3a0
Frank Dellaert 2024-09-29 00:21:40 -07:00
parent a25a0a923b
commit 4fa192248d
2 changed files with 13 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/hybrid/HybridGaussianConditional.cpp
View File

@ -43,18 +43,20 @@ struct HybridGaussianConditional::Helper {
/// Construct from a vector of mean and sigma pairs, plus extra args. /// Construct from a vector of mean and sigma pairs, plus extra args.
template <typename... Args> template <typename... Args>
Helper(const DiscreteKey &mode, const P &p, Args &&...args) { explicit Helper(const DiscreteKey &mode, const P &p, Args &&...args) {
nrFrontals = 1; nrFrontals = 1;
minNegLogConstant = std::numeric_limits<double>::infinity(); minNegLogConstant = std::numeric_limits<double>::infinity();
std::vector<GaussianFactorValuePair> fvs; std::vector<GaussianFactorValuePair> fvs;
std::vector<GC::shared_ptr> gcs; std::vector<GC::shared_ptr> gcs;
fvs.reserve(p.size());
gcs.reserve(p.size());
for (const auto &[mean, sigma] : p) { for (const auto &[mean, sigma] : p) {
auto gaussianConditional = auto gaussianConditional =
GC::sharedMeanAndStddev(std::forward<Args>(args)..., mean, sigma); GC::sharedMeanAndStddev(std::forward<Args>(args)..., mean, sigma);
double value = gaussianConditional->negLogConstant(); double value = gaussianConditional->negLogConstant();
minNegLogConstant = std::min(minNegLogConstant, value); minNegLogConstant = std::min(minNegLogConstant, value);
fvs.push_back({gaussianConditional, value}); fvs.emplace_back(gaussianConditional, value);
gcs.push_back(gaussianConditional); gcs.push_back(gaussianConditional);
} }
@ -63,7 +65,7 @@ struct HybridGaussianConditional::Helper {
} }
/// Construct from tree of GaussianConditionals. /// Construct from tree of GaussianConditionals.
Helper(const Conditionals &conditionals) explicit Helper(const Conditionals &conditionals)
: conditionals(conditionals), : conditionals(conditionals),
minNegLogConstant(std::numeric_limits<double>::infinity()) { minNegLogConstant(std::numeric_limits<double>::infinity()) {
auto func = [this](const GC::shared_ptr &c) -> GaussianFactorValuePair { auto func = [this](const GC::shared_ptr &c) -> GaussianFactorValuePair {
@ -80,7 +82,8 @@ struct HybridGaussianConditional::Helper {
pairs = FactorValuePairs(conditionals, func); pairs = FactorValuePairs(conditionals, func);
if (!nrFrontals.has_value()) { if (!nrFrontals.has_value()) {
throw std::runtime_error( throw std::runtime_error(
"HybridGaussianConditional: need at least one frontal variable."); "HybridGaussianConditional: need at least one frontal variable. "
"Provided conditionals do not contain any frontal variables.");
} }
} }
}; };
@ -100,20 +103,20 @@ HybridGaussianConditional::HybridGaussianConditional(
Conditionals({mode}, conditionals)) {} Conditionals({mode}, conditionals)) {}
HybridGaussianConditional::HybridGaussianConditional( HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKey mode, Key key, // const DiscreteKey &mode, Key key, //
const std::vector<std::pair<Vector, double>> &parameters) const std::vector<std::pair<Vector, double>> &parameters)
: HybridGaussianConditional(DiscreteKeys{mode}, : HybridGaussianConditional(DiscreteKeys{mode},
Helper(mode, parameters, key)) {} Helper(mode, parameters, key)) {}
HybridGaussianConditional::HybridGaussianConditional( HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKey mode, Key key, // const DiscreteKey &mode, Key key, //
const Matrix &A, Key parent, const Matrix &A, Key parent,
const std::vector<std::pair<Vector, double>> &parameters) const std::vector<std::pair<Vector, double>> &parameters)
: HybridGaussianConditional(DiscreteKeys{mode}, : HybridGaussianConditional(DiscreteKeys{mode},
Helper(mode, parameters, key, A, parent)) {} Helper(mode, parameters, key, A, parent)) {}
HybridGaussianConditional::HybridGaussianConditional( HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKey mode, Key key, // const DiscreteKey &mode, Key key, //
const Matrix &A1, Key parent1, const Matrix &A2, Key parent2, const Matrix &A1, Key parent1, const Matrix &A2, Key parent2,
const std::vector<std::pair<Vector, double>> &parameters) const std::vector<std::pair<Vector, double>> &parameters)
: HybridGaussianConditional( : HybridGaussianConditional(

6
gtsam/hybrid/HybridGaussianConditional.h
View File

@ -89,18 +89,18 @@ class GTSAM_EXPORT HybridGaussianConditional
/// Construct from mean `mu_i` and `sigma_i`. /// Construct from mean `mu_i` and `sigma_i`.
HybridGaussianConditional( HybridGaussianConditional(
const DiscreteKey mode, Key key, // const DiscreteKey &mode, Key key, //
const std::vector<std::pair<Vector, double>> &parameters); const std::vector<std::pair<Vector, double>> &parameters);
/// Construct from conditional mean `A1 p1 + b_i` and `sigma_i`. /// Construct from conditional mean `A1 p1 + b_i` and `sigma_i`.
HybridGaussianConditional( HybridGaussianConditional(
const DiscreteKey mode, Key key, // const DiscreteKey &mode, Key key, //
const Matrix &A, Key parent, const Matrix &A, Key parent,
const std::vector<std::pair<Vector, double>> &parameters); const std::vector<std::pair<Vector, double>> &parameters);
/// Construct from conditional mean `A1 p1 + A2 p2 + b_i` and `sigma_i`. /// Construct from conditional mean `A1 p1 + A2 p2 + b_i` and `sigma_i`.
HybridGaussianConditional( HybridGaussianConditional(
const DiscreteKey mode, Key key, // const DiscreteKey &mode, Key key, //
const Matrix &A1, Key parent1, const Matrix &A2, Key parent2, const Matrix &A1, Key parent1, const Matrix &A2, Key parent2,
const std::vector<std::pair<Vector, double>> &parameters); const std::vector<std::pair<Vector, double>> &parameters);