Accept list in constructor
Frank Dellaert
parent
d9b639ab97
commit
54afba0bbc
|
|
@ -16,196 +16,249 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// \callgraph
|
// \callgraph
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <iostream>
|
|
||||||
#include <boost/utility.hpp> // for noncopyable
|
|
||||||
#include <boost/foreach.hpp>
|
|
||||||
#include <boost/range/iterator_range.hpp>
|
|
||||||
#include <boost/lexical_cast.hpp>
|
|
||||||
#include <boost/serialization/nvp.hpp>
|
|
||||||
#include <gtsam/inference/Factor.h>
|
#include <gtsam/inference/Factor.h>
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
#include <list>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/**
|
|
||||||
* Base class for conditional densities, templated on KEY type. This class
|
|
||||||
* provides storage for the keys involved in a conditional, and iterators and
|
|
||||||
* access to the frontal and separator keys.
|
|
||||||
*
|
|
||||||
* Derived classes *must* redefine the Factor and shared_ptr typedefs to refer
|
|
||||||
* to the associated factor type and shared_ptr type of the derived class. See
|
|
||||||
* IndexConditional and GaussianConditional for examples.
|
|
||||||
* \nosubgrouping
|
|
||||||
*/
|
|
||||||
template<typename KEY>
|
|
||||||
class Conditional: public gtsam::Factor<KEY> {
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
/** Create keys by adding key in front */
|
|
||||||
template<typename ITERATOR>
|
|
||||||
static std::vector<KEY> MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) {
|
|
||||||
std::vector<KeyType> keys((lastParent - firstParent) + 1);
|
|
||||||
std::copy(firstParent, lastParent, keys.begin() + 1);
|
|
||||||
keys[0] = key;
|
|
||||||
return keys;
|
|
||||||
}
|
|
||||||
|
|
||||||
protected:
|
|
||||||
|
|
||||||
/** The first nFrontal variables are frontal and the rest are parents. */
|
|
||||||
size_t nrFrontals_;
|
|
||||||
|
|
||||||
// Calls the base class assertInvariants, which checks for unique keys
|
|
||||||
void assertInvariants() const { Factor<KEY>::assertInvariants(); }
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
typedef KEY KeyType;
|
|
||||||
typedef Conditional<KeyType> This;
|
|
||||||
typedef Factor<KeyType> Base;
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Typedef to the factor type that produces this conditional and that this
|
* Base class for conditional densities, templated on KEY type. This class
|
||||||
* conditional can be converted to using a factor constructor. Derived
|
* provides storage for the keys involved in a conditional, and iterators and
|
||||||
* classes must redefine this.
|
* access to the frontal and separator keys.
|
||||||
|
*
|
||||||
|
* Derived classes *must* redefine the Factor and shared_ptr typedefs to refer
|
||||||
|
* to the associated factor type and shared_ptr type of the derived class. See
|
||||||
|
* IndexConditional and GaussianConditional for examples.
|
||||||
|
* \nosubgrouping
|
||||||
*/
|
*/
|
||||||
typedef gtsam::Factor<KeyType> FactorType;
|
template<typename KEY>
|
||||||
|
class Conditional: public gtsam::Factor<KEY> {
|
||||||
|
|
||||||
/** A shared_ptr to this class. Derived classes must redefine this. */
|
private:
|
||||||
typedef boost::shared_ptr<This> shared_ptr;
|
|
||||||
|
|
||||||
/** Iterator over keys */
|
/** Create keys by adding key in front */
|
||||||
typedef typename FactorType::iterator iterator;
|
template<typename ITERATOR>
|
||||||
|
static std::vector<KEY> MakeKeys(KEY key, ITERATOR firstParent,
|
||||||
|
ITERATOR lastParent) {
|
||||||
|
std::vector<KeyType> keys((lastParent - firstParent) + 1);
|
||||||
|
std::copy(firstParent, lastParent, keys.begin() + 1);
|
||||||
|
keys[0] = key;
|
||||||
|
return keys;
|
||||||
|
}
|
||||||
|
|
||||||
/** Const iterator over keys */
|
protected:
|
||||||
typedef typename FactorType::const_iterator const_iterator;
|
|
||||||
|
|
||||||
/** View of the frontal keys (call frontals()) */
|
/** The first nFrontal variables are frontal and the rest are parents. */
|
||||||
typedef boost::iterator_range<const_iterator> Frontals;
|
size_t nrFrontals_;
|
||||||
|
|
||||||
/** View of the separator keys (call parents()) */
|
// Calls the base class assertInvariants, which checks for unique keys
|
||||||
typedef boost::iterator_range<const_iterator> Parents;
|
void assertInvariants() const {
|
||||||
|
Factor<KEY>::assertInvariants();
|
||||||
|
}
|
||||||
|
|
||||||
/// @name Standard Constructors
|
public:
|
||||||
/// @{
|
|
||||||
|
|
||||||
/** Empty Constructor to make serialization possible */
|
typedef KEY KeyType;
|
||||||
Conditional() : nrFrontals_(0) { assertInvariants(); }
|
typedef Conditional<KeyType> This;
|
||||||
|
typedef Factor<KeyType> Base;
|
||||||
|
|
||||||
/** No parents */
|
/**
|
||||||
Conditional(KeyType key) : FactorType(key), nrFrontals_(1) { assertInvariants(); }
|
* Typedef to the factor type that produces this conditional and that this
|
||||||
|
* conditional can be converted to using a factor constructor. Derived
|
||||||
|
* classes must redefine this.
|
||||||
|
*/
|
||||||
|
typedef gtsam::Factor<KeyType> FactorType;
|
||||||
|
|
||||||
/** Single parent */
|
/** A shared_ptr to this class. Derived classes must redefine this. */
|
||||||
Conditional(KeyType key, KeyType parent)
|
typedef boost::shared_ptr<This> shared_ptr;
|
||||||
: FactorType(key, parent), nrFrontals_(1) { assertInvariants(); }
|
|
||||||
|
|
||||||
/** Two parents */
|
/** Iterator over keys */
|
||||||
Conditional(KeyType key, KeyType parent1, KeyType parent2)
|
typedef typename FactorType::iterator iterator;
|
||||||
: FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); }
|
|
||||||
|
|
||||||
/** Three parents */
|
/** Const iterator over keys */
|
||||||
Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3)
|
typedef typename FactorType::const_iterator const_iterator;
|
||||||
: FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); }
|
|
||||||
|
|
||||||
/// @}
|
/** View of the frontal keys (call frontals()) */
|
||||||
/// @name Advanced Constructors
|
typedef boost::iterator_range<const_iterator> Frontals;
|
||||||
/// @{
|
|
||||||
|
|
||||||
/** Constructor from a frontal variable and a vector of parents */
|
/** View of the separator keys (call parents()) */
|
||||||
Conditional(KeyType key, const std::vector<KeyType>& parents) :
|
typedef boost::iterator_range<const_iterator> Parents;
|
||||||
FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(1) {
|
|
||||||
assertInvariants();
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Constructor from keys and nr of frontal variables */
|
/// @name Standard Constructors
|
||||||
Conditional(const std::vector<Index>& keys, size_t nrFrontals) :
|
/// @{
|
||||||
FactorType(keys), nrFrontals_(nrFrontals) {
|
|
||||||
assertInvariants();
|
|
||||||
}
|
|
||||||
|
|
||||||
/// @}
|
/** Empty Constructor to make serialization possible */
|
||||||
/// @name Testable
|
Conditional() :
|
||||||
/// @{
|
nrFrontals_(0) {
|
||||||
|
assertInvariants();
|
||||||
|
}
|
||||||
|
|
||||||
/** print with optional formatter */
|
/** No parents */
|
||||||
void print(const std::string& s = "Conditional", const IndexFormatter& formatter = DefaultIndexFormatter) const;
|
Conditional(KeyType key) :
|
||||||
|
FactorType(key), nrFrontals_(1) {
|
||||||
|
assertInvariants();
|
||||||
|
}
|
||||||
|
|
||||||
/** check equality */
|
/** Single parent */
|
||||||
template<class DERIVED>
|
Conditional(KeyType key, KeyType parent) :
|
||||||
bool equals(const DERIVED& c, double tol = 1e-9) const {
|
FactorType(key, parent), nrFrontals_(1) {
|
||||||
return nrFrontals_ == c.nrFrontals_ && FactorType::equals(c, tol); }
|
assertInvariants();
|
||||||
|
}
|
||||||
|
|
||||||
/// @}
|
/** Two parents */
|
||||||
/// @name Standard Interface
|
Conditional(KeyType key, KeyType parent1, KeyType parent2) :
|
||||||
/// @{
|
FactorType(key, parent1, parent2), nrFrontals_(1) {
|
||||||
|
assertInvariants();
|
||||||
|
}
|
||||||
|
|
||||||
/** return the number of frontals */
|
/** Three parents */
|
||||||
size_t nrFrontals() const { return nrFrontals_; }
|
Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) :
|
||||||
|
FactorType(key, parent1, parent2, parent3), nrFrontals_(1) {
|
||||||
|
assertInvariants();
|
||||||
|
}
|
||||||
|
|
||||||
/** return the number of parents */
|
/// @}
|
||||||
size_t nrParents() const { return FactorType::size() - nrFrontals_; }
|
/// @name Advanced Constructors
|
||||||
|
/// @{
|
||||||
|
|
||||||
/** Special accessor when there is only one frontal variable. */
|
/** Constructor from a frontal variable and a vector of parents */
|
||||||
KeyType firstFrontalKey() const { assert(nrFrontals_>0); return FactorType::front(); }
|
Conditional(KeyType key, const std::vector<KeyType>& parents) :
|
||||||
KeyType lastFrontalKey() const { assert(nrFrontals_>0); return *(endFrontals()-1); }
|
FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(
|
||||||
|
1) {
|
||||||
|
assertInvariants();
|
||||||
|
}
|
||||||
|
|
||||||
/** return a view of the frontal keys */
|
/** Constructor from keys and nr of frontal variables */
|
||||||
Frontals frontals() const {
|
Conditional(const std::vector<Index>& keys, size_t nrFrontals) :
|
||||||
return boost::make_iterator_range(beginFrontals(), endFrontals()); }
|
FactorType(keys), nrFrontals_(nrFrontals) {
|
||||||
|
assertInvariants();
|
||||||
|
}
|
||||||
|
|
||||||
/** return a view of the parent keys */
|
/** Constructor from keys and nr of frontal variables */
|
||||||
Parents parents() const {
|
Conditional(const std::list<Index>& keys, size_t nrFrontals) :
|
||||||
return boost::make_iterator_range(beginParents(), endParents()); }
|
FactorType(keys.begin(),keys.end()), nrFrontals_(nrFrontals) {
|
||||||
|
assertInvariants();
|
||||||
|
}
|
||||||
|
|
||||||
/** Iterators over frontal and parent variables. */
|
/// @}
|
||||||
const_iterator beginFrontals() const { return FactorType::begin(); } ///<TODO: comment
|
/// @name Testable
|
||||||
const_iterator endFrontals() const { return FactorType::begin()+nrFrontals_; } ///<TODO: comment
|
/// @{
|
||||||
const_iterator beginParents() const { return FactorType::begin()+nrFrontals_; } ///<TODO: comment
|
|
||||||
const_iterator endParents() const { return FactorType::end(); } ///<TODO: comment
|
|
||||||
|
|
||||||
/// @}
|
/** print with optional formatter */
|
||||||
/// @name Advanced Interface
|
void print(const std::string& s = "Conditional",
|
||||||
/// @{
|
const IndexFormatter& formatter = DefaultIndexFormatter) const;
|
||||||
|
|
||||||
/** Mutable iterators and accessors */
|
/** check equality */
|
||||||
iterator beginFrontals() { return FactorType::begin(); } ///<TODO: comment
|
template<class DERIVED>
|
||||||
iterator endFrontals() { return FactorType::begin()+nrFrontals_; } ///<TODO: comment
|
bool equals(const DERIVED& c, double tol = 1e-9) const {
|
||||||
iterator beginParents() { return FactorType::begin()+nrFrontals_; } ///<TODO: comment
|
return nrFrontals_ == c.nrFrontals_ && FactorType::equals(c, tol);
|
||||||
iterator endParents() { return FactorType::end(); } ///<TODO: comment
|
}
|
||||||
|
|
||||||
///TODO: comment
|
/// @}
|
||||||
boost::iterator_range<iterator> frontals() {
|
/// @name Standard Interface
|
||||||
return boost::make_iterator_range(beginFrontals(), endFrontals()); }
|
/// @{
|
||||||
|
|
||||||
///TODO: comment
|
/** return the number of frontals */
|
||||||
boost::iterator_range<iterator> parents() {
|
size_t nrFrontals() const {
|
||||||
return boost::make_iterator_range(beginParents(), endParents()); }
|
return nrFrontals_;
|
||||||
|
}
|
||||||
|
|
||||||
private:
|
/** return the number of parents */
|
||||||
/** Serialization function */
|
size_t nrParents() const {
|
||||||
friend class boost::serialization::access;
|
return FactorType::size() - nrFrontals_;
|
||||||
template<class ARCHIVE>
|
}
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
||||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
/** Special accessor when there is only one frontal variable. */
|
||||||
ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
|
KeyType firstFrontalKey() const {
|
||||||
|
assert(nrFrontals_>0);
|
||||||
|
return FactorType::front();
|
||||||
|
}
|
||||||
|
KeyType lastFrontalKey() const {
|
||||||
|
assert(nrFrontals_>0);
|
||||||
|
return *(endFrontals() - 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
/** return a view of the frontal keys */
|
||||||
|
Frontals frontals() const {
|
||||||
|
return boost::make_iterator_range(beginFrontals(), endFrontals());
|
||||||
|
}
|
||||||
|
|
||||||
|
/** return a view of the parent keys */
|
||||||
|
Parents parents() const {
|
||||||
|
return boost::make_iterator_range(beginParents(), endParents());
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Iterators over frontal and parent variables. */
|
||||||
|
const_iterator beginFrontals() const {
|
||||||
|
return FactorType::begin();
|
||||||
|
} ///<TODO: comment
|
||||||
|
const_iterator endFrontals() const {
|
||||||
|
return FactorType::begin() + nrFrontals_;
|
||||||
|
} ///<TODO: comment
|
||||||
|
const_iterator beginParents() const {
|
||||||
|
return FactorType::begin() + nrFrontals_;
|
||||||
|
} ///<TODO: comment
|
||||||
|
const_iterator endParents() const {
|
||||||
|
return FactorType::end();
|
||||||
|
} ///<TODO: comment
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
/// @name Advanced Interface
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/** Mutable iterators and accessors */
|
||||||
|
iterator beginFrontals() {
|
||||||
|
return FactorType::begin();
|
||||||
|
} ///<TODO: comment
|
||||||
|
iterator endFrontals() {
|
||||||
|
return FactorType::begin() + nrFrontals_;
|
||||||
|
} ///<TODO: comment
|
||||||
|
iterator beginParents() {
|
||||||
|
return FactorType::begin() + nrFrontals_;
|
||||||
|
} ///<TODO: comment
|
||||||
|
iterator endParents() {
|
||||||
|
return FactorType::end();
|
||||||
|
} ///<TODO: comment
|
||||||
|
|
||||||
|
///TODO: comment
|
||||||
|
boost::iterator_range<iterator> frontals() {
|
||||||
|
return boost::make_iterator_range(beginFrontals(), endFrontals());
|
||||||
|
}
|
||||||
|
|
||||||
|
///TODO: comment
|
||||||
|
boost::iterator_range<iterator> parents() {
|
||||||
|
return boost::make_iterator_range(beginParents(), endParents());
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
/** Serialization function */
|
||||||
|
friend class boost::serialization::access;
|
||||||
|
template<class ARCHIVE>
|
||||||
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||||
|
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
template<typename KEY>
|
||||||
|
void Conditional<KEY>::print(const std::string& s,
|
||||||
|
const IndexFormatter& formatter) const {
|
||||||
|
std::cout << s << " P(";
|
||||||
|
BOOST_FOREACH(KeyType key, frontals())
|
||||||
|
std::cout << " " << formatter(key);
|
||||||
|
if (nrParents() > 0)
|
||||||
|
std::cout << " |";
|
||||||
|
BOOST_FOREACH(KeyType parent, parents())
|
||||||
|
std::cout << " " << formatter(parent);
|
||||||
|
std::cout << ")" << std::endl;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// @}
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
template<typename KEY>
|
|
||||||
void Conditional<KEY>::print(const std::string& s, const IndexFormatter& formatter) const {
|
|
||||||
std::cout << s << " P(";
|
|
||||||
BOOST_FOREACH(KeyType key, frontals()) std::cout << " " << formatter(key);
|
|
||||||
if (nrParents()>0) std::cout << " |";
|
|
||||||
BOOST_FOREACH(KeyType parent, parents()) std::cout << " " << formatter(parent);
|
|
||||||
std::cout << ")" << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // gtsam
|
} // gtsam
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue