101 lines
3.1 KiB
C++
101 lines
3.1 KiB
C++
/*
|
|
* BinaryAllDiff.h
|
|
* @brief Binary "all-different" constraint
|
|
* @date Feb 6, 2012
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam_unstable/discrete/Domain.h>
|
|
#include <gtsam_unstable/discrete/Constraint.h>
|
|
#include <gtsam/discrete/DecisionTreeFactor.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* Binary AllDiff constraint
|
|
* Returns 1 if values for two keys are different, 0 otherwise
|
|
* DiscreteFactors are all awkward in that they have to store two types of keys:
|
|
* for each variable we have a Index and an Index. In this factor, we
|
|
* keep the Indices locally, and the Indices are stored in IndexFactor.
|
|
*/
|
|
class BinaryAllDiff: public Constraint {
|
|
|
|
size_t cardinality0_, cardinality1_; /// cardinality
|
|
|
|
public:
|
|
|
|
/// Constructor
|
|
BinaryAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) :
|
|
Constraint(key1.first, key2.first),
|
|
cardinality0_(key1.second), cardinality1_(key2.second) {
|
|
}
|
|
|
|
// print
|
|
virtual void print(const std::string& s = "",
|
|
const KeyFormatter& formatter = DefaultKeyFormatter) const {
|
|
std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and "
|
|
<< formatter(keys_[1]) << std::endl;
|
|
}
|
|
|
|
/// equals
|
|
bool equals(const DiscreteFactor& other, double tol) const {
|
|
if(!dynamic_cast<const BinaryAllDiff*>(&other))
|
|
return false;
|
|
else {
|
|
const BinaryAllDiff& f(static_cast<const BinaryAllDiff&>(other));
|
|
return (cardinality0_==f.cardinality0_) && (cardinality1_==f.cardinality1_);
|
|
}
|
|
}
|
|
|
|
/// Calculate value
|
|
virtual double operator()(const Values& values) const {
|
|
return (double) (values.at(keys_[0]) != values.at(keys_[1]));
|
|
}
|
|
|
|
/// Convert into a decisiontree
|
|
virtual DecisionTreeFactor toDecisionTreeFactor() const {
|
|
DiscreteKeys keys;
|
|
keys.push_back(DiscreteKey(keys_[0],cardinality0_));
|
|
keys.push_back(DiscreteKey(keys_[1],cardinality1_));
|
|
std::vector<double> table;
|
|
for (size_t i1 = 0; i1 < cardinality0_; i1++)
|
|
for (size_t i2 = 0; i2 < cardinality1_; i2++)
|
|
table.push_back(i1 != i2);
|
|
DecisionTreeFactor converted(keys, table);
|
|
return converted;
|
|
}
|
|
|
|
/// Multiply into a decisiontree
|
|
virtual DecisionTreeFactor operator*(const DecisionTreeFactor& f) const {
|
|
// TODO: can we do this more efficiently?
|
|
return toDecisionTreeFactor() * f;
|
|
}
|
|
|
|
/*
|
|
* Ensure Arc-consistency
|
|
* @param j domain to be checked
|
|
* @param domains all other domains
|
|
*/
|
|
///
|
|
bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const {
|
|
// throw std::runtime_error(
|
|
// "BinaryAllDiff::ensureArcConsistency not implemented");
|
|
return false;
|
|
}
|
|
|
|
/// Partially apply known values
|
|
virtual Constraint::shared_ptr partiallyApply(const Values&) const {
|
|
throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented");
|
|
}
|
|
|
|
/// Partially apply known values, domain version
|
|
virtual Constraint::shared_ptr partiallyApply(
|
|
const std::vector<Domain>&) const {
|
|
throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented");
|
|
}
|
|
};
|
|
|
|
} // namespace gtsam
|