TinyHybridExample.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010-2023, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 *  @file TinyHybridExample.h
 *  @date December, 2022
 *  @author Frank Dellaert
 */

#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#pragma once

namespace gtsam {
namespace tiny {

using symbol_shorthand::M;
using symbol_shorthand::X;
using symbol_shorthand::Z;

// Create mode key: 0 is low-noise, 1 is high-noise.
const DiscreteKey mode{M(0), 2};

inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1,
                                           bool manyModes = false) {
  HybridBayesNet bayesNet;

  // Create Gaussian mixture z_i = x0 + noise for each measurement.
  for (size_t i = 0; i < num_measurements; i++) {
    const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode;
    bayesNet.emplace_back(
        new GaussianMixture({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)}));
  }

  // Create prior on X(0).
  bayesNet.push_back(
      GaussianConditional::sharedMeanAndStddev(X(0), Vector1(5.0), 0.5));

  // Add prior on mode.
  const size_t nrModes = manyModes ? num_measurements : 1;
  for (size_t i = 0; i < nrModes; i++) {
    bayesNet.emplace_back(new DiscreteConditional({M(i), 2}, "4/6"));
  }
  return bayesNet;
}

inline HybridGaussianFactorGraph createHybridGaussianFactorGraph(
    size_t num_measurements = 1,
    std::optional<VectorValues> measurements = {},
    bool manyModes = false) {
  auto bayesNet = createHybridBayesNet(num_measurements, manyModes);
  if (measurements) {
    // Use the measurements to create a hybrid factor graph.
    return bayesNet.toFactorGraph(*measurements);
  } else {
    // Sample from the generative model to create a hybrid factor graph.
    return bayesNet.toFactorGraph(bayesNet.sample().continuous());
  }
}

}  // namespace tiny
}  // namespace gtsam