Formatting only
Frank Dellaert
parent
dd50975668
commit
b7f43906bc
|
|
@ -5,115 +5,112 @@
|
|||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/discrete/Domain.h>
|
||||
#include <gtsam_unstable/discrete/AllDiff.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam_unstable/discrete/AllDiff.h>
|
||||
#include <gtsam_unstable/discrete/Domain.h>
|
||||
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
AllDiff::AllDiff(const DiscreteKeys& dkeys) :
|
||||
Constraint(dkeys.indices()) {
|
||||
for(const DiscreteKey& dkey: dkeys)
|
||||
cardinalities_.insert(dkey);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
AllDiff::AllDiff(const DiscreteKeys& dkeys) : Constraint(dkeys.indices()) {
|
||||
for (const DiscreteKey& dkey : dkeys) cardinalities_.insert(dkey);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void AllDiff::print(const std::string& s,
|
||||
const KeyFormatter& formatter) const {
|
||||
std::cout << s << "AllDiff on ";
|
||||
for (Key dkey: keys_)
|
||||
std::cout << formatter(dkey) << " ";
|
||||
std::cout << std::endl;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
void AllDiff::print(const std::string& s, const KeyFormatter& formatter) const {
|
||||
std::cout << s << "AllDiff on ";
|
||||
for (Key dkey : keys_) std::cout << formatter(dkey) << " ";
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double AllDiff::operator()(const Values& values) const {
|
||||
std::set < size_t > taken; // record values taken by keys
|
||||
for(Key dkey: keys_) {
|
||||
size_t value = values.at(dkey); // get the value for that key
|
||||
if (taken.count(value)) return 0.0;// check if value alreday taken
|
||||
taken.insert(value);// if not, record it as taken and keep checking
|
||||
/* ************************************************************************* */
|
||||
double AllDiff::operator()(const Values& values) const {
|
||||
std::set<size_t> taken; // record values taken by keys
|
||||
for (Key dkey : keys_) {
|
||||
size_t value = values.at(dkey); // get the value for that key
|
||||
if (taken.count(value)) return 0.0; // check if value alreday taken
|
||||
taken.insert(value); // if not, record it as taken and keep checking
|
||||
}
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
DecisionTreeFactor AllDiff::toDecisionTreeFactor() const {
|
||||
// We will do this by converting the allDif into many BinaryAllDiff
|
||||
// constraints
|
||||
DecisionTreeFactor converted;
|
||||
size_t nrKeys = keys_.size();
|
||||
for (size_t i1 = 0; i1 < nrKeys; i1++)
|
||||
for (size_t i2 = i1 + 1; i2 < nrKeys; i2++) {
|
||||
BinaryAllDiff binary12(discreteKey(i1), discreteKey(i2));
|
||||
converted = converted * binary12.toDecisionTreeFactor();
|
||||
}
|
||||
return 1.0;
|
||||
return converted;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const {
|
||||
// TODO: can we do this more efficiently?
|
||||
return toDecisionTreeFactor() * f;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool AllDiff::ensureArcConsistency(size_t j,
|
||||
std::vector<Domain>* domains) const {
|
||||
// We are changing the domain of variable j.
|
||||
// TODO(dellaert): confusing, I thought we were changing others...
|
||||
Domain& Dj = domains->at(j);
|
||||
|
||||
// Though strictly not part of allDiff, we check for
|
||||
// a value in domains[j] that does not occur in any other connected domain.
|
||||
// If found, we make this a singleton...
|
||||
// TODO: make a new constraint where this really is true
|
||||
boost::optional<Domain> maybeChanged = Dj.checkAllDiff(keys_, *domains);
|
||||
if (maybeChanged) {
|
||||
Dj = *maybeChanged;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
DecisionTreeFactor AllDiff::toDecisionTreeFactor() const {
|
||||
// We will do this by converting the allDif into many BinaryAllDiff constraints
|
||||
DecisionTreeFactor converted;
|
||||
size_t nrKeys = keys_.size();
|
||||
for (size_t i1 = 0; i1 < nrKeys; i1++)
|
||||
for (size_t i2 = i1 + 1; i2 < nrKeys; i2++) {
|
||||
BinaryAllDiff binary12(discreteKey(i1),discreteKey(i2));
|
||||
converted = converted * binary12.toDecisionTreeFactor();
|
||||
}
|
||||
return converted;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const {
|
||||
// TODO: can we do this more efficiently?
|
||||
return toDecisionTreeFactor() * f;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool AllDiff::ensureArcConsistency(size_t j,
|
||||
std::vector<Domain>* domains) const {
|
||||
// We are changing the domain of variable j.
|
||||
// TODO(dellaert): confusing, I thought we were changing others...
|
||||
Domain& Dj = domains->at(j);
|
||||
|
||||
// Though strictly not part of allDiff, we check for
|
||||
// a value in domains[j] that does not occur in any other connected domain.
|
||||
// If found, we make this a singleton...
|
||||
// TODO: make a new constraint where this really is true
|
||||
boost::optional<Domain> maybeChanged = Dj.checkAllDiff(keys_, *domains);
|
||||
if (maybeChanged) {
|
||||
Dj = *maybeChanged;
|
||||
return true;
|
||||
}
|
||||
|
||||
// Check all other domains for singletons and erase corresponding values.
|
||||
// This is the same as arc-consistency on the equivalent binary constraints
|
||||
bool changed = false;
|
||||
for (Key k : keys_)
|
||||
if (k != j) {
|
||||
const Domain& Dk = domains->at(k);
|
||||
if (Dk.isSingleton()) { // check if singleton
|
||||
size_t value = Dk.firstValue();
|
||||
if (Dj.contains(value)) {
|
||||
Dj.erase(value); // erase value if true
|
||||
changed = true;
|
||||
}
|
||||
// Check all other domains for singletons and erase corresponding values.
|
||||
// This is the same as arc-consistency on the equivalent binary constraints
|
||||
bool changed = false;
|
||||
for (Key k : keys_)
|
||||
if (k != j) {
|
||||
const Domain& Dk = domains->at(k);
|
||||
if (Dk.isSingleton()) { // check if singleton
|
||||
size_t value = Dk.firstValue();
|
||||
if (Dj.contains(value)) {
|
||||
Dj.erase(value); // erase value if true
|
||||
changed = true;
|
||||
}
|
||||
}
|
||||
return changed;
|
||||
}
|
||||
}
|
||||
return changed;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const {
|
||||
DiscreteKeys newKeys;
|
||||
// loop over keys and add them only if they do not appear in values
|
||||
for(Key k: keys_)
|
||||
if (values.find(k) == values.end()) {
|
||||
newKeys.push_back(DiscreteKey(k,cardinalities_.at(k)));
|
||||
}
|
||||
return boost::make_shared<AllDiff>(newKeys);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const {
|
||||
DiscreteKeys newKeys;
|
||||
// loop over keys and add them only if they do not appear in values
|
||||
for (Key k : keys_)
|
||||
if (values.find(k) == values.end()) {
|
||||
newKeys.push_back(DiscreteKey(k, cardinalities_.at(k)));
|
||||
}
|
||||
return boost::make_shared<AllDiff>(newKeys);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Constraint::shared_ptr AllDiff::partiallyApply(
|
||||
const std::vector<Domain>& domains) const {
|
||||
DiscreteFactor::Values known;
|
||||
for(Key k: keys_) {
|
||||
const Domain& Dk = domains[k];
|
||||
if (Dk.isSingleton())
|
||||
known[k] = Dk.firstValue();
|
||||
}
|
||||
return partiallyApply(known);
|
||||
/* ************************************************************************* */
|
||||
Constraint::shared_ptr AllDiff::partiallyApply(
|
||||
const std::vector<Domain>& domains) const {
|
||||
DiscreteFactor::Values known;
|
||||
for (Key k : keys_) {
|
||||
const Domain& Dk = domains[k];
|
||||
if (Dk.isSingleton()) known[k] = Dk.firstValue();
|
||||
}
|
||||
return partiallyApply(known);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -7,70 +7,68 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/discrete/BinaryAllDiff.h>
|
||||
#include <gtsam/discrete/DiscreteKey.h>
|
||||
#include <gtsam_unstable/discrete/BinaryAllDiff.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* General AllDiff constraint.
|
||||
* Returns 1 if values for all keys are different, 0 otherwise.
|
||||
/**
|
||||
* General AllDiff constraint.
|
||||
* Returns 1 if values for all keys are different, 0 otherwise.
|
||||
*/
|
||||
class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint {
|
||||
std::map<Key, size_t> cardinalities_;
|
||||
|
||||
DiscreteKey discreteKey(size_t i) const {
|
||||
Key j = keys_[i];
|
||||
return DiscreteKey(j, cardinalities_.at(j));
|
||||
}
|
||||
|
||||
public:
|
||||
/// Construct from keys.
|
||||
AllDiff(const DiscreteKeys& dkeys);
|
||||
|
||||
// print
|
||||
void print(const std::string& s = "", const KeyFormatter& formatter =
|
||||
DefaultKeyFormatter) const override;
|
||||
|
||||
/// equals
|
||||
bool equals(const DiscreteFactor& other, double tol) const override {
|
||||
if (!dynamic_cast<const AllDiff*>(&other))
|
||||
return false;
|
||||
else {
|
||||
const AllDiff& f(static_cast<const AllDiff&>(other));
|
||||
return cardinalities_.size() == f.cardinalities_.size() &&
|
||||
std::equal(cardinalities_.begin(), cardinalities_.end(),
|
||||
f.cardinalities_.begin());
|
||||
}
|
||||
}
|
||||
|
||||
/// Calculate value = expensive !
|
||||
double operator()(const Values& values) const override;
|
||||
|
||||
/// Convert into a decisiontree, can be *very* expensive !
|
||||
DecisionTreeFactor toDecisionTreeFactor() const override;
|
||||
|
||||
/// Multiply into a decisiontree
|
||||
DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
|
||||
|
||||
/*
|
||||
* Ensure Arc-consistency
|
||||
* Arc-consistency involves creating binaryAllDiff constraints
|
||||
* In which case the combinatorial hyper-arc explosion disappears.
|
||||
* @param j domain to be checked
|
||||
* @param (in/out) domains all other domains
|
||||
*/
|
||||
class GTSAM_UNSTABLE_EXPORT AllDiff: public Constraint {
|
||||
bool ensureArcConsistency(size_t j,
|
||||
std::vector<Domain>* domains) const override;
|
||||
|
||||
std::map<Key,size_t> cardinalities_;
|
||||
/// Partially apply known values
|
||||
Constraint::shared_ptr partiallyApply(const Values&) const override;
|
||||
|
||||
DiscreteKey discreteKey(size_t i) const {
|
||||
Key j = keys_[i];
|
||||
return DiscreteKey(j,cardinalities_.at(j));
|
||||
}
|
||||
/// Partially apply known values, domain version
|
||||
Constraint::shared_ptr partiallyApply(
|
||||
const std::vector<Domain>&) const override;
|
||||
};
|
||||
|
||||
public:
|
||||
|
||||
/// Construct from keys.
|
||||
AllDiff(const DiscreteKeys& dkeys);
|
||||
|
||||
// print
|
||||
void print(const std::string& s = "",
|
||||
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
|
||||
|
||||
/// equals
|
||||
bool equals(const DiscreteFactor& other, double tol) const override {
|
||||
if(!dynamic_cast<const AllDiff*>(&other))
|
||||
return false;
|
||||
else {
|
||||
const AllDiff& f(static_cast<const AllDiff&>(other));
|
||||
return cardinalities_.size() == f.cardinalities_.size()
|
||||
&& std::equal(cardinalities_.begin(), cardinalities_.end(),
|
||||
f.cardinalities_.begin());
|
||||
}
|
||||
}
|
||||
|
||||
/// Calculate value = expensive !
|
||||
double operator()(const Values& values) const override;
|
||||
|
||||
/// Convert into a decisiontree, can be *very* expensive !
|
||||
DecisionTreeFactor toDecisionTreeFactor() const override;
|
||||
|
||||
/// Multiply into a decisiontree
|
||||
DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
|
||||
|
||||
/*
|
||||
* Ensure Arc-consistency
|
||||
* Arc-consistency involves creating binaryAllDiff constraints
|
||||
* In which case the combinatorial hyper-arc explosion disappears.
|
||||
* @param j domain to be checked
|
||||
* @param (in/out) domains all other domains
|
||||
*/
|
||||
bool ensureArcConsistency(size_t j,
|
||||
std::vector<Domain>* domains) const override;
|
||||
|
||||
/// Partially apply known values
|
||||
Constraint::shared_ptr partiallyApply(const Values&) const override;
|
||||
|
||||
/// Partially apply known values, domain version
|
||||
Constraint::shared_ptr partiallyApply(
|
||||
const std::vector<Domain>&) const override;
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -7,92 +7,91 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/discrete/Domain.h>
|
||||
#include <gtsam_unstable/discrete/Constraint.h>
|
||||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||
#include <gtsam_unstable/discrete/Constraint.h>
|
||||
#include <gtsam_unstable/discrete/Domain.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Binary AllDiff constraint
|
||||
* Returns 1 if values for two keys are different, 0 otherwise.
|
||||
*/
|
||||
class BinaryAllDiff: public Constraint {
|
||||
/**
|
||||
* Binary AllDiff constraint
|
||||
* Returns 1 if values for two keys are different, 0 otherwise.
|
||||
*/
|
||||
class BinaryAllDiff : public Constraint {
|
||||
size_t cardinality0_, cardinality1_; /// cardinality
|
||||
|
||||
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) {}
|
||||
|
||||
public:
|
||||
// print
|
||||
void print(
|
||||
const std::string& s = "",
|
||||
const KeyFormatter& formatter = DefaultKeyFormatter) const override {
|
||||
std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and "
|
||||
<< formatter(keys_[1]) << std::endl;
|
||||
}
|
||||
|
||||
/// Constructor
|
||||
BinaryAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) :
|
||||
Constraint(key1.first, key2.first),
|
||||
cardinality0_(key1.second), cardinality1_(key2.second) {
|
||||
}
|
||||
|
||||
// print
|
||||
void print(const std::string& s = "",
|
||||
const KeyFormatter& formatter = DefaultKeyFormatter) const override {
|
||||
std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and "
|
||||
<< formatter(keys_[1]) << std::endl;
|
||||
}
|
||||
|
||||
/// equals
|
||||
bool equals(const DiscreteFactor& other, double tol) const override {
|
||||
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
|
||||
double operator()(const Values& values) const override {
|
||||
return (double) (values.at(keys_[0]) != values.at(keys_[1]));
|
||||
}
|
||||
|
||||
/// Convert into a decisiontree
|
||||
DecisionTreeFactor toDecisionTreeFactor() const override {
|
||||
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
|
||||
DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override {
|
||||
// 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 override {
|
||||
throw std::runtime_error(
|
||||
"BinaryAllDiff::ensureArcConsistency not implemented");
|
||||
/// equals
|
||||
bool equals(const DiscreteFactor& other, double tol) const override {
|
||||
if (!dynamic_cast<const BinaryAllDiff*>(&other))
|
||||
return false;
|
||||
else {
|
||||
const BinaryAllDiff& f(static_cast<const BinaryAllDiff&>(other));
|
||||
return (cardinality0_ == f.cardinality0_) &&
|
||||
(cardinality1_ == f.cardinality1_);
|
||||
}
|
||||
}
|
||||
|
||||
/// Partially apply known values
|
||||
Constraint::shared_ptr partiallyApply(const Values&) const override {
|
||||
throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented");
|
||||
}
|
||||
/// Calculate value
|
||||
double operator()(const Values& values) const override {
|
||||
return (double)(values.at(keys_[0]) != values.at(keys_[1]));
|
||||
}
|
||||
|
||||
/// Partially apply known values, domain version
|
||||
Constraint::shared_ptr partiallyApply(
|
||||
const std::vector<Domain>&) const override {
|
||||
throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented");
|
||||
}
|
||||
};
|
||||
/// Convert into a decisiontree
|
||||
DecisionTreeFactor toDecisionTreeFactor() const override {
|
||||
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;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
/// Multiply into a decisiontree
|
||||
DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override {
|
||||
// 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 override {
|
||||
throw std::runtime_error(
|
||||
"BinaryAllDiff::ensureArcConsistency not implemented");
|
||||
return false;
|
||||
}
|
||||
|
||||
/// Partially apply known values
|
||||
Constraint::shared_ptr partiallyApply(const Values&) const override {
|
||||
throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented");
|
||||
}
|
||||
|
||||
/// Partially apply known values, domain version
|
||||
Constraint::shared_ptr partiallyApply(
|
||||
const std::vector<Domain>&) const override {
|
||||
throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented");
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -17,79 +17,69 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/dllexport.h>
|
||||
#include <gtsam/discrete/DiscreteFactor.h>
|
||||
#include <gtsam_unstable/dllexport.h>
|
||||
|
||||
#include <boost/assign.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class Domain;
|
||||
class Domain;
|
||||
|
||||
/**
|
||||
* Base class for constraint factors
|
||||
* Derived classes include SingleValue, BinaryAllDiff, and AllDiff.
|
||||
/**
|
||||
* Base class for constraint factors
|
||||
* Derived classes include SingleValue, BinaryAllDiff, and AllDiff.
|
||||
*/
|
||||
class GTSAM_EXPORT Constraint : public DiscreteFactor {
|
||||
public:
|
||||
typedef boost::shared_ptr<Constraint> shared_ptr;
|
||||
|
||||
protected:
|
||||
/// Construct unary constraint factor.
|
||||
Constraint(Key j) : DiscreteFactor(boost::assign::cref_list_of<1>(j)) {}
|
||||
|
||||
/// Construct binary constraint factor.
|
||||
Constraint(Key j1, Key j2)
|
||||
: DiscreteFactor(boost::assign::cref_list_of<2>(j1)(j2)) {}
|
||||
|
||||
/// Construct n-way constraint factor.
|
||||
Constraint(const KeyVector& js) : DiscreteFactor(js) {}
|
||||
|
||||
/// construct from container
|
||||
template <class KeyIterator>
|
||||
Constraint(KeyIterator beginKey, KeyIterator endKey)
|
||||
: DiscreteFactor(beginKey, endKey) {}
|
||||
|
||||
public:
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// Default constructor for I/O
|
||||
Constraint();
|
||||
|
||||
/// Virtual destructor
|
||||
~Constraint() override {}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/*
|
||||
* Ensure Arc-consistency, possibly changing domains of connected variables.
|
||||
* @param j domain to be checked
|
||||
* @param (in/out) domains all other domains
|
||||
* @return true if domains were changed, false otherwise.
|
||||
*/
|
||||
class GTSAM_EXPORT Constraint : public DiscreteFactor {
|
||||
virtual bool ensureArcConsistency(size_t j,
|
||||
std::vector<Domain>* domains) const = 0;
|
||||
|
||||
public:
|
||||
/// Partially apply known values
|
||||
virtual shared_ptr partiallyApply(const Values&) const = 0;
|
||||
|
||||
typedef boost::shared_ptr<Constraint> shared_ptr;
|
||||
|
||||
protected:
|
||||
|
||||
/// Construct unary constraint factor.
|
||||
Constraint(Key j) :
|
||||
DiscreteFactor(boost::assign::cref_list_of<1>(j)) {
|
||||
}
|
||||
|
||||
/// Construct binary constraint factor.
|
||||
Constraint(Key j1, Key j2) :
|
||||
DiscreteFactor(boost::assign::cref_list_of<2>(j1)(j2)) {
|
||||
}
|
||||
|
||||
/// Construct n-way constraint factor.
|
||||
Constraint(const KeyVector& js) :
|
||||
DiscreteFactor(js) {
|
||||
}
|
||||
|
||||
/// construct from container
|
||||
template<class KeyIterator>
|
||||
Constraint(KeyIterator beginKey, KeyIterator endKey) :
|
||||
DiscreteFactor(beginKey, endKey) {
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// Default constructor for I/O
|
||||
Constraint();
|
||||
|
||||
/// Virtual destructor
|
||||
~Constraint() override {}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/*
|
||||
* Ensure Arc-consistency, possibly changing domains of connected variables.
|
||||
* @param j domain to be checked
|
||||
* @param (in/out) domains all other domains
|
||||
* @return true if domains were changed, false otherwise.
|
||||
*/
|
||||
virtual bool ensureArcConsistency(size_t j,
|
||||
std::vector<Domain>* domains) const = 0;
|
||||
|
||||
/// Partially apply known values
|
||||
virtual shared_ptr partiallyApply(const Values&) const = 0;
|
||||
|
||||
|
||||
/// Partially apply known values, domain version
|
||||
virtual shared_ptr partiallyApply(const std::vector<Domain>&) const = 0;
|
||||
/// @}
|
||||
};
|
||||
/// Partially apply known values, domain version
|
||||
virtual shared_ptr partiallyApply(const std::vector<Domain>&) const = 0;
|
||||
/// @}
|
||||
};
|
||||
// DiscreteFactor
|
||||
|
||||
}// namespace gtsam
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -5,90 +5,88 @@
|
|||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/discrete/Domain.h>
|
||||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||
#include <gtsam_unstable/discrete/Domain.h>
|
||||
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Domain::print(const string& s,
|
||||
const KeyFormatter& formatter) const {
|
||||
cout << s << ": Domain on " << formatter(keys_[0]) << " (j=" <<
|
||||
formatter(keys_[0]) << ") with values";
|
||||
for (size_t v: values_) cout << " " << v;
|
||||
cout << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Domain::operator()(const Values& values) const {
|
||||
return contains(values.at(keys_[0]));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
DecisionTreeFactor Domain::toDecisionTreeFactor() const {
|
||||
DiscreteKeys keys;
|
||||
keys += DiscreteKey(keys_[0],cardinality_);
|
||||
vector<double> table;
|
||||
for (size_t i1 = 0; i1 < cardinality_; ++i1)
|
||||
table.push_back(contains(i1));
|
||||
DecisionTreeFactor converted(keys, table);
|
||||
return converted;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
DecisionTreeFactor Domain::operator*(const DecisionTreeFactor& f) const {
|
||||
// TODO: can we do this more efficiently?
|
||||
return toDecisionTreeFactor() * f;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Domain::ensureArcConsistency(size_t j, vector<Domain>* domains) const {
|
||||
if (j != keys_[0]) throw invalid_argument("Domain check on wrong domain");
|
||||
Domain& D = domains->at(j);
|
||||
for(size_t value: values_)
|
||||
if (!D.contains(value)) throw runtime_error("Unsatisfiable");
|
||||
D = *this;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::optional<Domain> Domain::checkAllDiff(
|
||||
const KeyVector keys, const vector<Domain>& domains) const {
|
||||
Key j = keys_[0];
|
||||
// for all values in this domain
|
||||
for (const size_t value : values_) {
|
||||
// for all connected domains
|
||||
for (const Key k : keys)
|
||||
// if any domain contains the value we cannot make this domain singleton
|
||||
if (k != j && domains[k].contains(value)) goto found;
|
||||
// Otherwise: return a singleton:
|
||||
return Domain(this->discreteKey(), value);
|
||||
found:;
|
||||
}
|
||||
return boost::none; // we did not change it
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Constraint::shared_ptr Domain::partiallyApply(
|
||||
const Values& values) const {
|
||||
Values::const_iterator it = values.find(keys_[0]);
|
||||
if (it != values.end() && !contains(it->second)) throw runtime_error(
|
||||
"Domain::partiallyApply: unsatisfiable");
|
||||
return boost::make_shared < Domain > (*this);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Constraint::shared_ptr Domain::partiallyApply(
|
||||
const vector<Domain>& domains) const {
|
||||
const Domain& Dk = domains[keys_[0]];
|
||||
if (Dk.isSingleton() && !contains(*Dk.begin())) throw runtime_error(
|
||||
"Domain::partiallyApply: unsatisfiable");
|
||||
return boost::make_shared < Domain > (Dk);
|
||||
}
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
void Domain::print(const string& s, const KeyFormatter& formatter) const {
|
||||
cout << s << ": Domain on " << formatter(keys_[0])
|
||||
<< " (j=" << formatter(keys_[0]) << ") with values";
|
||||
for (size_t v : values_) cout << " " << v;
|
||||
cout << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Domain::operator()(const Values& values) const {
|
||||
return contains(values.at(keys_[0]));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
DecisionTreeFactor Domain::toDecisionTreeFactor() const {
|
||||
DiscreteKeys keys;
|
||||
keys += DiscreteKey(keys_[0], cardinality_);
|
||||
vector<double> table;
|
||||
for (size_t i1 = 0; i1 < cardinality_; ++i1) table.push_back(contains(i1));
|
||||
DecisionTreeFactor converted(keys, table);
|
||||
return converted;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
DecisionTreeFactor Domain::operator*(const DecisionTreeFactor& f) const {
|
||||
// TODO: can we do this more efficiently?
|
||||
return toDecisionTreeFactor() * f;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Domain::ensureArcConsistency(size_t j, vector<Domain>* domains) const {
|
||||
if (j != keys_[0]) throw invalid_argument("Domain check on wrong domain");
|
||||
Domain& D = domains->at(j);
|
||||
for (size_t value : values_)
|
||||
if (!D.contains(value)) throw runtime_error("Unsatisfiable");
|
||||
D = *this;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::optional<Domain> Domain::checkAllDiff(
|
||||
const KeyVector keys, const vector<Domain>& domains) const {
|
||||
Key j = keys_[0];
|
||||
// for all values in this domain
|
||||
for (const size_t value : values_) {
|
||||
// for all connected domains
|
||||
for (const Key k : keys)
|
||||
// if any domain contains the value we cannot make this domain singleton
|
||||
if (k != j && domains[k].contains(value)) goto found;
|
||||
// Otherwise: return a singleton:
|
||||
return Domain(this->discreteKey(), value);
|
||||
found:;
|
||||
}
|
||||
return boost::none; // we did not change it
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Constraint::shared_ptr Domain::partiallyApply(const Values& values) const {
|
||||
Values::const_iterator it = values.find(keys_[0]);
|
||||
if (it != values.end() && !contains(it->second))
|
||||
throw runtime_error("Domain::partiallyApply: unsatisfiable");
|
||||
return boost::make_shared<Domain>(*this);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Constraint::shared_ptr Domain::partiallyApply(
|
||||
const vector<Domain>& domains) const {
|
||||
const Domain& Dk = domains[keys_[0]];
|
||||
if (Dk.isSingleton() && !contains(*Dk.begin()))
|
||||
throw runtime_error("Domain::partiallyApply: unsatisfiable");
|
||||
return boost::make_shared<Domain>(Dk);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -7,8 +7,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/discrete/Constraint.h>
|
||||
#include <gtsam/discrete/DiscreteKey.h>
|
||||
#include <gtsam_unstable/discrete/Constraint.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -101,6 +101,6 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint {
|
|||
/// Partially apply known values, domain version
|
||||
Constraint::shared_ptr partiallyApply(
|
||||
const std::vector<Domain>& domains) const override;
|
||||
};
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -5,75 +5,74 @@
|
|||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/discrete/SingleValue.h>
|
||||
#include <gtsam_unstable/discrete/Domain.h>
|
||||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||
#include <gtsam_unstable/discrete/Domain.h>
|
||||
#include <gtsam_unstable/discrete/SingleValue.h>
|
||||
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void SingleValue::print(const string& s,
|
||||
const KeyFormatter& formatter) const {
|
||||
cout << s << "SingleValue on " << "j=" << formatter(keys_[0])
|
||||
<< " with value " << value_ << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double SingleValue::operator()(const Values& values) const {
|
||||
return (double) (values.at(keys_[0]) == value_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
DecisionTreeFactor SingleValue::toDecisionTreeFactor() const {
|
||||
DiscreteKeys keys;
|
||||
keys += DiscreteKey(keys_[0],cardinality_);
|
||||
vector<double> table;
|
||||
for (size_t i1 = 0; i1 < cardinality_; i1++)
|
||||
table.push_back(i1 == value_);
|
||||
DecisionTreeFactor converted(keys, table);
|
||||
return converted;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
DecisionTreeFactor SingleValue::operator*(const DecisionTreeFactor& f) const {
|
||||
// TODO: can we do this more efficiently?
|
||||
return toDecisionTreeFactor() * f;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool SingleValue::ensureArcConsistency(size_t j,
|
||||
vector<Domain>* domains) const {
|
||||
if (j != keys_[0])
|
||||
throw invalid_argument("SingleValue check on wrong domain");
|
||||
Domain& D = domains->at(j);
|
||||
if (D.isSingleton()) {
|
||||
if (D.firstValue() != value_) throw runtime_error("Unsatisfiable");
|
||||
return false;
|
||||
}
|
||||
D = Domain(discreteKey(), value_);
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Constraint::shared_ptr SingleValue::partiallyApply(const Values& values) const {
|
||||
Values::const_iterator it = values.find(keys_[0]);
|
||||
if (it != values.end() && it->second != value_) throw runtime_error(
|
||||
"SingleValue::partiallyApply: unsatisfiable");
|
||||
return boost::make_shared<SingleValue>(keys_[0], cardinality_, value_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Constraint::shared_ptr SingleValue::partiallyApply(
|
||||
const vector<Domain>& domains) const {
|
||||
const Domain& Dk = domains[keys_[0]];
|
||||
if (Dk.isSingleton() && !Dk.contains(value_)) throw runtime_error(
|
||||
"SingleValue::partiallyApply: unsatisfiable");
|
||||
return boost::make_shared<SingleValue>(discreteKey(), value_);
|
||||
}
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
void SingleValue::print(const string& s, const KeyFormatter& formatter) const {
|
||||
cout << s << "SingleValue on "
|
||||
<< "j=" << formatter(keys_[0]) << " with value " << value_ << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double SingleValue::operator()(const Values& values) const {
|
||||
return (double)(values.at(keys_[0]) == value_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
DecisionTreeFactor SingleValue::toDecisionTreeFactor() const {
|
||||
DiscreteKeys keys;
|
||||
keys += DiscreteKey(keys_[0], cardinality_);
|
||||
vector<double> table;
|
||||
for (size_t i1 = 0; i1 < cardinality_; i1++) table.push_back(i1 == value_);
|
||||
DecisionTreeFactor converted(keys, table);
|
||||
return converted;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
DecisionTreeFactor SingleValue::operator*(const DecisionTreeFactor& f) const {
|
||||
// TODO: can we do this more efficiently?
|
||||
return toDecisionTreeFactor() * f;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool SingleValue::ensureArcConsistency(size_t j,
|
||||
vector<Domain>* domains) const {
|
||||
if (j != keys_[0])
|
||||
throw invalid_argument("SingleValue check on wrong domain");
|
||||
Domain& D = domains->at(j);
|
||||
if (D.isSingleton()) {
|
||||
if (D.firstValue() != value_) throw runtime_error("Unsatisfiable");
|
||||
return false;
|
||||
}
|
||||
D = Domain(discreteKey(), value_);
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Constraint::shared_ptr SingleValue::partiallyApply(const Values& values) const {
|
||||
Values::const_iterator it = values.find(keys_[0]);
|
||||
if (it != values.end() && it->second != value_)
|
||||
throw runtime_error("SingleValue::partiallyApply: unsatisfiable");
|
||||
return boost::make_shared<SingleValue>(keys_[0], cardinality_, value_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Constraint::shared_ptr SingleValue::partiallyApply(
|
||||
const vector<Domain>& domains) const {
|
||||
const Domain& Dk = domains[keys_[0]];
|
||||
if (Dk.isSingleton() && !Dk.contains(value_))
|
||||
throw runtime_error("SingleValue::partiallyApply: unsatisfiable");
|
||||
return boost::make_shared<SingleValue>(discreteKey(), value_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -7,74 +7,71 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/discrete/DiscreteKey.h>
|
||||
#include <gtsam_unstable/discrete/Constraint.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* SingleValue constraint: ensures a variable takes on a certain value.
|
||||
* This could of course also be implemented by changing its `Domain`.
|
||||
/**
|
||||
* SingleValue constraint: ensures a variable takes on a certain value.
|
||||
* This could of course also be implemented by changing its `Domain`.
|
||||
*/
|
||||
class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint {
|
||||
size_t cardinality_; /// < Number of values
|
||||
size_t value_; ///< allowed value
|
||||
|
||||
DiscreteKey discreteKey() const {
|
||||
return DiscreteKey(keys_[0], cardinality_);
|
||||
}
|
||||
|
||||
public:
|
||||
typedef boost::shared_ptr<SingleValue> shared_ptr;
|
||||
|
||||
/// Construct from key, cardinality, and given value.
|
||||
SingleValue(Key key, size_t n, size_t value)
|
||||
: Constraint(key), cardinality_(n), value_(value) {}
|
||||
|
||||
/// Construct from DiscreteKey and given value.
|
||||
SingleValue(const DiscreteKey& dkey, size_t value)
|
||||
: Constraint(dkey.first), cardinality_(dkey.second), value_(value) {}
|
||||
|
||||
// print
|
||||
void print(const std::string& s = "", const KeyFormatter& formatter =
|
||||
DefaultKeyFormatter) const override;
|
||||
|
||||
/// equals
|
||||
bool equals(const DiscreteFactor& other, double tol) const override {
|
||||
if (!dynamic_cast<const SingleValue*>(&other))
|
||||
return false;
|
||||
else {
|
||||
const SingleValue& f(static_cast<const SingleValue&>(other));
|
||||
return (cardinality_ == f.cardinality_) && (value_ == f.value_);
|
||||
}
|
||||
}
|
||||
|
||||
/// Calculate value
|
||||
double operator()(const Values& values) const override;
|
||||
|
||||
/// Convert into a decisiontree
|
||||
DecisionTreeFactor toDecisionTreeFactor() const override;
|
||||
|
||||
/// Multiply into a decisiontree
|
||||
DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
|
||||
|
||||
/*
|
||||
* Ensure Arc-consistency: just sets domain[j] to {value_}
|
||||
* @param j domain to be checked
|
||||
* @param domains all other domains
|
||||
*/
|
||||
class GTSAM_UNSTABLE_EXPORT SingleValue: public Constraint {
|
||||
|
||||
size_t cardinality_; /// < Number of values
|
||||
size_t value_; ///< allowed value
|
||||
bool ensureArcConsistency(size_t j,
|
||||
std::vector<Domain>* domains) const override;
|
||||
|
||||
DiscreteKey discreteKey() const {
|
||||
return DiscreteKey(keys_[0],cardinality_);
|
||||
}
|
||||
/// Partially apply known values
|
||||
Constraint::shared_ptr partiallyApply(const Values& values) const override;
|
||||
|
||||
public:
|
||||
/// Partially apply known values, domain version
|
||||
Constraint::shared_ptr partiallyApply(
|
||||
const std::vector<Domain>& domains) const override;
|
||||
};
|
||||
|
||||
typedef boost::shared_ptr<SingleValue> shared_ptr;
|
||||
|
||||
/// Construct from key, cardinality, and given value.
|
||||
SingleValue(Key key, size_t n, size_t value) :
|
||||
Constraint(key), cardinality_(n), value_(value) {
|
||||
}
|
||||
|
||||
/// Construct from DiscreteKey and given value.
|
||||
SingleValue(const DiscreteKey& dkey, size_t value) :
|
||||
Constraint(dkey.first), cardinality_(dkey.second), value_(value) {
|
||||
}
|
||||
|
||||
// print
|
||||
void print(const std::string& s = "",
|
||||
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
|
||||
|
||||
/// equals
|
||||
bool equals(const DiscreteFactor& other, double tol) const override {
|
||||
if(!dynamic_cast<const SingleValue*>(&other))
|
||||
return false;
|
||||
else {
|
||||
const SingleValue& f(static_cast<const SingleValue&>(other));
|
||||
return (cardinality_==f.cardinality_) && (value_==f.value_);
|
||||
}
|
||||
}
|
||||
|
||||
/// Calculate value
|
||||
double operator()(const Values& values) const override;
|
||||
|
||||
/// Convert into a decisiontree
|
||||
DecisionTreeFactor toDecisionTreeFactor() const override;
|
||||
|
||||
/// Multiply into a decisiontree
|
||||
DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
|
||||
|
||||
/*
|
||||
* Ensure Arc-consistency: just sets domain[j] to {value_}
|
||||
* @param j domain to be checked
|
||||
* @param domains all other domains
|
||||
*/
|
||||
bool ensureArcConsistency(size_t j,
|
||||
std::vector<Domain>* domains) const override;
|
||||
|
||||
/// Partially apply known values
|
||||
Constraint::shared_ptr partiallyApply(const Values& values) const override;
|
||||
|
||||
/// Partially apply known values, domain version
|
||||
Constraint::shared_ptr partiallyApply(
|
||||
const std::vector<Domain>& domains) const override;
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -30,7 +30,7 @@ TEST(CSP, SingleValue) {
|
|||
EXPECT(assert_equal(f1, singleValue.toDecisionTreeFactor()));
|
||||
|
||||
// Create domains, laid out as a vector.
|
||||
// TODO(dellaert): should be map??
|
||||
// TODO(dellaert): should be map??
|
||||
vector<Domain> domains;
|
||||
domains += Domain(ID), Domain(AZ), Domain(UT);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue