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

Add conditional constructors as well

release/4.3a0
Frank Dellaert 2024-09-28 23:19:02 -07:00
parent 3a5d7a4648
commit ad9fd1969e
8 changed files with 110 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
gtsam/hybrid/HybridGaussianConditional.cpp
View File

@ -33,14 +33,14 @@
namespace gtsam {
/* *******************************************************************************/
struct HybridGaussianConditional::ConstructorHelper {
struct HybridGaussianConditional::Helper {
std::optional<size_t> nrFrontals;
FactorValuePairs pairs;
Conditionals conditionals;
double minNegLogConstant;
/// Construct from tree of GaussianConditionals.
ConstructorHelper(const Conditionals &conditionals)
Helper(const Conditionals &conditionals)
: conditionals(conditionals),
minNegLogConstant(std::numeric_limits<double>::infinity()) {
auto func = [this](const GaussianConditional::shared_ptr &c)
@ -63,14 +63,54 @@ struct HybridGaussianConditional::ConstructorHelper {
}
/// Construct from means and a sigmas.
ConstructorHelper(Key x, const DiscreteKey mode,
const std::vector<std::pair<Vector, double>> &parameters)
Helper(const DiscreteKey mode, Key key,
const std::vector<std::pair<Vector, double>> &parameters)
: nrFrontals(1),
minNegLogConstant(std::numeric_limits<double>::infinity()) {
std::vector<GaussianConditional::shared_ptr> gcs;
std::vector<GaussianFactorValuePair> fvs;
for (const auto &[mean, sigma] : parameters) {
auto c = GaussianConditional::sharedMeanAndStddev(x, mean, sigma);
auto c = GaussianConditional::sharedMeanAndStddev(key, mean, sigma);
double value = c->negLogConstant();
minNegLogConstant = std::min(minNegLogConstant, value);
gcs.push_back(c);
fvs.push_back({c, value});
}
conditionals = Conditionals({mode}, gcs);
pairs = FactorValuePairs({mode}, fvs);
}
/// Construct from means and a sigmas.
Helper(const DiscreteKey mode, Key key, //
const Matrix &A, Key parent,
const std::vector<std::pair<Vector, double>> &parameters)
: nrFrontals(1),
minNegLogConstant(std::numeric_limits<double>::infinity()) {
std::vector<GaussianConditional::shared_ptr> gcs;
std::vector<GaussianFactorValuePair> fvs;
for (const auto &[mean, sigma] : parameters) {
auto c =
GaussianConditional::sharedMeanAndStddev(key, A, parent, mean, sigma);
double value = c->negLogConstant();
minNegLogConstant = std::min(minNegLogConstant, value);
gcs.push_back(c);
fvs.push_back({c, value});
}
conditionals = Conditionals({mode}, gcs);
pairs = FactorValuePairs({mode}, fvs);
}
/// Construct from means and a sigmas.
Helper(const DiscreteKey mode, Key key, //
const Matrix &A1, Key parent1, const Matrix &A2, Key parent2,
const std::vector<std::pair<Vector, double>> &parameters)
: nrFrontals(1),
minNegLogConstant(std::numeric_limits<double>::infinity()) {
std::vector<GaussianConditional::shared_ptr> gcs;
std::vector<GaussianFactorValuePair> fvs;
for (const auto &[mean, sigma] : parameters) {
auto c = GaussianConditional::sharedMeanAndStddev(key, A1, parent1, A2,
parent2, mean, sigma);
double value = c->negLogConstant();
minNegLogConstant = std::min(minNegLogConstant, value);
gcs.push_back(c);
@ -83,7 +123,7 @@ struct HybridGaussianConditional::ConstructorHelper {
/* *******************************************************************************/
HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKeys &discreteParents, const ConstructorHelper &helper)
const DiscreteKeys &discreteParents, const Helper &helper)
: BaseFactor(discreteParents, helper.pairs),
BaseConditional(*helper.nrFrontals),
conditionals_(helper.conditionals),
@ -96,16 +136,30 @@ HybridGaussianConditional::HybridGaussianConditional(
Conditionals({mode}, conditionals)) {}
HybridGaussianConditional::HybridGaussianConditional(
Key x, const DiscreteKey mode,
const DiscreteKey mode, Key key, //
const std::vector<std::pair<Vector, double>> &parameters)
: HybridGaussianConditional(DiscreteKeys{mode},
ConstructorHelper(x, mode, parameters)) {}
Helper(mode, key, parameters)) {}
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)) {}
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)) {}
HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKeys &discreteParents,
const HybridGaussianConditional::Conditionals &conditionals)
: HybridGaussianConditional(discreteParents,
ConstructorHelper(conditionals)) {}
: HybridGaussianConditional(discreteParents, Helper(conditionals)) {}
/* *******************************************************************************/
const HybridGaussianConditional::Conditionals &

26
gtsam/hybrid/HybridGaussianConditional.h
View File

@ -87,15 +87,21 @@ class GTSAM_EXPORT HybridGaussianConditional
const DiscreteKey &mode,
const std::vector<GaussianConditional::shared_ptr> &conditionals);
/**
* @brief Construct from vector of means and sigmas.
*
* @param x The continuous key.
* @param mode The discrete key.
* @param parameters The means and sigmas for the Gaussian conditionals.
*/
/// Construct from mean `mu_i` and `sigma_i`.
HybridGaussianConditional(
Key x, const DiscreteKey mode,
const DiscreteKey mode, Key key, //
const std::vector<std::pair<Vector, double>> &parameters);
/// Construct from conditional mean `A1 p1 + b_i` and `sigma_i`.
HybridGaussianConditional(
const DiscreteKey mode, Key key, //
const Matrix &A, Key parent,
const std::vector<std::pair<Vector, double>> &parameters);
/// Construct from conditional mean `A1 p1 + A2 p2 + b_i` and `sigma_i`.
HybridGaussianConditional(
const DiscreteKey mode, Key key, //
const Matrix &A1, Key parent1, const Matrix &A2, Key parent2,
const std::vector<std::pair<Vector, double>> &parameters);
/**
@ -194,11 +200,11 @@ class GTSAM_EXPORT HybridGaussianConditional
private:
/// Helper struct for private constructor.
struct ConstructorHelper;
struct Helper;
/// Private constructor that uses helper struct above.
HybridGaussianConditional(const DiscreteKeys &discreteParents,
const ConstructorHelper &helper);
const Helper &helper);
/// Convert to a DecisionTree of Gaussian factor graphs.
GaussianFactorGraphTree asGaussianFactorGraphTree() const;

8
gtsam/hybrid/tests/TinyHybridExample.h
View File

@ -41,12 +41,12 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1,
HybridBayesNet bayesNet;
// Create hybrid Gaussian factor z_i = x0 + noise for each measurement.
std::vector<std::pair<Vector, double>> measurementModels{{Z_1x1, 0.5},
{Z_1x1, 3.0}};
for (size_t i = 0; i < num_measurements; i++) {
const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode;
std::vector<GaussianConditional::shared_ptr> conditionals{
GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 0.5),
GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 3)};
bayesNet.emplace_shared<HybridGaussianConditional>(mode_i, conditionals);
bayesNet.emplace_shared<HybridGaussianConditional>(mode_i, Z(i), I_1x1,
X(0), measurementModels);
}
// Create prior on X(0).

4
gtsam/hybrid/tests/testGaussianMixture.cpp
View File

@ -82,7 +82,7 @@ TEST(GaussianMixture, GaussianMixtureModel) {
HybridBayesNet hbn;
std::vector<std::pair<Vector, double>> parameters{{Vector1(mu0), sigma},
{Vector1(mu1), sigma}};
hbn.emplace_shared<HybridGaussianConditional>(Z(0), m, parameters);
hbn.emplace_shared<HybridGaussianConditional>(m, Z(0), parameters);
hbn.push_back(mixing);
// At the halfway point between the means, we should get P(m|z)=0.5
@ -120,7 +120,7 @@ TEST(GaussianMixture, GaussianMixtureModel2) {
HybridBayesNet hbn;
std::vector<std::pair<Vector, double>> parameters{{Vector1(mu0), sigma0},
{Vector1(mu1), sigma1}};
hbn.emplace_shared<HybridGaussianConditional>(Z(0), m, parameters);
hbn.emplace_shared<HybridGaussianConditional>(m, Z(0), parameters);
hbn.push_back(mixing);
// We get zMax=3.1333 by finding the maximum value of the function, at which

2
gtsam/hybrid/tests/testHybridBayesNet.cpp
View File

@ -99,7 +99,7 @@ const auto gc = GaussianConditional::sharedMeanAndStddev(X(0), 2 * I_1x1, X(1),
const std::vector<std::pair<Vector, double>> parms{{Vector1(5), 2.0},
{Vector1(2), 3.0}};
const auto hgc = std::make_shared<HybridGaussianConditional>(X(1), Asia, parms);
const auto hgc = std::make_shared<HybridGaussianConditional>(Asia, X(1), parms);
const auto prior = std::make_shared<DiscreteConditional>(Asia, "99/1");
auto wrap = [](const auto& c) {

20
gtsam/hybrid/tests/testHybridEstimation.cpp
View File

@ -573,12 +573,10 @@ TEST(HybridEstimation, ModeSelection) {
bn.push_back(
GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(1), Z_1x1, 0.1));
std::vector<GaussianConditional::shared_ptr> conditionals{
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1),
Z_1x1, noise_loose),
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1),
Z_1x1, noise_tight)};
bn.emplace_shared<HybridGaussianConditional>(mode, conditionals);
std::vector<std::pair<Vector, double>> parameters{{Z_1x1, noise_loose},
{Z_1x1, noise_tight}};
bn.emplace_shared<HybridGaussianConditional>(mode, Z(0), I_1x1, X(0), -I_1x1,
X(1), parameters);
VectorValues vv;
vv.insert(Z(0), Z_1x1);
@ -605,12 +603,10 @@ TEST(HybridEstimation, ModeSelection2) {
bn.push_back(
GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(1), Z_3x1, 0.1));
std::vector<GaussianConditional::shared_ptr> conditionals{
GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1),
Z_3x1, noise_loose),
GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1),
Z_3x1, noise_tight)};
bn.emplace_shared<HybridGaussianConditional>(mode, conditionals);
std::vector<std::pair<Vector, double>> parameters{{Z_3x1, noise_loose},
{Z_3x1, noise_tight}};
bn.emplace_shared<HybridGaussianConditional>(mode, Z(0), I_3x3, X(0), -I_3x3,
X(1), parameters);
VectorValues vv;
vv.insert(Z(0), Z_3x1);

30
gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
View File

@ -803,11 +803,8 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) {
// mode-dependent: 1 is low-noise, 0 is high-noise.
// Create hybrid Gaussian factor z_0 = x0 + noise for each measurement.
std::vector<GaussianConditional::shared_ptr> conditionals{
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 3),
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 0.5)};
auto gm = std::make_shared<HybridGaussianConditional>(m1, conditionals);
bn.push_back(gm);
std::vector<std::pair<Vector, double>> parms{{Z_1x1, 3}, {Z_1x1, 0.5}};
bn.emplace_shared<HybridGaussianConditional>(m1, Z(0), I_1x1, X(0), parms);
// Create prior on X(0).
bn.push_back(
@ -912,32 +909,27 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
// NOTE: we add reverse topological so we can sample from the Bayes net.:
// Add measurements:
std::vector<std::pair<Vector, double>> measurementModels{{Z_1x1, 3},
{Z_1x1, 0.5}};
for (size_t t : {0, 1, 2}) {
// Create hybrid Gaussian factor on Z(t) conditioned on X(t) and mode N(t):
const auto noise_mode_t = DiscreteKey{N(t), 2};
std::vector<GaussianConditional::shared_ptr> conditionals{
GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1, 0.5),
GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1,
3.0)};
bn.emplace_shared<HybridGaussianConditional>(noise_mode_t, conditionals);
bn.emplace_shared<HybridGaussianConditional>(noise_mode_t, Z(t), I_1x1,
X(t), measurementModels);
// Create prior on discrete mode N(t):
bn.emplace_shared<DiscreteConditional>(noise_mode_t, "20/80");
}
// Add motion models:
// Add motion models. TODO(frank): why are they exactly the same?
std::vector<std::pair<Vector, double>> motionModels{{Z_1x1, 0.2},
{Z_1x1, 0.2}};
for (size_t t : {2, 1}) {
// Create hybrid Gaussian factor on X(t) conditioned on X(t-1)
// and mode M(t-1):
const auto motion_model_t = DiscreteKey{M(t), 2};
std::vector<GaussianConditional::shared_ptr> conditionals{
GaussianConditional::sharedMeanAndStddev(X(t), I_1x1, X(t - 1), Z_1x1,
0.2),
GaussianConditional::sharedMeanAndStddev(X(t), I_1x1, X(t - 1), I_1x1,
0.2)};
auto gm = std::make_shared<HybridGaussianConditional>(motion_model_t,
conditionals);
bn.push_back(gm);
bn.emplace_shared<HybridGaussianConditional>(motion_model_t, X(t), I_1x1,
X(t - 1), motionModels);
// Create prior on motion model M(t):
bn.emplace_shared<DiscreteConditional>(motion_model_t, "40/60");

11
gtsam/hybrid/tests/testHybridMotionModel.cpp
View File

@ -55,13 +55,10 @@ void addMeasurement(HybridBayesNet &hbn, Key z_key, Key x_key, double sigma) {
/// Create hybrid motion model p(x1 | x0, m1)
static HybridGaussianConditional::shared_ptr CreateHybridMotionModel(
double mu0, double mu1, double sigma0, double sigma1) {
auto model0 = noiseModel::Isotropic::Sigma(1, sigma0);
auto model1 = noiseModel::Isotropic::Sigma(1, sigma1);
auto c0 = make_shared<GaussianConditional>(X(1), Vector1(mu0), I_1x1, X(0),
-I_1x1, model0),
c1 = make_shared<GaussianConditional>(X(1), Vector1(mu1), I_1x1, X(0),
-I_1x1, model1);
return std::make_shared<HybridGaussianConditional>(m1, std::vector{c0, c1});
std::vector<std::pair<Vector, double>> motionModels{{Vector1(mu0), sigma0},
{Vector1(mu1), sigma1}};
return std::make_shared<HybridGaussianConditional>(m1, X(1), I_1x1, X(0),
motionModels);
}
/// Create two state Bayes network with 1 or two measurement models