12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Accept list in constructor

release/4.3a0
Frank Dellaert 2012-09-17 00:23:42 +00:00
parent d9b639ab97
commit 54afba0bbc
1 changed files with 206 additions and 153 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

157
gtsam/inference/Conditional.h
View File

@ -16,20 +16,16 @@
*/
// \callgraph
#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 <iostream>
#include <list>
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.
@ -39,29 +35,32 @@ namespace gtsam {
* IndexConditional and GaussianConditional for examples.
* \nosubgrouping
*/
template<typename KEY>
class Conditional: public gtsam::Factor<KEY> {
template<typename KEY>
class Conditional: public gtsam::Factor<KEY> {
private:
private:
/** Create keys by adding key in front */
template<typename ITERATOR>
static std::vector<KEY> MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) {
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:
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(); }
void assertInvariants() const {
Factor<KEY>::assertInvariants();
}
public:
public:
typedef KEY KeyType;
typedef Conditional<KeyType> This;
@ -93,22 +92,34 @@ public:
/// @{
/** Empty Constructor to make serialization possible */
Conditional() : nrFrontals_(0) { assertInvariants(); }
Conditional() :
nrFrontals_(0) {
assertInvariants();
}
/** No parents */
Conditional(KeyType key) : FactorType(key), nrFrontals_(1) { assertInvariants(); }
Conditional(KeyType key) :
FactorType(key), nrFrontals_(1) {
assertInvariants();
}
/** Single parent */
Conditional(KeyType key, KeyType parent)
: FactorType(key, parent), nrFrontals_(1) { assertInvariants(); }
Conditional(KeyType key, KeyType parent) :
FactorType(key, parent), nrFrontals_(1) {
assertInvariants();
}
/** Two parents */
Conditional(KeyType key, KeyType parent1, KeyType parent2)
: FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); }
Conditional(KeyType key, KeyType parent1, KeyType parent2) :
FactorType(key, parent1, parent2), nrFrontals_(1) {
assertInvariants();
}
/** Three parents */
Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3)
: FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); }
Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) :
FactorType(key, parent1, parent2, parent3), nrFrontals_(1) {
assertInvariants();
}
/// @}
/// @name Advanced Constructors
@ -116,7 +127,8 @@ public:
/** Constructor from a frontal variable and a vector of parents */
Conditional(KeyType key, const std::vector<KeyType>& parents) :
FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(1) {
FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(
1) {
assertInvariants();
}
@ -126,65 +138,103 @@ public:
assertInvariants();
}
/** Constructor from keys and nr of frontal variables */
Conditional(const std::list<Index>& keys, size_t nrFrontals) :
FactorType(keys.begin(),keys.end()), nrFrontals_(nrFrontals) {
assertInvariants();
}
/// @}
/// @name Testable
/// @{
/** print with optional formatter */
void print(const std::string& s = "Conditional", const IndexFormatter& formatter = DefaultIndexFormatter) const;
void print(const std::string& s = "Conditional",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** check equality */
template<class DERIVED>
bool equals(const DERIVED& c, double tol = 1e-9) const {
return nrFrontals_ == c.nrFrontals_ && FactorType::equals(c, tol); }
return nrFrontals_ == c.nrFrontals_ && FactorType::equals(c, tol);
}
/// @}
/// @name Standard Interface
/// @{
/** return the number of frontals */
size_t nrFrontals() const { return nrFrontals_; }
size_t nrFrontals() const {
return nrFrontals_;
}
/** return the number of parents */
size_t nrParents() const { return FactorType::size() - nrFrontals_; }
size_t nrParents() const {
return FactorType::size() - nrFrontals_;
}
/** Special accessor when there is only one frontal variable. */
KeyType firstFrontalKey() const { assert(nrFrontals_>0); return FactorType::front(); }
KeyType lastFrontalKey() const { assert(nrFrontals_>0); return *(endFrontals()-1); }
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 boost::make_iterator_range(beginFrontals(), endFrontals());
}
/** return a view of the parent keys */
Parents parents() const {
return boost::make_iterator_range(beginParents(), endParents()); }
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
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
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()); }
return boost::make_iterator_range(beginFrontals(), endFrontals());
}
///TODO: comment
boost::iterator_range<iterator> parents() {
return boost::make_iterator_range(beginParents(), endParents()); }
return boost::make_iterator_range(beginParents(), endParents());
}
private:
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
@ -195,17 +245,20 @@ private:
/// @}
};
};
/* ************************************************************************* */
template<typename KEY>
void Conditional<KEY>::print(const std::string& s, const IndexFormatter& formatter) const {
/* ************************************************************************* */
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);
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