DiscretePriorFactor.h

Go to the documentation of this file.

 
#pragma once
 
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/discrete/DiscreteKey.h>
 
#include <vector>
 
namespace dcsam {
 
class DiscretePriorFactor : public gtsam::DiscreteFactor {
 protected:
  gtsam::DiscreteKey dk_;
  std::vector<double> probs_;
 
 public:
  using Base = gtsam::DiscreteFactor;
 
  DiscretePriorFactor() = default;
 
  DiscretePriorFactor(const gtsam::DiscreteKey& dk,
                      const std::vector<double> probs)
      : dk_(dk), probs_(probs) {
    // Ensure that length of probs is equal to the cardinality of the discrete
    // variable (for gtsam::DiscreteKey dk, dk.second is the cardinality).
    assert(probs.size() == dk.second);
 
    // For gtsam::DiscreteKey dk, dk.first is the Key for this variable.
    keys_.push_back(dk.first);
  }
 
  bool equals(const DiscreteFactor& other, double tol = 1e-9) const override {
    if (!dynamic_cast<const DiscretePriorFactor*>(&other)) return false;
    const DiscretePriorFactor& f(
        static_cast<const DiscretePriorFactor&>(other));
    if (probs_.size() != f.probs_.size() || (dk_ != f.dk_)) {
      return false;
    } else {
      for (size_t i = 0; i < probs_.size(); i++)
        if (abs(probs_[i] - f.probs_[i]) > tol) return false;
      return true;
    }
  }
 
  DiscretePriorFactor& operator=(const DiscretePriorFactor& rhs) {
    Base::operator=(rhs);
    dk_ = rhs.dk_;
    probs_ = rhs.probs_;
    return *this;
  }
 
  gtsam::DecisionTreeFactor toDecisionTreeFactor() const override {
    gtsam::DecisionTreeFactor converted(dk_, probs_);
    return converted;
  }
 
  gtsam::DecisionTreeFactor operator*(
      const gtsam::DecisionTreeFactor& f) const override {
    return toDecisionTreeFactor() * f;
  }
 
  double operator()(const DiscreteValues& values) const override {
    size_t assignment = values.at(dk_.first);
    return probs_[assignment];
  }
 
  std::string markdown(const gtsam::KeyFormatter& keyFormatter,
                       const Names& names) const override {
    return toDecisionTreeFactor().markdown(keyFormatter, names);
  }
 
  std::string html(const gtsam::KeyFormatter& keyFormatter,
                   const Names& names) const override {
    return toDecisionTreeFactor().markdown(keyFormatter, names);
  }
};
 
}  // namespace dcsam