HybridGaussianConditional.cpp

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, 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
 
 * -------------------------------------------------------------------------- */
 
#include <gtsam/base/utilities.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/inference/Conditional-inst.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
 
#include <cstddef>
#include <memory>
 
namespace gtsam {
 
/* *******************************************************************************/
GaussianConditional::shared_ptr checkConditional(
    const GaussianFactor::shared_ptr &factor) {
  if (auto conditional =
          std::dynamic_pointer_cast<GaussianConditional>(factor)) {
    return conditional;
  } else {
    throw std::logic_error(
        "A HybridGaussianConditional unexpectedly contained a non-conditional");
  }
}
 
/* *******************************************************************************/
struct HybridGaussianConditional::Helper {
  FactorValuePairs pairs;
  std::optional<size_t> nrFrontals = {};
  double minNegLogConstant = std::numeric_limits<double>::infinity();
 
  using GC = GaussianConditional;
  using P = std::vector<std::pair<Vector, double>>;
 
  template <typename... Args>
  explicit Helper(const DiscreteKey &mode, const P &p, Args &&...args) {
    nrFrontals = 1;
    std::vector<GaussianFactorValuePair> fvs;
    std::vector<GC::shared_ptr> gcs;
    fvs.reserve(p.size());
    gcs.reserve(p.size());
    for (auto &&[mean, sigma] : p) {
      auto gaussianConditional =
          GC::sharedMeanAndStddev(std::forward<Args>(args)..., mean, sigma);
      double value = gaussianConditional->negLogConstant();
      minNegLogConstant = std::min(minNegLogConstant, value);
      fvs.emplace_back(gaussianConditional, value);
      gcs.push_back(gaussianConditional);
    }
 
    pairs = FactorValuePairs({mode}, fvs);
  }
 
  explicit Helper(const Conditionals &conditionals) {
    auto func = [this](const GC::shared_ptr &gc) -> GaussianFactorValuePair {
      if (!gc) return {nullptr, std::numeric_limits<double>::infinity()};
      if (!nrFrontals) nrFrontals = gc->nrFrontals();
      double value = gc->negLogConstant();
      minNegLogConstant = std::min(minNegLogConstant, value);
      return {gc, value};
    };
    pairs = FactorValuePairs(conditionals, func);
    if (!nrFrontals.has_value()) {
      throw std::runtime_error(
          "HybridGaussianConditional: need at least one frontal variable. "
          "Provided conditionals do not contain any frontal variables.");
    }
  }
 
  explicit Helper(const FactorValuePairs &pairs) : pairs(pairs) {
    auto func = [this](const GaussianFactorValuePair &pair) {
      if (!pair.first) return;
      auto gc = checkConditional(pair.first);
      if (!nrFrontals) nrFrontals = gc->nrFrontals();
      minNegLogConstant = std::min(minNegLogConstant, pair.second);
    };
    pairs.visit(func);
    if (!nrFrontals.has_value()) {
      throw std::runtime_error(
          "HybridGaussianConditional: need at least one frontal variable. "
          "Provided conditionals do not contain any frontal variables.");
    }
  }
};
 
/* *******************************************************************************/
HybridGaussianConditional::HybridGaussianConditional(
    const DiscreteKeys &discreteParents, Helper &&helper, bool pruned)
    : BaseFactor(discreteParents,
                 FactorValuePairs(
                     [&](const GaussianFactorValuePair
                             &pair) {  // subtract minNegLogConstant
                       return GaussianFactorValuePair{
                           pair.first, pair.second - helper.minNegLogConstant};
                     },
                     std::move(helper.pairs))),
      BaseConditional(*helper.nrFrontals),
      negLogConstant_(helper.minNegLogConstant),
      pruned_(pruned) {}
 
HybridGaussianConditional::HybridGaussianConditional(
    const DiscreteKey &discreteParent,
    const std::vector<GaussianConditional::shared_ptr> &conditionals)
    : HybridGaussianConditional(DiscreteKeys{discreteParent},
                                Conditionals({discreteParent}, conditionals)) {}
 
HybridGaussianConditional::HybridGaussianConditional(
    const DiscreteKey &discreteParent, Key key,  //
    const std::vector<std::pair<Vector, double>> &parameters)
    : HybridGaussianConditional(DiscreteKeys{discreteParent},
                                Helper(discreteParent, parameters, key)) {}
 
HybridGaussianConditional::HybridGaussianConditional(
    const DiscreteKey &discreteParent, Key key,  //
    const Matrix &A, Key parent,
    const std::vector<std::pair<Vector, double>> &parameters)
    : HybridGaussianConditional(
          DiscreteKeys{discreteParent},
          Helper(discreteParent, parameters, key, A, parent)) {}
 
HybridGaussianConditional::HybridGaussianConditional(
    const DiscreteKey &discreteParent, Key key,  //
    const Matrix &A1, Key parent1, const Matrix &A2, Key parent2,
    const std::vector<std::pair<Vector, double>> &parameters)
    : HybridGaussianConditional(
          DiscreteKeys{discreteParent},
          Helper(discreteParent, parameters, key, A1, parent1, A2, parent2)) {}
 
HybridGaussianConditional::HybridGaussianConditional(
    const DiscreteKeys &discreteParents,
    const HybridGaussianConditional::Conditionals &conditionals)
    : HybridGaussianConditional(discreteParents, Helper(conditionals)) {}
 
HybridGaussianConditional::HybridGaussianConditional(
    const DiscreteKeys &discreteParents, const FactorValuePairs &pairs,
    bool pruned)
    : HybridGaussianConditional(discreteParents, Helper(pairs), pruned) {}
 
/* *******************************************************************************/
const HybridGaussianConditional::Conditionals
HybridGaussianConditional::conditionals() const {
  return Conditionals(factors(), [](auto &&pair) {
    return std::dynamic_pointer_cast<GaussianConditional>(pair.first);
  });
}
 
/* *******************************************************************************/
size_t HybridGaussianConditional::nrComponents() const {
  size_t total = 0;
  factors().visit([&total](auto &&node) {
    if (node.first) total += 1;
  });
  return total;
}
 
/* *******************************************************************************/
GaussianConditional::shared_ptr HybridGaussianConditional::choose(
    const DiscreteValues &discreteValues) const {
  try {
    auto &[factor, _] = factors()(discreteValues);
    if (!factor) return nullptr;
 
    auto conditional = checkConditional(factor);
    return conditional;
  } catch (const std::out_of_range &e) {
    GTSAM_PRINT(*this);
    GTSAM_PRINT(discreteValues);
    throw std::runtime_error(
        "HybridGaussianConditional::choose: discreteValues does not contain "
        "all discrete parents.");
  }
}
 
/* *******************************************************************************/
bool HybridGaussianConditional::equals(const HybridFactor &lf,
                                       double tol) const {
  const This *e = dynamic_cast<const This *>(&lf);
  if (e == nullptr) return false;
 
  // Factors existence and scalar values are checked in BaseFactor::equals.
  // Here we check additionally that the factors *are* conditionals
  // and are equal.
  auto compareFunc = [tol](const GaussianFactorValuePair &pair1,
                           const GaussianFactorValuePair &pair2) {
    auto c1 = std::dynamic_pointer_cast<GaussianConditional>(pair1.first),
         c2 = std::dynamic_pointer_cast<GaussianConditional>(pair2.first);
    return (!c1 && !c2) || (c1 && c2 && c1->equals(*c2, tol));
  };
  return Base::equals(*e, tol) && factors().equals(e->factors(), compareFunc);
}
 
/* *******************************************************************************/
void HybridGaussianConditional::print(const std::string &s,
                                      const KeyFormatter &formatter) const {
  std::cout << (s.empty() ? "" : s + "\n");
  BaseConditional::print("", formatter);
  std::cout << " Discrete Keys = ";
  for (auto &dk : discreteKeys()) {
    std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), ";
  }
  std::cout << std::endl
            << " logNormalizationConstant: " << -negLogConstant() << std::endl
            << std::endl;
  factors().print(
      "", [&](Key k) { return formatter(k); },
      [&](const GaussianFactorValuePair &pair) -> std::string {
        RedirectCout rd;
        if (auto gf =
                std::dynamic_pointer_cast<GaussianConditional>(pair.first)) {
          gf->print("", formatter);
          return rd.str();
        } else {
          return "nullptr";
        }
      });
}
 
/* ************************************************************************* */
KeyVector HybridGaussianConditional::continuousParents() const {
  // Get all parent keys:
  const auto range = parents();
  KeyVector continuousParentKeys(range.begin(), range.end());
  // Loop over all discrete keys:
  for (const auto &discreteKey : discreteKeys()) {
    const Key key = discreteKey.first;
    // remove that key from continuousParentKeys:
    continuousParentKeys.erase(std::remove(continuousParentKeys.begin(),
                                           continuousParentKeys.end(), key),
                               continuousParentKeys.end());
  }
  return continuousParentKeys;
}
 
/* ************************************************************************* */
bool HybridGaussianConditional::allFrontalsGiven(
    const VectorValues &given) const {
  for (auto &&kv : given) {
    if (given.find(kv.first) == given.end()) {
      return false;
    }
  }
  return true;
}
 
/* ************************************************************************* */
std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
    const VectorValues &given) const {
  if (!allFrontalsGiven(given)) {
    throw std::runtime_error(
        "HybridGaussianConditional::likelihood: given values are missing some "
        "frontals.");
  }
 
  const DiscreteKeys discreteParentKeys = discreteKeys();
  const KeyVector continuousParentKeys = continuousParents();
  const HybridGaussianFactor::FactorValuePairs likelihoods(
      factors(),
      [&](const GaussianFactorValuePair &pair) -> GaussianFactorValuePair {
        if (auto conditional =
                std::dynamic_pointer_cast<GaussianConditional>(pair.first)) {
          const auto likelihood_m = conditional->likelihood(given);
          // pair.second == conditional->negLogConstant() - negLogConstant_
          return {likelihood_m, pair.second};
        } else {
          return {nullptr, std::numeric_limits<double>::infinity()};
        }
      });
  return std::make_shared<HybridGaussianFactor>(discreteParentKeys,
                                                likelihoods);
}
 
/* ************************************************************************* */
std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &discreteKeys) {
  std::set<DiscreteKey> s(discreteKeys.begin(), discreteKeys.end());
  return s;
}
 
/* *******************************************************************************/
HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune(
    const DiscreteConditional &discreteProbs) const {
  // Find keys in discreteProbs.keys() but not in this->keys():
  std::set<Key> mine(this->keys().begin(), this->keys().end());
  std::set<Key> theirs(discreteProbs.keys().begin(),
                       discreteProbs.keys().end());
  std::vector<Key> diff;
  std::set_difference(theirs.begin(), theirs.end(), mine.begin(), mine.end(),
                      std::back_inserter(diff));
 
  // Find maximum probability value for every combination of *our* keys.
  Ordering ordering(diff);
  auto max = discreteProbs.max(ordering);
 
  // Check the max value for every combination of our keys.
  // If the max value is 0.0, we can prune the corresponding conditional.
  bool allPruned = true;
  auto pruner =
      [&](const Assignment<Key> &choices,
          const GaussianFactorValuePair &pair) -> GaussianFactorValuePair {
    // If this choice is zero probability or Gaussian is null, return infinity
    if (!pair.first || max->evaluate(choices) == 0.0) {
      return {nullptr, std::numeric_limits<double>::infinity()};
    } else {
      allPruned = false;
      // Add negLogConstant_ back so that the minimum negLogConstant in the
      // HybridGaussianConditional is set correctly.
      return {pair.first, pair.second + negLogConstant_};
    }
  };
 
  FactorValuePairs prunedConditionals = factors().apply(pruner);
  if (allPruned) return nullptr;
  return std::make_shared<HybridGaussianConditional>(discreteKeys(),
                                                     prunedConditionals, true);
}
 
/* *******************************************************************************/
double HybridGaussianConditional::logProbability(
    const HybridValues &values) const {
  auto [factor, _] = factors()(values.discrete());
  auto conditional = checkConditional(factor);
  return conditional->logProbability(values.continuous());
}
 
/* *******************************************************************************/
double HybridGaussianConditional::evaluate(const HybridValues &values) const {
  auto [factor, _] = factors()(values.discrete());
  auto conditional = checkConditional(factor);
  return conditional->evaluate(values.continuous());
}
 
/* ************************************************************************ */
std::shared_ptr<Factor> HybridGaussianConditional::restrict(
    const DiscreteValues &assignment) const {
  throw std::runtime_error(
      "HybridGaussianConditional::restrict not implemented");
}
 
/* ************************************************************************ */
}  // namespace gtsam