Merge pull request #1371 from borglab/hybrid/multimode
Frank Dellaert
GitHub
commit
c24e975729
|
|
@ -377,4 +377,27 @@ AlgebraicDecisionTree<Key> HybridBayesNet::probPrime(
|
|||
return error_tree.apply([](double error) { return exp(-error); });
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
HybridGaussianFactorGraph HybridBayesNet::toFactorGraph(
|
||||
const VectorValues &measurements) const {
|
||||
HybridGaussianFactorGraph fg;
|
||||
|
||||
// For all nodes in the Bayes net, if its frontal variable is in measurements,
|
||||
// replace it by a likelihood factor:
|
||||
for (auto &&conditional : *this) {
|
||||
if (conditional->frontalsIn(measurements)) {
|
||||
if (auto gc = conditional->asGaussian())
|
||||
fg.push_back(gc->likelihood(measurements));
|
||||
else if (auto gm = conditional->asMixture())
|
||||
fg.push_back(gm->likelihood(measurements));
|
||||
else {
|
||||
throw std::runtime_error("Unknown conditional type");
|
||||
}
|
||||
} else {
|
||||
fg.push_back(conditional);
|
||||
}
|
||||
}
|
||||
return fg;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -229,6 +229,12 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
|
|||
AlgebraicDecisionTree<Key> probPrime(
|
||||
const VectorValues &continuousValues) const;
|
||||
|
||||
/**
|
||||
* Convert a hybrid Bayes net to a hybrid Gaussian factor graph by converting
|
||||
* all conditionals with instantiated measurements into likelihood factors.
|
||||
*/
|
||||
HybridGaussianFactorGraph toFactorGraph(
|
||||
const VectorValues &measurements) const;
|
||||
/// @}
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -178,6 +178,16 @@ class GTSAM_EXPORT HybridConditional
|
|||
/// Return the error of the underlying conditional.
|
||||
double error(const HybridValues& values) const override;
|
||||
|
||||
/// Check if VectorValues `measurements` contains all frontal keys.
|
||||
bool frontalsIn(const VectorValues& measurements) const {
|
||||
for (Key key : frontals()) {
|
||||
if (!measurements.exists(key)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -33,46 +33,34 @@ const DiscreteKey mode{M(0), 2};
|
|||
/**
|
||||
* Create a tiny two variable hybrid model which represents
|
||||
* the generative probability P(z,x,mode) = P(z|x,mode)P(x)P(mode).
|
||||
* num_measurements is the number of measurements of the continuous variable x0.
|
||||
* If manyModes is true, then we introduce one mode per measurement.
|
||||
*/
|
||||
inline HybridBayesNet createHybridBayesNet(int num_measurements = 1) {
|
||||
inline HybridBayesNet createHybridBayesNet(int num_measurements = 1,
|
||||
bool manyModes = false) {
|
||||
HybridBayesNet bayesNet;
|
||||
|
||||
// Create Gaussian mixture z_i = x0 + noise for each measurement.
|
||||
for (int i = 0; i < num_measurements; i++) {
|
||||
const auto conditional0 = boost::make_shared<GaussianConditional>(
|
||||
GaussianConditional::FromMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 0.5));
|
||||
const auto conditional1 = boost::make_shared<GaussianConditional>(
|
||||
GaussianConditional::FromMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 3));
|
||||
GaussianMixture gm({Z(i)}, {X(0)}, {mode}, {conditional0, conditional1});
|
||||
const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode;
|
||||
GaussianMixture gm({Z(i)}, {X(0)}, {mode_i},
|
||||
{GaussianConditional::sharedMeanAndStddev(
|
||||
Z(i), I_1x1, X(0), Z_1x1, 0.5),
|
||||
GaussianConditional::sharedMeanAndStddev(
|
||||
Z(i), I_1x1, X(0), Z_1x1, 3)});
|
||||
bayesNet.emplaceMixture(gm); // copy :-(
|
||||
}
|
||||
|
||||
// Create prior on X(0).
|
||||
const auto prior_on_x0 =
|
||||
GaussianConditional::FromMeanAndStddev(X(0), Vector1(5.0), 0.5);
|
||||
bayesNet.emplaceGaussian(prior_on_x0); // copy :-(
|
||||
bayesNet.addGaussian(
|
||||
GaussianConditional::sharedMeanAndStddev(X(0), Vector1(5.0), 0.5));
|
||||
|
||||
// Add prior on mode.
|
||||
bayesNet.emplaceDiscrete(mode, "4/6");
|
||||
|
||||
return bayesNet;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert a hybrid Bayes net to a hybrid Gaussian factor graph.
|
||||
*/
|
||||
inline HybridGaussianFactorGraph convertBayesNet(
|
||||
const HybridBayesNet& bayesNet, const VectorValues& measurements) {
|
||||
HybridGaussianFactorGraph fg;
|
||||
int num_measurements = bayesNet.size() - 2;
|
||||
for (int i = 0; i < num_measurements; i++) {
|
||||
auto conditional = bayesNet.atMixture(i);
|
||||
auto factor = conditional->likelihood({{Z(i), measurements.at(Z(i))}});
|
||||
fg.push_back(factor);
|
||||
const size_t nrModes = manyModes ? num_measurements : 1;
|
||||
for (int i = 0; i < nrModes; i++) {
|
||||
bayesNet.emplaceDiscrete(DiscreteKey{M(i), 2}, "4/6");
|
||||
}
|
||||
fg.push_back(bayesNet.atGaussian(num_measurements));
|
||||
fg.push_back(bayesNet.atDiscrete(num_measurements + 1));
|
||||
return fg;
|
||||
return bayesNet;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -83,12 +71,15 @@ inline HybridGaussianFactorGraph convertBayesNet(
|
|||
*/
|
||||
inline HybridGaussianFactorGraph createHybridGaussianFactorGraph(
|
||||
int num_measurements = 1,
|
||||
boost::optional<VectorValues> measurements = boost::none) {
|
||||
auto bayesNet = createHybridBayesNet(num_measurements);
|
||||
boost::optional<VectorValues> measurements = boost::none,
|
||||
bool manyModes = false) {
|
||||
auto bayesNet = createHybridBayesNet(num_measurements, manyModes);
|
||||
if (measurements) {
|
||||
return convertBayesNet(bayesNet, *measurements);
|
||||
// Use the measurements to create a hybrid factor graph.
|
||||
return bayesNet.toFactorGraph(*measurements);
|
||||
} else {
|
||||
return convertBayesNet(bayesNet, bayesNet.sample().continuous());
|
||||
// Sample from the generative model to create a hybrid factor graph.
|
||||
return bayesNet.toFactorGraph(bayesNet.sample().continuous());
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -54,8 +54,10 @@ using namespace gtsam;
|
|||
|
||||
using gtsam::symbol_shorthand::D;
|
||||
using gtsam::symbol_shorthand::M;
|
||||
using gtsam::symbol_shorthand::N;
|
||||
using gtsam::symbol_shorthand::X;
|
||||
using gtsam::symbol_shorthand::Y;
|
||||
using gtsam::symbol_shorthand::Z;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HybridGaussianFactorGraph, Creation) {
|
||||
|
|
@ -624,30 +626,36 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) {
|
|||
num_measurements, VectorValues{{Z(0), Vector1(5.0)}});
|
||||
EXPECT_LONGS_EQUAL(3, fg.size());
|
||||
|
||||
auto sum = fg.assembleGraphTree();
|
||||
// Assemble graph tree:
|
||||
auto actual = fg.assembleGraphTree();
|
||||
|
||||
// Create expected decision tree with two factor graphs:
|
||||
|
||||
// Get mixture factor:
|
||||
auto mixture = boost::dynamic_pointer_cast<GaussianMixtureFactor>(fg.at(0));
|
||||
using GF = GaussianFactor::shared_ptr;
|
||||
CHECK(mixture);
|
||||
|
||||
// Get prior factor:
|
||||
const GF prior =
|
||||
boost::dynamic_pointer_cast<HybridGaussianFactor>(fg.at(1))->inner();
|
||||
const auto gf = boost::dynamic_pointer_cast<HybridConditional>(fg.at(1));
|
||||
CHECK(gf);
|
||||
using GF = GaussianFactor::shared_ptr;
|
||||
const GF prior = gf->asGaussian();
|
||||
CHECK(prior);
|
||||
|
||||
// Create DiscreteValues for both 0 and 1:
|
||||
DiscreteValues d0{{M(0), 0}}, d1{{M(0), 1}};
|
||||
|
||||
// Expected decision tree with two factor graphs:
|
||||
// f(x0;mode=0)P(x0) and f(x0;mode=1)P(x0)
|
||||
GaussianFactorGraphTree expectedSum{
|
||||
GaussianFactorGraphTree expected{
|
||||
M(0),
|
||||
{GaussianFactorGraph(std::vector<GF>{mixture->factor(d0), prior}),
|
||||
mixture->constant(d0)},
|
||||
{GaussianFactorGraph(std::vector<GF>{mixture->factor(d1), prior}),
|
||||
mixture->constant(d1)}};
|
||||
|
||||
EXPECT(assert_equal(expectedSum(d0), sum(d0), 1e-5));
|
||||
EXPECT(assert_equal(expectedSum(d1), sum(d1), 1e-5));
|
||||
EXPECT(assert_equal(expected(d0), actual(d0), 1e-5));
|
||||
EXPECT(assert_equal(expected(d1), actual(d1), 1e-5));
|
||||
}
|
||||
|
||||
/* ****************************************************************************/
|
||||
|
|
@ -657,6 +665,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) {
|
|||
const int num_measurements = 1;
|
||||
auto fg = tiny::createHybridGaussianFactorGraph(
|
||||
num_measurements, VectorValues{{Z(0), Vector1(5.0)}});
|
||||
EXPECT_LONGS_EQUAL(3, fg.size());
|
||||
|
||||
// Create expected Bayes Net:
|
||||
HybridBayesNet expectedBayesNet;
|
||||
|
|
@ -691,6 +700,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) {
|
|||
auto fg = tiny::createHybridGaussianFactorGraph(
|
||||
num_measurements,
|
||||
VectorValues{{Z(0), Vector1(4.0)}, {Z(1), Vector1(6.0)}});
|
||||
EXPECT_LONGS_EQUAL(4, fg.size());
|
||||
|
||||
// Create expected Bayes Net:
|
||||
HybridBayesNet expectedBayesNet;
|
||||
|
|
@ -716,6 +726,152 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) {
|
|||
EXPECT(assert_equal(expectedBayesNet, *posterior, 0.01));
|
||||
}
|
||||
|
||||
/* ****************************************************************************/
|
||||
// Test eliminating tiny net with 1 mode per measurement.
|
||||
TEST(HybridGaussianFactorGraph, EliminateTiny22) {
|
||||
// Create factor graph with 2 measurements such that posterior mean = 5.0.
|
||||
using symbol_shorthand::Z;
|
||||
const int num_measurements = 2;
|
||||
const bool manyModes = true;
|
||||
|
||||
// Create Bayes net and convert to factor graph.
|
||||
auto bn = tiny::createHybridBayesNet(num_measurements, manyModes);
|
||||
const VectorValues measurements{{Z(0), Vector1(4.0)}, {Z(1), Vector1(6.0)}};
|
||||
auto fg = bn.toFactorGraph(measurements);
|
||||
EXPECT_LONGS_EQUAL(5, fg.size());
|
||||
|
||||
// Test elimination
|
||||
Ordering ordering;
|
||||
ordering.push_back(X(0));
|
||||
ordering.push_back(M(0));
|
||||
ordering.push_back(M(1));
|
||||
const auto posterior = fg.eliminateSequential(ordering);
|
||||
|
||||
// Compute the log-ratio between the Bayes net and the factor graph.
|
||||
auto compute_ratio = [&](HybridValues *sample) -> double {
|
||||
// update sample with given measurements:
|
||||
sample->update(measurements);
|
||||
return bn.evaluate(*sample) / posterior->evaluate(*sample);
|
||||
};
|
||||
|
||||
// Set up sampling
|
||||
std::mt19937_64 rng(42);
|
||||
|
||||
// The error evaluated by the factor graph and the Bayes net should differ by
|
||||
// the normalizing term computed via the Bayes net determinant.
|
||||
HybridValues sample = bn.sample(&rng);
|
||||
double expected_ratio = compute_ratio(&sample);
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(0.018253037966018862, expected_ratio, 1e-6);
|
||||
|
||||
// 3. Do sampling
|
||||
constexpr int num_samples = 100;
|
||||
for (size_t i = 0; i < num_samples; i++) {
|
||||
// Sample from the bayes net
|
||||
HybridValues sample = bn.sample(&rng);
|
||||
|
||||
// Check that the ratio is constant.
|
||||
EXPECT_DOUBLES_EQUAL(expected_ratio, compute_ratio(&sample), 1e-6);
|
||||
}
|
||||
}
|
||||
|
||||
/* ****************************************************************************/
|
||||
// Test elimination of a switching network with one mode per measurement.
|
||||
TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
|
||||
// Create a switching network with one mode per measurement.
|
||||
HybridBayesNet bn;
|
||||
|
||||
// NOTE: we add reverse topological so we can sample from the Bayes net.:
|
||||
|
||||
// Add measurements:
|
||||
for (size_t t : {0, 1, 2}) {
|
||||
// Create Gaussian mixture on Z(t) conditioned on X(t) and mode N(t):
|
||||
const auto noise_mode_t = DiscreteKey{N(t), 2};
|
||||
GaussianMixture gm({Z(t)}, {X(t)}, {noise_mode_t},
|
||||
{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.emplaceMixture(gm); // copy :-(
|
||||
|
||||
// Create prior on discrete mode M(t):
|
||||
bn.emplaceDiscrete(noise_mode_t, "20/80");
|
||||
}
|
||||
|
||||
// Add motion models:
|
||||
for (size_t t : {2, 1}) {
|
||||
// Create Gaussian mixture on X(t) conditioned on X(t-1) and mode M(t-1):
|
||||
const auto motion_model_t = DiscreteKey{M(t), 2};
|
||||
GaussianMixture gm({X(t)}, {X(t - 1)}, {motion_model_t},
|
||||
{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)});
|
||||
bn.emplaceMixture(gm); // copy :-(
|
||||
|
||||
// Create prior on motion model M(t):
|
||||
bn.emplaceDiscrete(motion_model_t, "40/60");
|
||||
}
|
||||
|
||||
// Create Gaussian prior on continuous X(0) using sharedMeanAndStddev:
|
||||
bn.addGaussian(GaussianConditional::sharedMeanAndStddev(X(0), Z_1x1, 0.1));
|
||||
|
||||
// Make sure we an sample from the Bayes net:
|
||||
EXPECT_LONGS_EQUAL(6, bn.sample().continuous().size());
|
||||
|
||||
// Create measurements consistent with moving right every time:
|
||||
const VectorValues measurements{
|
||||
{Z(0), Vector1(0.0)}, {Z(1), Vector1(1.0)}, {Z(2), Vector1(2.0)}};
|
||||
const auto fg = bn.toFactorGraph(measurements);
|
||||
|
||||
// Create ordering that eliminates in time order, then discrete modes:
|
||||
Ordering ordering;
|
||||
ordering.push_back(X(2));
|
||||
ordering.push_back(X(1));
|
||||
ordering.push_back(X(0));
|
||||
ordering.push_back(N(0));
|
||||
ordering.push_back(N(1));
|
||||
ordering.push_back(N(2));
|
||||
ordering.push_back(M(1));
|
||||
ordering.push_back(M(2));
|
||||
|
||||
// Test elimination result has correct size:
|
||||
const auto posterior = fg.eliminateSequential(ordering);
|
||||
// GTSAM_PRINT(*posterior);
|
||||
|
||||
// Test elimination result has correct size:
|
||||
EXPECT_LONGS_EQUAL(8, posterior->size());
|
||||
|
||||
// TODO(dellaert): below is copy/pasta from above, refactor
|
||||
|
||||
// Compute the log-ratio between the Bayes net and the factor graph.
|
||||
auto compute_ratio = [&](HybridValues *sample) -> double {
|
||||
// update sample with given measurements:
|
||||
sample->update(measurements);
|
||||
return bn.evaluate(*sample) / posterior->evaluate(*sample);
|
||||
};
|
||||
|
||||
// Set up sampling
|
||||
std::mt19937_64 rng(42);
|
||||
|
||||
// The error evaluated by the factor graph and the Bayes net should differ by
|
||||
// the normalizing term computed via the Bayes net determinant.
|
||||
HybridValues sample = bn.sample(&rng);
|
||||
double expected_ratio = compute_ratio(&sample);
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(0.0094526745785019472, expected_ratio, 1e-6);
|
||||
|
||||
// 3. Do sampling
|
||||
constexpr int num_samples = 100;
|
||||
for (size_t i = 0; i < num_samples; i++) {
|
||||
// Sample from the bayes net
|
||||
HybridValues sample = bn.sample(&rng);
|
||||
|
||||
// Check that the ratio is constant.
|
||||
EXPECT_DOUBLES_EQUAL(expected_ratio, compute_ratio(&sample), 1e-6);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -263,7 +263,7 @@ double GaussianConditional::evaluate(const VectorValues& x) const {
|
|||
Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals()));
|
||||
frontalVec = R().transpose().triangularView<Eigen::Lower>().solve(frontalVec);
|
||||
|
||||
// Check for indeterminant solution
|
||||
// Check for indeterminate solution
|
||||
if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());
|
||||
|
||||
for (const_iterator it = beginParents(); it!= endParents(); it++)
|
||||
|
|
|
|||
|
|
@ -100,6 +100,12 @@ namespace gtsam {
|
|||
const Matrix& A2, Key parent2,
|
||||
const Vector& b, double sigma);
|
||||
|
||||
/// Create shared pointer by forwarding arguments to fromMeanAndStddev.
|
||||
template<typename... Args>
|
||||
static shared_ptr sharedMeanAndStddev(Args&&... args) {
|
||||
return boost::make_shared<This>(FromMeanAndStddev(std::forward<Args>(args)...));
|
||||
}
|
||||
|
||||
/** Combine several GaussianConditional into a single dense GC. The conditionals enumerated by
|
||||
* \c first and \c last must be in increasing order, meaning that the parents of any
|
||||
* conditional may not include a conditional coming before it.
|
||||
|
|
|
|||
Loading…
Reference in New Issue