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 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;
    bayesNet.emplace_shared<HybridGaussianConditional>(mode_i, Z(i), I_1x1,
                                                       X(0), measurementModels);
  }
 
  // 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_shared<DiscreteConditional>(DiscreteKey{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