Renamed typedef name Factor to FactorType
parent
d132066e12
commit
d99d047a77
|
@ -296,17 +296,17 @@ namespace gtsam {
|
||||||
}
|
}
|
||||||
|
|
||||||
// The root conditional
|
// The root conditional
|
||||||
FactorGraph<typename CONDITIONAL::Factor> p_R(*R);
|
FactorGraph<typename CONDITIONAL::FactorType> p_R(*R);
|
||||||
|
|
||||||
// The parent clique has a CONDITIONAL for each frontal node in Fp
|
// The parent clique has a CONDITIONAL for each frontal node in Fp
|
||||||
// so we can obtain P(Fp|Sp) in factor graph form
|
// so we can obtain P(Fp|Sp) in factor graph form
|
||||||
FactorGraph<typename CONDITIONAL::Factor> p_Fp_Sp(*parent);
|
FactorGraph<typename CONDITIONAL::FactorType> p_Fp_Sp(*parent);
|
||||||
|
|
||||||
// If not the base case, obtain the parent shortcut P(Sp|R) as factors
|
// If not the base case, obtain the parent shortcut P(Sp|R) as factors
|
||||||
FactorGraph<typename CONDITIONAL::Factor> p_Sp_R(parent->shortcut(R));
|
FactorGraph<typename CONDITIONAL::FactorType> p_Sp_R(parent->shortcut(R));
|
||||||
|
|
||||||
// now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R)
|
// now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R)
|
||||||
FactorGraph<typename CONDITIONAL::Factor> p_Cp_R;
|
FactorGraph<typename CONDITIONAL::FactorType> p_Cp_R;
|
||||||
p_Cp_R.push_back(p_R);
|
p_Cp_R.push_back(p_R);
|
||||||
p_Cp_R.push_back(p_Fp_Sp);
|
p_Cp_R.push_back(p_Fp_Sp);
|
||||||
p_Cp_R.push_back(p_Sp_R);
|
p_Cp_R.push_back(p_Sp_R);
|
||||||
|
@ -345,9 +345,9 @@ namespace gtsam {
|
||||||
vector<Index>(variablesAtBack.begin(), variablesAtBack.end()),
|
vector<Index>(variablesAtBack.begin(), variablesAtBack.end()),
|
||||||
R->back()->key() + 1);
|
R->back()->key() + 1);
|
||||||
Permutation::shared_ptr toBackInverse(toBack.inverse());
|
Permutation::shared_ptr toBackInverse(toBack.inverse());
|
||||||
BOOST_FOREACH(const typename CONDITIONAL::Factor::shared_ptr& factor, p_Cp_R) {
|
BOOST_FOREACH(const typename CONDITIONAL::FactorType::shared_ptr& factor, p_Cp_R) {
|
||||||
factor->permuteWithInverse(*toBackInverse); }
|
factor->permuteWithInverse(*toBackInverse); }
|
||||||
typename BayesNet<CONDITIONAL>::shared_ptr eliminated(EliminationTree<typename CONDITIONAL::Factor>::Create(p_Cp_R)->eliminate());
|
typename BayesNet<CONDITIONAL>::shared_ptr eliminated(EliminationTree<typename CONDITIONAL::FactorType>::Create(p_Cp_R)->eliminate());
|
||||||
|
|
||||||
// Take only the conditionals for p(S|R). We check for each variable being
|
// Take only the conditionals for p(S|R). We check for each variable being
|
||||||
// in the separator set because if some separator variables overlap with
|
// in the separator set because if some separator variables overlap with
|
||||||
|
@ -380,7 +380,7 @@ namespace gtsam {
|
||||||
// Because the root clique could be very big.
|
// Because the root clique could be very big.
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template<class CONDITIONAL>
|
template<class CONDITIONAL>
|
||||||
FactorGraph<typename CONDITIONAL::Factor> BayesTree<CONDITIONAL>::Clique::marginal(shared_ptr R) {
|
FactorGraph<typename CONDITIONAL::FactorType> BayesTree<CONDITIONAL>::Clique::marginal(shared_ptr R) {
|
||||||
// If we are the root, just return this root
|
// If we are the root, just return this root
|
||||||
// NOTE: immediately cast to a factor graph
|
// NOTE: immediately cast to a factor graph
|
||||||
if (R.get()==this) return *R;
|
if (R.get()==this) return *R;
|
||||||
|
@ -391,18 +391,18 @@ namespace gtsam {
|
||||||
p_FSR.push_back(*R);
|
p_FSR.push_back(*R);
|
||||||
|
|
||||||
assertInvariants();
|
assertInvariants();
|
||||||
return *GenericSequentialSolver<typename CONDITIONAL::Factor>(p_FSR).jointFactorGraph(keys());
|
return *GenericSequentialSolver<typename CONDITIONAL::FactorType>(p_FSR).jointFactorGraph(keys());
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R)
|
// P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R)
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template<class CONDITIONAL>
|
template<class CONDITIONAL>
|
||||||
FactorGraph<typename CONDITIONAL::Factor> BayesTree<CONDITIONAL>::Clique::joint(shared_ptr C2, shared_ptr R) {
|
FactorGraph<typename CONDITIONAL::FactorType> BayesTree<CONDITIONAL>::Clique::joint(shared_ptr C2, shared_ptr R) {
|
||||||
// For now, assume neither is the root
|
// For now, assume neither is the root
|
||||||
|
|
||||||
// Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R)
|
// Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R)
|
||||||
FactorGraph<typename CONDITIONAL::Factor> joint;
|
FactorGraph<typename CONDITIONAL::FactorType> joint;
|
||||||
if (!isRoot()) joint.push_back(*this); // P(F1|S1)
|
if (!isRoot()) joint.push_back(*this); // P(F1|S1)
|
||||||
if (!isRoot()) joint.push_back(shortcut(R)); // P(S1|R)
|
if (!isRoot()) joint.push_back(shortcut(R)); // P(S1|R)
|
||||||
if (!C2->isRoot()) joint.push_back(*C2); // P(F2|S2)
|
if (!C2->isRoot()) joint.push_back(*C2); // P(F2|S2)
|
||||||
|
@ -420,7 +420,7 @@ namespace gtsam {
|
||||||
vector<Index> keys12vector; keys12vector.reserve(keys12.size());
|
vector<Index> keys12vector; keys12vector.reserve(keys12.size());
|
||||||
keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end());
|
keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end());
|
||||||
assertInvariants();
|
assertInvariants();
|
||||||
return *GenericSequentialSolver<typename CONDITIONAL::Factor>(joint).jointFactorGraph(keys12vector);
|
return *GenericSequentialSolver<typename CONDITIONAL::FactorType>(joint).jointFactorGraph(keys12vector);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
@ -729,15 +729,15 @@ namespace gtsam {
|
||||||
// First finds clique marginal then marginalizes that
|
// First finds clique marginal then marginalizes that
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template<class CONDITIONAL>
|
template<class CONDITIONAL>
|
||||||
typename CONDITIONAL::Factor::shared_ptr BayesTree<CONDITIONAL>::marginalFactor(Index key) const {
|
typename CONDITIONAL::FactorType::shared_ptr BayesTree<CONDITIONAL>::marginalFactor(Index key) const {
|
||||||
|
|
||||||
// get clique containing key
|
// get clique containing key
|
||||||
sharedClique clique = (*this)[key];
|
sharedClique clique = (*this)[key];
|
||||||
|
|
||||||
// calculate or retrieve its marginal
|
// calculate or retrieve its marginal
|
||||||
FactorGraph<typename CONDITIONAL::Factor> cliqueMarginal = clique->marginal(root_);
|
FactorGraph<typename CONDITIONAL::FactorType> cliqueMarginal = clique->marginal(root_);
|
||||||
|
|
||||||
return GenericSequentialSolver<typename CONDITIONAL::Factor>(cliqueMarginal).marginalFactor(key);
|
return GenericSequentialSolver<typename CONDITIONAL::FactorType>(cliqueMarginal).marginalFactor(key);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
@ -745,29 +745,29 @@ namespace gtsam {
|
||||||
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL>::marginalBayesNet(Index key) const {
|
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL>::marginalBayesNet(Index key) const {
|
||||||
|
|
||||||
// calculate marginal as a factor graph
|
// calculate marginal as a factor graph
|
||||||
FactorGraph<typename CONDITIONAL::Factor> fg;
|
FactorGraph<typename CONDITIONAL::FactorType> fg;
|
||||||
fg.push_back(this->marginalFactor(key));
|
fg.push_back(this->marginalFactor(key));
|
||||||
|
|
||||||
// eliminate factor graph marginal to a Bayes net
|
// eliminate factor graph marginal to a Bayes net
|
||||||
return GenericSequentialSolver<typename CONDITIONAL::Factor>(fg).eliminate();
|
return GenericSequentialSolver<typename CONDITIONAL::FactorType>(fg).eliminate();
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// Find two cliques, their joint, then marginalizes
|
// Find two cliques, their joint, then marginalizes
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template<class CONDITIONAL>
|
template<class CONDITIONAL>
|
||||||
typename FactorGraph<typename CONDITIONAL::Factor>::shared_ptr
|
typename FactorGraph<typename CONDITIONAL::FactorType>::shared_ptr
|
||||||
BayesTree<CONDITIONAL>::joint(Index key1, Index key2) const {
|
BayesTree<CONDITIONAL>::joint(Index key1, Index key2) const {
|
||||||
|
|
||||||
// get clique C1 and C2
|
// get clique C1 and C2
|
||||||
sharedClique C1 = (*this)[key1], C2 = (*this)[key2];
|
sharedClique C1 = (*this)[key1], C2 = (*this)[key2];
|
||||||
|
|
||||||
// calculate joint
|
// calculate joint
|
||||||
FactorGraph<typename CONDITIONAL::Factor> p_C1C2(C1->joint(C2, root_));
|
FactorGraph<typename CONDITIONAL::FactorType> p_C1C2(C1->joint(C2, root_));
|
||||||
|
|
||||||
// eliminate remaining factor graph to get requested joint
|
// eliminate remaining factor graph to get requested joint
|
||||||
vector<Index> key12(2); key12[0] = key1; key12[1] = key2;
|
vector<Index> key12(2); key12[0] = key1; key12[1] = key2;
|
||||||
return GenericSequentialSolver<typename CONDITIONAL::Factor>(p_C1C2).jointFactorGraph(key12);
|
return GenericSequentialSolver<typename CONDITIONAL::FactorType>(p_C1C2).jointFactorGraph(key12);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
@ -775,7 +775,7 @@ namespace gtsam {
|
||||||
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL>::jointBayesNet(Index key1, Index key2) const {
|
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL>::jointBayesNet(Index key1, Index key2) const {
|
||||||
|
|
||||||
// eliminate factor graph marginal to a Bayes net
|
// eliminate factor graph marginal to a Bayes net
|
||||||
return GenericSequentialSolver<typename CONDITIONAL::Factor>(*this->joint(key1, key2)).eliminate();
|
return GenericSequentialSolver<typename CONDITIONAL::FactorType>(*this->joint(key1, key2)).eliminate();
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
@ -63,7 +63,7 @@ namespace gtsam {
|
||||||
weak_ptr parent_;
|
weak_ptr parent_;
|
||||||
std::list<shared_ptr> children_;
|
std::list<shared_ptr> children_;
|
||||||
std::list<Index> separator_; /** separator keys */
|
std::list<Index> separator_; /** separator keys */
|
||||||
typename CONDITIONAL::Factor::shared_ptr cachedFactor_;
|
typename CONDITIONAL::FactorType::shared_ptr cachedFactor_;
|
||||||
|
|
||||||
friend class BayesTree<CONDITIONAL>;
|
friend class BayesTree<CONDITIONAL>;
|
||||||
|
|
||||||
|
@ -96,7 +96,7 @@ namespace gtsam {
|
||||||
size_t treeSize() const;
|
size_t treeSize() const;
|
||||||
|
|
||||||
/** Access the cached factor (this is a hack) */
|
/** Access the cached factor (this is a hack) */
|
||||||
typename CONDITIONAL::Factor::shared_ptr& cachedFactor() { return cachedFactor_; }
|
typename CONDITIONAL::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
|
||||||
|
|
||||||
/** print this node and entire subtree below it */
|
/** print this node and entire subtree below it */
|
||||||
void printTree(const std::string& indent="") const;
|
void printTree(const std::string& indent="") const;
|
||||||
|
@ -116,10 +116,10 @@ namespace gtsam {
|
||||||
BayesNet<CONDITIONAL> shortcut(shared_ptr root);
|
BayesNet<CONDITIONAL> shortcut(shared_ptr root);
|
||||||
|
|
||||||
/** return the marginal P(C) of the clique */
|
/** return the marginal P(C) of the clique */
|
||||||
FactorGraph<typename CONDITIONAL::Factor> marginal(shared_ptr root);
|
FactorGraph<typename CONDITIONAL::FactorType> marginal(shared_ptr root);
|
||||||
|
|
||||||
/** return the joint P(C1,C2), where C1==this. TODO: not a method? */
|
/** return the joint P(C1,C2), where C1==this. TODO: not a method? */
|
||||||
FactorGraph<typename CONDITIONAL::Factor> joint(shared_ptr C2, shared_ptr root);
|
FactorGraph<typename CONDITIONAL::FactorType> joint(shared_ptr C2, shared_ptr root);
|
||||||
|
|
||||||
}; // \struct Clique
|
}; // \struct Clique
|
||||||
|
|
||||||
|
@ -262,7 +262,7 @@ namespace gtsam {
|
||||||
CliqueData getCliqueData() const;
|
CliqueData getCliqueData() const;
|
||||||
|
|
||||||
/** return marginal on any variable */
|
/** return marginal on any variable */
|
||||||
typename CONDITIONAL::Factor::shared_ptr marginalFactor(Index key) const;
|
typename CONDITIONAL::FactorType::shared_ptr marginalFactor(Index key) const;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* return marginal on any variable, as a Bayes Net
|
* return marginal on any variable, as a Bayes Net
|
||||||
|
@ -272,7 +272,7 @@ namespace gtsam {
|
||||||
typename BayesNet<CONDITIONAL>::shared_ptr marginalBayesNet(Index key) const;
|
typename BayesNet<CONDITIONAL>::shared_ptr marginalBayesNet(Index key) const;
|
||||||
|
|
||||||
/** return joint on two variables */
|
/** return joint on two variables */
|
||||||
typename FactorGraph<typename CONDITIONAL::Factor>::shared_ptr joint(Index key1, Index key2) const;
|
typename FactorGraph<typename CONDITIONAL::FactorType>::shared_ptr joint(Index key1, Index key2) const;
|
||||||
|
|
||||||
/** return joint on two variables as a BayesNet */
|
/** return joint on two variables as a BayesNet */
|
||||||
typename BayesNet<CONDITIONAL>::shared_ptr jointBayesNet(Index key1, Index key2) const;
|
typename BayesNet<CONDITIONAL>::shared_ptr jointBayesNet(Index key1, Index key2) const;
|
||||||
|
|
|
@ -69,16 +69,16 @@ public:
|
||||||
* conditional can be converted to using a factor constructor. Derived
|
* conditional can be converted to using a factor constructor. Derived
|
||||||
* classes must redefine this.
|
* classes must redefine this.
|
||||||
*/
|
*/
|
||||||
typedef gtsam::FactorBase<Key> Factor;
|
typedef gtsam::FactorBase<Key> FactorType;
|
||||||
|
|
||||||
/** A shared_ptr to this class. Derived classes must redefine this. */
|
/** A shared_ptr to this class. Derived classes must redefine this. */
|
||||||
typedef boost::shared_ptr<This> shared_ptr;
|
typedef boost::shared_ptr<This> shared_ptr;
|
||||||
|
|
||||||
/** Iterator over keys */
|
/** Iterator over keys */
|
||||||
typedef typename Factor::iterator iterator;
|
typedef typename FactorType::iterator iterator;
|
||||||
|
|
||||||
/** Const iterator over keys */
|
/** Const iterator over keys */
|
||||||
typedef typename Factor::const_iterator const_iterator;
|
typedef typename FactorType::const_iterator const_iterator;
|
||||||
|
|
||||||
/** View of the frontal keys (call frontals()) */
|
/** View of the frontal keys (call frontals()) */
|
||||||
typedef boost::iterator_range<const_iterator> Frontals;
|
typedef boost::iterator_range<const_iterator> Frontals;
|
||||||
|
@ -90,20 +90,20 @@ public:
|
||||||
ConditionalBase() : nrFrontals_(0) {}
|
ConditionalBase() : nrFrontals_(0) {}
|
||||||
|
|
||||||
/** No parents */
|
/** No parents */
|
||||||
ConditionalBase(Key key) : Factor(key), nrFrontals_(1) {}
|
ConditionalBase(Key key) : FactorType(key), nrFrontals_(1) {}
|
||||||
|
|
||||||
/** Single parent */
|
/** Single parent */
|
||||||
ConditionalBase(Key key, Key parent) : Factor(key, parent), nrFrontals_(1) {}
|
ConditionalBase(Key key, Key parent) : FactorType(key, parent), nrFrontals_(1) {}
|
||||||
|
|
||||||
/** Two parents */
|
/** Two parents */
|
||||||
ConditionalBase(Key key, Key parent1, Key parent2) : Factor(key, parent1, parent2), nrFrontals_(1) {}
|
ConditionalBase(Key key, Key parent1, Key parent2) : FactorType(key, parent1, parent2), nrFrontals_(1) {}
|
||||||
|
|
||||||
/** Three parents */
|
/** Three parents */
|
||||||
ConditionalBase(Key key, Key parent1, Key parent2, Key parent3) : Factor(key, parent1, parent2, parent3), nrFrontals_(1) {}
|
ConditionalBase(Key key, Key parent1, Key parent2, Key parent3) : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) {}
|
||||||
|
|
||||||
/** Constructor from a frontal variable and a vector of parents */
|
/** Constructor from a frontal variable and a vector of parents */
|
||||||
ConditionalBase(Key key, const std::vector<Key>& parents) : nrFrontals_(1) {
|
ConditionalBase(Key key, const std::vector<Key>& parents) : nrFrontals_(1) {
|
||||||
Factor::keys_.resize(1 + parents.size());
|
FactorType::keys_.resize(1 + parents.size());
|
||||||
*(beginFrontals()) = key;
|
*(beginFrontals()) = key;
|
||||||
std::copy(parents.begin(), parents.end(), beginParents());
|
std::copy(parents.begin(), parents.end(), beginParents());
|
||||||
}
|
}
|
||||||
|
@ -130,28 +130,28 @@ public:
|
||||||
/** check equality */
|
/** check equality */
|
||||||
template<class DERIVED>
|
template<class DERIVED>
|
||||||
bool equals(const DERIVED& c, double tol = 1e-9) const {
|
bool equals(const DERIVED& c, double tol = 1e-9) const {
|
||||||
return nrFrontals_ == c.nrFrontals_ && Factor::equals(c, tol); }
|
return nrFrontals_ == c.nrFrontals_ && FactorType::equals(c, tol); }
|
||||||
|
|
||||||
/** return the number of frontals */
|
/** return the number of frontals */
|
||||||
size_t nrFrontals() const { return nrFrontals_; }
|
size_t nrFrontals() const { return nrFrontals_; }
|
||||||
|
|
||||||
/** return the number of parents */
|
/** return the number of parents */
|
||||||
size_t nrParents() const { return Factor::keys_.size() - nrFrontals_; }
|
size_t nrParents() const { return FactorType::keys_.size() - nrFrontals_; }
|
||||||
|
|
||||||
/** Special accessor when there is only one frontal variable. */
|
/** Special accessor when there is only one frontal variable. */
|
||||||
Key key() const { assert(nrFrontals_==1); return Factor::keys_[0]; }
|
Key key() const { assert(nrFrontals_==1); return FactorType::keys_[0]; }
|
||||||
|
|
||||||
/** Iterators over frontal and parent variables. */
|
/** Iterators over frontal and parent variables. */
|
||||||
const_iterator beginFrontals() const { return Factor::keys_.begin(); }
|
const_iterator beginFrontals() const { return FactorType::keys_.begin(); }
|
||||||
const_iterator endFrontals() const { return Factor::keys_.begin()+nrFrontals_; }
|
const_iterator endFrontals() const { return FactorType::keys_.begin()+nrFrontals_; }
|
||||||
const_iterator beginParents() const { return Factor::keys_.begin()+nrFrontals_; }
|
const_iterator beginParents() const { return FactorType::keys_.begin()+nrFrontals_; }
|
||||||
const_iterator endParents() const { return Factor::keys_.end(); }
|
const_iterator endParents() const { return FactorType::keys_.end(); }
|
||||||
|
|
||||||
/** Mutable iterators and accessors */
|
/** Mutable iterators and accessors */
|
||||||
iterator beginFrontals() { return Factor::keys_.begin(); }
|
iterator beginFrontals() { return FactorType::keys_.begin(); }
|
||||||
iterator endFrontals() { return Factor::keys_.begin()+nrFrontals_; }
|
iterator endFrontals() { return FactorType::keys_.begin()+nrFrontals_; }
|
||||||
iterator beginParents() { return Factor::keys_.begin()+nrFrontals_; }
|
iterator beginParents() { return FactorType::keys_.begin()+nrFrontals_; }
|
||||||
iterator endParents() { return Factor::keys_.end(); }
|
iterator endParents() { return FactorType::keys_.end(); }
|
||||||
boost::iterator_range<iterator> frontals() { return boost::make_iterator_range(beginFrontals(), endFrontals()); }
|
boost::iterator_range<iterator> frontals() { return boost::make_iterator_range(beginFrontals(), endFrontals()); }
|
||||||
boost::iterator_range<iterator> parents() { return boost::make_iterator_range(beginParents(), endParents()); }
|
boost::iterator_range<iterator> parents() { return boost::make_iterator_range(beginParents(), endParents()); }
|
||||||
|
|
||||||
|
@ -226,7 +226,7 @@ void ConditionalBase<KEY>::permuteWithInverse(const Permutation& inversePermutat
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
Factor::permuteWithInverse(inversePermutation);
|
FactorType::permuteWithInverse(inversePermutation);
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -41,7 +41,7 @@ namespace gtsam {
|
||||||
template<class FACTOR>
|
template<class FACTOR>
|
||||||
class FactorGraph: public Testable<FactorGraph<FACTOR> > {
|
class FactorGraph: public Testable<FactorGraph<FACTOR> > {
|
||||||
public:
|
public:
|
||||||
typedef FACTOR Factor;
|
typedef FACTOR FactorType;
|
||||||
typedef boost::shared_ptr<FactorGraph<FACTOR> > shared_ptr;
|
typedef boost::shared_ptr<FactorGraph<FACTOR> > shared_ptr;
|
||||||
typedef typename boost::shared_ptr<FACTOR> sharedFactor;
|
typedef typename boost::shared_ptr<FACTOR> sharedFactor;
|
||||||
typedef typename std::vector<sharedFactor>::iterator iterator;
|
typedef typename std::vector<sharedFactor>::iterator iterator;
|
||||||
|
@ -115,7 +115,7 @@ namespace gtsam {
|
||||||
typename RELATED::shared_ptr ret(new RELATED);
|
typename RELATED::shared_ptr ret(new RELATED);
|
||||||
ret->reserve(this->size());
|
ret->reserve(this->size());
|
||||||
BOOST_FOREACH(const sharedFactor& factor, *this) {
|
BOOST_FOREACH(const sharedFactor& factor, *this) {
|
||||||
typename RELATED::Factor::shared_ptr castedFactor(boost::dynamic_pointer_cast<typename RELATED::Factor>(factor));
|
typename RELATED::FactorType::shared_ptr castedFactor(boost::dynamic_pointer_cast<typename RELATED::FactorType>(factor));
|
||||||
if(castedFactor)
|
if(castedFactor)
|
||||||
ret->push_back(castedFactor);
|
ret->push_back(castedFactor);
|
||||||
else
|
else
|
||||||
|
@ -133,11 +133,11 @@ namespace gtsam {
|
||||||
typename TARGET::shared_ptr ret(new TARGET);
|
typename TARGET::shared_ptr ret(new TARGET);
|
||||||
ret->reserve(this->size());
|
ret->reserve(this->size());
|
||||||
BOOST_FOREACH(const sharedFactor& factor, *this) {
|
BOOST_FOREACH(const sharedFactor& factor, *this) {
|
||||||
typename TARGET::Factor::shared_ptr castedFactor(boost::dynamic_pointer_cast<typename TARGET::Factor>(factor));
|
typename TARGET::FactorType::shared_ptr castedFactor(boost::dynamic_pointer_cast<typename TARGET::FactorType>(factor));
|
||||||
if(castedFactor)
|
if(castedFactor)
|
||||||
ret->push_back(castedFactor);
|
ret->push_back(castedFactor);
|
||||||
else
|
else
|
||||||
ret->push_back(typename TARGET::Factor::shared_ptr(new typename TARGET::Factor(*factor)));
|
ret->push_back(typename TARGET::FactorType::shared_ptr(new typename TARGET::FactorType(*factor)));
|
||||||
}
|
}
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
|
@ -53,7 +53,7 @@ namespace gtsam {
|
||||||
factors.push_back(newFactors);
|
factors.push_back(newFactors);
|
||||||
|
|
||||||
// eliminate into a Bayes net
|
// eliminate into a Bayes net
|
||||||
typename BayesNet<CONDITIONAL>::shared_ptr bayesNet = GenericSequentialSolver<typename CONDITIONAL::Factor>(factors).eliminate();
|
typename BayesNet<CONDITIONAL>::shared_ptr bayesNet = GenericSequentialSolver<typename CONDITIONAL::FactorType>(factors).eliminate();
|
||||||
|
|
||||||
// insert conditionals back in, straight into the topless bayesTree
|
// insert conditionals back in, straight into the topless bayesTree
|
||||||
typename BayesNet<CONDITIONAL>::const_reverse_iterator rit;
|
typename BayesNet<CONDITIONAL>::const_reverse_iterator rit;
|
||||||
|
|
|
@ -37,7 +37,7 @@ namespace gtsam {
|
||||||
|
|
||||||
typedef IndexConditional This;
|
typedef IndexConditional This;
|
||||||
typedef ConditionalBase<Index> Base;
|
typedef ConditionalBase<Index> Base;
|
||||||
typedef IndexFactor Factor;
|
typedef IndexFactor FactorType;
|
||||||
typedef boost::shared_ptr<IndexConditional> shared_ptr;
|
typedef boost::shared_ptr<IndexConditional> shared_ptr;
|
||||||
|
|
||||||
/** Empty Constructor to make serialization possible */
|
/** Empty Constructor to make serialization possible */
|
||||||
|
|
|
@ -89,7 +89,7 @@ namespace gtsam {
|
||||||
Index maxVar = 0;
|
Index maxVar = 0;
|
||||||
for(size_t i=0; i<fg.size(); ++i)
|
for(size_t i=0; i<fg.size(); ++i)
|
||||||
if(fg[i]) {
|
if(fg[i]) {
|
||||||
typename FG::Factor::const_iterator min = std::min_element(fg[i]->begin(), fg[i]->end());
|
typename FG::FactorType::const_iterator min = std::min_element(fg[i]->begin(), fg[i]->end());
|
||||||
if(min == fg[i]->end())
|
if(min == fg[i]->end())
|
||||||
lowestOrdered[i] = numeric_limits<Index>::max();
|
lowestOrdered[i] = numeric_limits<Index>::max();
|
||||||
else {
|
else {
|
||||||
|
@ -171,8 +171,8 @@ namespace gtsam {
|
||||||
// Now that we know which factors and variables, and where variables
|
// Now that we know which factors and variables, and where variables
|
||||||
// come from and go to, create and eliminate the new joint factor.
|
// come from and go to, create and eliminate the new joint factor.
|
||||||
tic(2, "CombineAndEliminate");
|
tic(2, "CombineAndEliminate");
|
||||||
pair<typename BayesNet<typename FG::Factor::Conditional>::shared_ptr, typename FG::sharedFactor> eliminated(
|
pair<typename BayesNet<typename FG::FactorType::Conditional>::shared_ptr, typename FG::sharedFactor> eliminated(
|
||||||
FG::Factor::CombineAndEliminate(fg, current->frontal.size()));
|
FG::FactorType::CombineAndEliminate(fg, current->frontal.size()));
|
||||||
toc(2, "CombineAndEliminate");
|
toc(2, "CombineAndEliminate");
|
||||||
|
|
||||||
assert(std::equal(eliminated.second->begin(), eliminated.second->end(), current->separator.begin()));
|
assert(std::equal(eliminated.second->begin(), eliminated.second->end(), current->separator.begin()));
|
||||||
|
|
|
@ -46,7 +46,7 @@ namespace gtsam {
|
||||||
typedef typename ClusterTree<FG>::Cluster Clique;
|
typedef typename ClusterTree<FG>::Cluster Clique;
|
||||||
typedef typename Clique::shared_ptr sharedClique;
|
typedef typename Clique::shared_ptr sharedClique;
|
||||||
|
|
||||||
typedef class BayesTree<typename FG::Factor::Conditional> BayesTree;
|
typedef class BayesTree<typename FG::FactorType::Conditional> BayesTree;
|
||||||
|
|
||||||
typedef boost::shared_ptr<JunctionTree<FG> > shared_ptr;
|
typedef boost::shared_ptr<JunctionTree<FG> > shared_ptr;
|
||||||
|
|
||||||
|
|
|
@ -42,7 +42,7 @@ class GaussianFactor;
|
||||||
class GaussianConditional : public IndexConditional {
|
class GaussianConditional : public IndexConditional {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
typedef GaussianFactor Factor;
|
typedef GaussianFactor FactorType;
|
||||||
typedef boost::shared_ptr<GaussianConditional> shared_ptr;
|
typedef boost::shared_ptr<GaussianConditional> shared_ptr;
|
||||||
|
|
||||||
/** Store the conditional matrix as upper-triangular column-major */
|
/** Store the conditional matrix as upper-triangular column-major */
|
||||||
|
|
|
@ -45,7 +45,7 @@ public:
|
||||||
|
|
||||||
// update dimensions
|
// update dimensions
|
||||||
BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, newFactors) {
|
BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, newFactors) {
|
||||||
for(typename FACTORGRAPH::Factor::const_iterator key = factor->begin(); key != factor->end(); ++key) {
|
for(typename FACTORGRAPH::FactorType::const_iterator key = factor->begin(); key != factor->end(); ++key) {
|
||||||
if(*key >= dims_.size())
|
if(*key >= dims_.size())
|
||||||
dims_.resize(*key + 1);
|
dims_.resize(*key + 1);
|
||||||
if(dims_[*key] == 0)
|
if(dims_[*key] == 0)
|
||||||
|
|
Loading…
Reference in New Issue