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

Symbolic eliminate now works, new compilation unit SymbolicFactor

release/4.3a0
Frank Dellaert 2009-10-30 03:48:32 +00:00
parent 65b949c008
commit dd3795ad5a
11 changed files with 261 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cpp/BayesChain-inl.h
View File

@ -20,10 +20,10 @@ namespace gtsam {
/* ************************************************************************* */
template<class Conditional>
void BayesChain<Conditional>::print(const string& s) const {
cout << s << ":" << endl;
cout << s << ":\n";
BOOST_FOREACH(string key, keys_) {
const_iterator it = nodes_.find(key);
it->second->print("\nNode[" + key + "]");
it->second->print("Node[" + key + "]");
}
}
@ -42,6 +42,14 @@ namespace gtsam {
return true;
}
/* ************************************************************************* */
template<class Conditional>
void BayesChain<Conditional>::insert
(const string& key, boost::shared_ptr<Conditional> node) {
keys_.push_front(key);
nodes_.insert(make_pair(key,node));
}
/* ************************************************************************* */
} // namespace gtsam

3
cpp/BayesChain.h
View File

@ -42,6 +42,9 @@ namespace gtsam {
/** check equality */
bool equals(const BayesChain& other, double tol = 1e-9) const;
/** insert: use reverse topological sort (i.e. parents last) */
void insert(const std::string& key, boost::shared_ptr<Conditional> node);
};
} /// namespace gtsam

19
cpp/Makefile.am
View File

@ -57,23 +57,28 @@ testVector_LDADD = libgtsam.la
testMatrix_LDADD = libgtsam.la
# nodes
sources += VectorConfig.cpp Ordering.cpp LinearFactor.cpp NonlinearFactor.cpp
sources += VectorConfig.cpp Ordering.cpp
sources += SymbolicFactor.cpp LinearFactor.cpp NonlinearFactor.cpp
sources += ConditionalGaussian.cpp LinearConstraint.cpp ConstrainedConditionalGaussian.cpp
check_PROGRAMS += testVectorConfig testLinearFactor testConditionalGaussian testNonlinearFactor testLinearConstraint testConstrainedConditionalGaussian
check_PROGRAMS += testVectorConfig testSymbolicFactor testLinearFactor
check_PROGRAMS += testConditionalGaussian testNonlinearFactor
check_PROGRAMS += testLinearConstraint testConstrainedConditionalGaussian
example = smallExample.cpp
testVectorConfig_SOURCES = testVectorConfig.cpp
testVectorConfig_SOURCES = testVectorConfig.cpp
testSymbolicFactor_SOURCES = $(example) testSymbolicFactor.cpp
testLinearFactor_SOURCES = $(example) testLinearFactor.cpp
testConditionalGaussian_SOURCES = $(example) testConditionalGaussian.cpp
testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp
testLinearConstraint_SOURCES = $(example) testLinearConstraint.cpp
testConstrainedConditionalGaussian_SOURCES = testConstrainedConditionalGaussian.cpp
testLinearConstraint_SOURCES = $(example) testLinearConstraint.cpp
testConstrainedConditionalGaussian_SOURCES = testConstrainedConditionalGaussian.cpp
testVectorConfig_LDADD = libgtsam.la
testVectorConfig_LDADD = libgtsam.la
testSymbolicFactor_LDADD = libgtsam.la
testLinearFactor_LDADD = libgtsam.la
testConditionalGaussian_LDADD = libgtsam.la
testNonlinearFactor_LDADD = libgtsam.la
testLinearConstraint_LDADD = libgtsam.la
testConstrainedConditionalGaussian_LDADD = libgtsam.la
testConstrainedConditionalGaussian_LDADD = libgtsam.la
# not the correct way, I'm sure: Kai ?
timeLinearFactor: timeLinearFactor.cpp

8
cpp/SymbolicBayesChain.h
View File

@ -27,6 +27,14 @@ namespace gtsam {
class SymbolicBayesChain: public BayesChain<SymbolicConditional> {
public:
/** convenience typename for a shared pointer to this class */
typedef boost::shared_ptr<SymbolicBayesChain> shared_ptr;
/**
* Empty constructor
*/
SymbolicBayesChain() {}
/**
* Construct from a map of nodes
*/

8
cpp/SymbolicConditional.h
View File

@ -8,8 +8,11 @@
#pragma once
#include "Testable.h"
#include <list>
#include <string>
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp> // TODO: make cpp file
#include "Testable.h"
namespace gtsam {
@ -23,6 +26,7 @@ namespace gtsam {
public:
/** convenience typename for a shared pointer to this class */
typedef boost::shared_ptr<SymbolicConditional> shared_ptr;
/**
@ -54,7 +58,7 @@ namespace gtsam {
/** print */
void print(const std::string& s = "SymbolicConditional") const {
std::cout << s << std::endl;
std::cout << s;
BOOST_FOREACH(std::string parent, parents_) std::cout << " " << parent;
std::cout << std::endl;
}

69
cpp/SymbolicFactor.cpp Normal file
View File

@ -0,0 +1,69 @@
/*
* SymbolicFactor.cpp
*
* Created on: Oct 29, 2009
* Author: Frank Dellaert
*/
#include <map>
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include "SymbolicConditional.h"
#include "SymbolicFactor.h"
using namespace std;
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
namespace gtsam {
/* ************************************************************************* */
SymbolicFactor::SymbolicFactor(const vector<shared_ptr> & factors) {
// store keys in a map to make them unique (set is not portable)
map<string, string> map;
BOOST_FOREACH(shared_ptr factor, factors)
BOOST_FOREACH(string key, factor->keys())
map.insert(make_pair(key,key));
// create the unique keys
string key,val;
FOREACH_PAIR(key, val, map)
keys_.push_back(key);
}
/* ************************************************************************* */
void SymbolicFactor::print(const string& s) const {
cout << s << " ";
BOOST_FOREACH(string key, keys_) cout << " " << key;
cout << endl;
}
/* ************************************************************************* */
bool SymbolicFactor::equals(const SymbolicFactor& other, double tol) const {
return keys_ == other.keys_;
}
/* ************************************************************************* */
pair<SymbolicConditional::shared_ptr, SymbolicFactor::shared_ptr>
SymbolicFactor::eliminate(const string& key) const
{
// get keys from input factor
list<string> separator;
BOOST_FOREACH(string j,keys_)
if (j!=key) separator.push_back(j);
// start empty remaining factor to be returned
boost::shared_ptr<SymbolicFactor> lf(new SymbolicFactor(separator));
// create SymbolicConditional on separator
SymbolicConditional::shared_ptr cg (new SymbolicConditional(separator));
return make_pair(cg,lf);
}
/* ************************************************************************* */
}

78
cpp/SymbolicFactor.h Normal file
View File

@ -0,0 +1,78 @@
/*
* SymbolicFactor.h
*
* Created on: Oct 29, 2009
* Author: Frank Dellaert
*/
#ifndef SYMBOLICFACTOR_H_
#define SYMBOLICFACTOR_H_
#include <list>
#include <string>
#include <vector>
#include <boost/shared_ptr.hpp>
#include "Testable.h"
namespace gtsam {
class SymbolicConditional;
/** Symbolic Factor */
class SymbolicFactor: public Testable<SymbolicFactor> {
private:
std::list<std::string> keys_;
public:
typedef boost::shared_ptr<SymbolicFactor> shared_ptr;
/**
* Constructor from a list of keys
*/
SymbolicFactor(std::list<std::string> keys) :
keys_(keys) {
}
/**
* Constructor that combines a set of factors
* @param factors Set of factors to combine
*/
SymbolicFactor(const std::vector<shared_ptr> & factors);
/** print */
void print(const std::string& s = "SymbolicFactor") const;
/** check equality */
bool equals(const SymbolicFactor& other, double tol = 1e-9) const;
/**
* Find all variables
* @return The set of all variable keys
*/
std::list<std::string> keys() const {
return keys_;
}
/**
* eliminate one of the variables connected to this factor
* @param key the key of the node to be eliminated
* @return a new factor and a symbolic conditional on the eliminated variable
*/
std::pair<boost::shared_ptr<SymbolicConditional>, shared_ptr>
eliminate(const std::string& key) const;
/**
* Check if empty factor
*/
inline bool empty() const {
return keys_.empty();
}
};
}
#endif /* SYMBOLICFACTOR_H_ */

54
cpp/SymbolicFactorGraph.cpp
View File

@ -6,59 +6,27 @@
*/
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include "Ordering.h"
#include "SymbolicFactorGraph.h"
#include "BayesChain-inl.h"
#include "SymbolicBayesChain.h"
using namespace std;
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
namespace gtsam {
/* ************************************************************************* */
SymbolicFactor::SymbolicFactor(const vector<shared_ptr> & factors) {
// store keys in a map to make them unique (set is not portable)
map<string, string> map;
BOOST_FOREACH(shared_ptr factor, factors)
BOOST_FOREACH(string key, factor->keys())
map.insert(make_pair(key,key));
// create the unique keys
string key,val;
FOREACH_PAIR(key, val, map)
keys_.push_back(key);
}
/* ************************************************************************* */
void SymbolicFactor::print(const string& s) const {
cout << s << " ";
BOOST_FOREACH(string key, keys_) cout << " " << key;
cout << endl;
}
/* ************************************************************************* */
bool SymbolicFactor::equals(const SymbolicFactor& other, double tol) const {
return keys_ == other.keys_;
}
/* ************************************************************************* */
pair<SymbolicConditional::shared_ptr, SymbolicFactor::shared_ptr>
SymbolicFactor::eliminate(const string& key) const
SymbolicBayesChain::shared_ptr
SymbolicFactorGraph::eliminate(const Ordering& ordering)
{
// get keys from input factor
list<string> separator;
BOOST_FOREACH(string j,keys_)
if (j!=key) separator.push_back(j);
SymbolicBayesChain::shared_ptr bayesChain (new SymbolicBayesChain());
// start empty remaining factor to be returned
boost::shared_ptr<SymbolicFactor> lf(new SymbolicFactor(separator));
BOOST_FOREACH(string key, ordering) {
SymbolicConditional::shared_ptr conditional = eliminateOne<SymbolicConditional>(key);
bayesChain->insert(key,conditional);
}
// create SymbolicConditional on separator
SymbolicConditional::shared_ptr cg (new SymbolicConditional(separator));
return make_pair(cg,lf);
return bayesChain;
}
/* ************************************************************************* */

69
cpp/SymbolicFactorGraph.h
View File

@ -11,72 +11,25 @@
#include <string>
#include <list>
#include "FactorGraph.h"
#include "SymbolicConditional.h"
#include "SymbolicFactor.h"
namespace gtsam {
/** Symbolic Factor */
class SymbolicFactor: public Testable<SymbolicFactor> {
private:
std::list<std::string> keys_;
public:
typedef boost::shared_ptr<SymbolicFactor> shared_ptr;
/**
* Constructor from a list of keys
*/
SymbolicFactor(std::list<std::string> keys) :
keys_(keys) {
}
/**
* Constructor that combines a set of factors
* @param factors Set of factors to combine
*/
SymbolicFactor(const std::vector<shared_ptr> & factors);
/** print */
void print(const std::string& s = "SymbolicFactor") const;
/** check equality */
bool equals(const SymbolicFactor& other, double tol = 1e-9) const;
/**
* Find all variables
* @return The set of all variable keys
*/
std::list<std::string> keys() const {
return keys_;
}
/**
* eliminate one of the variables connected to this factor
* @param key the key of the node to be eliminated
* @return a new factor and a symbolic conditional on the eliminated variable
*/
std::pair<SymbolicConditional::shared_ptr, SymbolicFactor::shared_ptr>
eliminate(const std::string& key) const;
/**
* Check if empty factor
*/
inline bool empty() const {
return keys_.empty();
}
};
class SymbolicBayesChain;
/** Symbolic Factor Graph */
class SymbolicFactorGraph: public FactorGraph<SymbolicFactor> {
public:
/**
* Construct empty factor graph
*/
SymbolicFactorGraph() {
}
/**
* Construct from a factor graph of any type
*/
template<class Factor>
SymbolicFactorGraph(const FactorGraph<Factor>& fg) {
for (size_t i = 0; i < fg.size(); i++) {
@ -87,6 +40,12 @@ namespace gtsam {
}
}
/**
* eliminate factor graph in place(!) in the given order, yielding
* a chordal Bayes net
*/
boost::shared_ptr<SymbolicBayesChain> eliminate(const Ordering& ordering);
};
}

19
cpp/testSymbolicFactor.cpp Normal file
View File

@ -0,0 +1,19 @@
/**
* @file testSymbolicFactor.cpp
* @brief Unit tests for a symbolic Factor
* @author Frank Dellaert
*/
#include <CppUnitLite/TestHarness.h>
#include "SymbolicFactor.h"
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

32
cpp/testSymbolicFactorGraph.cpp
View File

@ -9,6 +9,9 @@
#include "smallExample.h"
#include "FactorGraph-inl.h"
#include "SymbolicFactorGraph.h"
#include "BayesChain-inl.h"
#include "SymbolicConditional.h"
#include "SymbolicBayesChain.h"
using namespace std;
using namespace gtsam;
@ -100,7 +103,7 @@ TEST( LinearFactorGraph, removeAndCombineFactors )
}
/* ************************************************************************* */
TEST( LinearFactorGraph, eliminateOne_x1 )
TEST( LinearFactorGraph, eliminateOne )
{
// create a test graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
@ -116,6 +119,33 @@ TEST( LinearFactorGraph, eliminateOne_x1 )
CHECK(assert_equal(expected,*actual));
}
/* ************************************************************************* */
TEST( LinearFactorGraph, eliminate )
{
// create expected Chordal bayes Net
SymbolicConditional::shared_ptr c1(new SymbolicConditional());
SymbolicConditional::shared_ptr c2(new SymbolicConditional("x1"));
SymbolicConditional::shared_ptr c3(new SymbolicConditional("l1", "x1"));
SymbolicBayesChain expected;
expected.insert("x1", c1);
expected.insert("l1", c2);
expected.insert("x2", c3);
// create a test graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
SymbolicFactorGraph fg(factorGraph);
// eliminate it
Ordering ordering;
ordering.push_back("x2");
ordering.push_back("l1");
ordering.push_back("x1");
SymbolicBayesChain::shared_ptr actual = fg.eliminate(ordering);
CHECK(assert_equal(expected,*actual));
}
/* ************************************************************************* */
int main() {
TestResult tr;