MixtureFactor.h

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

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

#pragma once

#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Symbol.h>

#include <algorithm>
#include <cmath>
#include <limits>
#include <vector>

namespace gtsam {

class MixtureFactor : public HybridFactor {
 public:
  using Base = HybridFactor;
  using This = MixtureFactor;
  using shared_ptr = std::shared_ptr<MixtureFactor>;
  using sharedFactor = std::shared_ptr<NonlinearFactor>;

  using Factors = DecisionTree<Key, sharedFactor>;

 private:
  Factors factors_;
  bool normalized_;

 public:
  MixtureFactor() = default;

  MixtureFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
                const Factors& factors, bool normalized = false)
      : Base(keys, discreteKeys), factors_(factors), normalized_(normalized) {}

  template <typename FACTOR>
  MixtureFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
                const std::vector<std::shared_ptr<FACTOR>>& factors,
                bool normalized = false)
      : Base(keys, discreteKeys), normalized_(normalized) {
    std::vector<NonlinearFactor::shared_ptr> nonlinear_factors;
    KeySet continuous_keys_set(keys.begin(), keys.end());
    KeySet factor_keys_set;
    for (auto&& f : factors) {
      // Insert all factor continuous keys in the continuous keys set.
      std::copy(f->keys().begin(), f->keys().end(),
                std::inserter(factor_keys_set, factor_keys_set.end()));

      if (auto nf = std::dynamic_pointer_cast<NonlinearFactor>(f)) {
        nonlinear_factors.push_back(nf);
      } else {
        throw std::runtime_error(
            "Factors passed into MixtureFactor need to be nonlinear!");
      }
    }
    factors_ = Factors(discreteKeys, nonlinear_factors);

    if (continuous_keys_set != factor_keys_set) {
      throw std::runtime_error(
          "The specified continuous keys and the keys in the factors don't "
          "match!");
    }
  }

  AlgebraicDecisionTree<Key> error(const Values& continuousValues) const {
    // functor to convert from sharedFactor to double error value.
    auto errorFunc = [continuousValues](const sharedFactor& factor) {
      return factor->error(continuousValues);
    };
    DecisionTree<Key, double> errorTree(factors_, errorFunc);
    return errorTree;
  }

  double error(const Values& continuousValues,
               const DiscreteValues& discreteValues) const {
    // Retrieve the factor corresponding to the assignment in discreteValues.
    auto factor = factors_(discreteValues);
    // Compute the error for the selected factor
    const double factorError = factor->error(continuousValues);
    if (normalized_) return factorError;
    return factorError + this->nonlinearFactorLogNormalizingConstant(
                             factor, continuousValues);
  }

  double error(const HybridValues& values) const override {
    return error(values.nonlinear(), values.discrete());
  }

  size_t dim() const {
    const auto assignments = DiscreteValues::CartesianProduct(discreteKeys_);
    auto factor = factors_(assignments.at(0));
    return factor->dim();
  }


  void print(
      const std::string& s = "",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    std::cout << (s.empty() ? "" : s + " ");
    Base::print("", keyFormatter);
    std::cout << "\nMixtureFactor\n";
    auto valueFormatter = [](const sharedFactor& v) {
      if (v) {
        return "Nonlinear factor on " + std::to_string(v->size()) + " keys";
      } else {
        return std::string("nullptr");
      }
    };
    factors_.print("", keyFormatter, valueFormatter);
  }

  bool equals(const HybridFactor& other, double tol = 1e-9) const override {
    // We attempt a dynamic cast from HybridFactor to MixtureFactor. If it
    // fails, return false.
    if (!dynamic_cast<const MixtureFactor*>(&other)) return false;

    // If the cast is successful, we'll properly construct a MixtureFactor
    // object from `other`
    const MixtureFactor& f(static_cast<const MixtureFactor&>(other));

    // Ensure that this MixtureFactor and `f` have the same `factors_`.
    auto compare = [tol](const sharedFactor& a, const sharedFactor& b) {
      return traits<NonlinearFactor>::Equals(*a, *b, tol);
    };
    if (!factors_.equals(f.factors_, compare)) return false;

    // If everything above passes, and the keys_, discreteKeys_ and normalized_
    // member variables are identical, return true.
    return (std::equal(keys_.begin(), keys_.end(), f.keys().begin()) &&
            (discreteKeys_ == f.discreteKeys_) &&
            (normalized_ == f.normalized_));
  }


  GaussianFactor::shared_ptr linearize(
      const Values& continuousValues,
      const DiscreteValues& discreteValues) const {
    auto factor = factors_(discreteValues);
    return factor->linearize(continuousValues);
  }

  std::shared_ptr<GaussianMixtureFactor> linearize(
      const Values& continuousValues) const {
    // functional to linearize each factor in the decision tree
    auto linearizeDT = [continuousValues](const sharedFactor& factor) {
      return factor->linearize(continuousValues);
    };

    DecisionTree<Key, GaussianFactor::shared_ptr> linearized_factors(
        factors_, linearizeDT);

    return std::make_shared<GaussianMixtureFactor>(
        continuousKeys_, discreteKeys_, linearized_factors);
  }

  double nonlinearFactorLogNormalizingConstant(const sharedFactor& factor,
                                               const Values& values) const {
    // Information matrix (inverse covariance matrix) for the factor.
    Matrix infoMat;

    // If this is a NoiseModelFactor, we'll use its noiseModel to
    // otherwise noiseModelFactor will be nullptr
    if (auto noiseModelFactor =
            std::dynamic_pointer_cast<NoiseModelFactor>(factor)) {
      // If dynamic cast to NoiseModelFactor succeeded, see if the noise model
      // is Gaussian
      auto noiseModel = noiseModelFactor->noiseModel();

      auto gaussianNoiseModel =
          std::dynamic_pointer_cast<noiseModel::Gaussian>(noiseModel);
      if (gaussianNoiseModel) {
        // If the noise model is Gaussian, retrieve the information matrix
        infoMat = gaussianNoiseModel->information();
      } else {
        // If the factor is not a Gaussian factor, we'll linearize it to get
        // something with a normalized noise model
        // TODO(kevin): does this make sense to do? I think maybe not in
        // general? Should we just yell at the user?
        auto gaussianFactor = factor->linearize(values);
        infoMat = gaussianFactor->information();
      }
    }

    // Compute the (negative) log of the normalizing constant
    return -(factor->dim() * log(2.0 * M_PI) / 2.0) -
           (log(infoMat.determinant()) / 2.0);
  }
};

}  // namespace gtsam