DCFactor.h

Go to the documentation of this file.

 
#pragma once
 
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Symbol.h>
#include <math.h>
 
#include <algorithm>
#include <limits>
#include <string>
#include <vector>
 
#include "dcsam/DCSAM_types.h"
#include "dcsam/DCSAM_utils.h"
 
namespace dcsam {
 
class DCFactor : public gtsam::Factor {
 protected:
  // Set of DiscreteKeys for this factor.
  gtsam::DiscreteKeys discreteKeys_;
 
 public:
  using Base = gtsam::Factor;
 
  DCFactor() = default;
 
  DCFactor(const gtsam::KeyVector& continuousKeys,
           const gtsam::DiscreteKeys& discreteKeys)
      : Base(continuousKeys), discreteKeys_(discreteKeys) {}
 
  // NOTE unsure if needed?
  explicit DCFactor(const gtsam::DiscreteKeys& discreteKeys)
      : discreteKeys_(discreteKeys) {}
 
  DCFactor& operator=(const DCFactor& rhs) {
    Base::operator=(rhs);
    discreteKeys_ = rhs.discreteKeys_;
    return *this;
  }
 
  virtual ~DCFactor() = default;
 
  virtual double error(
      const gtsam::Values& continuousVals,
      const gtsam::DiscreteFactor::Values& discreteVals) const = 0;
 
  virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(
      const gtsam::Values& continuousVals,
      const DiscreteValues& discreteVals) const = 0;
 
  virtual bool equals(const DCFactor& other, double tol = 1e-9) const = 0;
 
  virtual size_t dim() const = 0;
 
  /*
   * Return the discrete keys for this factor.
   */
  gtsam::DiscreteKeys discreteKeys() const { return discreteKeys_; }
 
  virtual gtsam::DecisionTreeFactor toDecisionTreeFactor(
      const gtsam::Values& continuousVals,
      const DiscreteValues& discreteVals) const {
    gtsam::DecisionTreeFactor converted;
    for (const gtsam::DiscreteKey& dkey : discreteKeys_) {
      std::vector<double> probs = evalProbs(dkey, continuousVals);
      // Cardinality of gtsam::DiscreteKey is located at `second`
      assert(probs.size() == dkey.second);
      gtsam::DecisionTreeFactor unary(dkey, probs);
      converted = converted * unary;
    }
    return converted;
  }
 
  virtual double logNormalizingConstant(const gtsam::Values& values) const {
    throw std::logic_error(
        "Normalizing constant not implemented."
        "One or more of the factors in use requires access to the normalization"
        "constant for a child class of DCFactor, but`logNormalizingConstant` "
        "has not been overridden.");
  }
 
  template <typename NonlinearFactorType>
  double nonlinearFactorLogNormalizingConstant(
      const NonlinearFactorType& factor, const gtsam::Values& values) const {
    // Information matrix (inverse covariance matrix) for the factor.
    gtsam::Matrix infoMat;
 
    // NOTE: This is sloppy, is there a cleaner way?
    boost::shared_ptr<NonlinearFactorType> fPtr =
        boost::make_shared<NonlinearFactorType>(factor);
    boost::shared_ptr<NonlinearFactorType> factorPtr(fPtr);
 
    // If this is a NoiseModelFactor, we'll use its noiseModel to
    // otherwise noiseModelFactor will be nullptr
    boost::shared_ptr<gtsam::NoiseModelFactor> noiseModelFactor =
        boost::dynamic_pointer_cast<gtsam::NoiseModelFactor>(factorPtr);
    if (noiseModelFactor) {
      // If dynamic cast to NoiseModelFactor succeeded, see if the noise model
      // is Gaussian
      gtsam::noiseModel::Base::shared_ptr noiseModel =
          noiseModelFactor->noiseModel();
 
      boost::shared_ptr<gtsam::noiseModel::Gaussian> gaussianNoiseModel =
          boost::dynamic_pointer_cast<gtsam::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?
        boost::shared_ptr<gtsam::GaussianFactor> 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);
  }
 
  std::vector<double> evalProbs(const gtsam::DiscreteKey& dk,
                                const gtsam::Values& continuousVals) const {
    std::vector<double> logProbs;
    for (size_t i = 0; i < dk.second; i++) {
      DiscreteValues testDiscreteVals;
      testDiscreteVals[dk.first] = i;
      // Recall: `error` returns -log(prob), so we compute exp(-error) to
      // recover probability
      double logProb = -error(continuousVals, testDiscreteVals);
      logProbs.push_back(logProb);
    }
    return expNormalize(logProbs);
  }
 
  gtsam::DecisionTreeFactor conditionalTimes(
      const gtsam::DecisionTreeFactor& f, const gtsam::Values& continuousVals,
      const DiscreteValues& discreteVals) const {
    return toDecisionTreeFactor(continuousVals, discreteVals) * f;
  }
};
}  // namespace dcsam