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

Made some progress on symbolic analysis

release/4.3a0
Frank Dellaert 2009-10-28 02:57:38 +00:00
parent 50763ef764
commit 1f792a53ea
9 changed files with 195 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
cpp/BayesChain-inl.h Normal file
View File

@ -0,0 +1,47 @@
/**
* @file BayesChain-inl.h
* @brief Bayes chain template definitions
* @author Frank Dellaert
*/
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include "BayesChain.h"
using namespace std;
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
namespace gtsam {
/* ************************************************************************* */
template<class Conditional>
void BayesChain<Conditional>::print(const string& s) const {
cout << s << ":" << endl;
BOOST_FOREACH(string key, keys_) {
const_iterator it = nodes_.find(key);
it->second->print("\nNode[" + key + "]");
}
}
/* ************************************************************************* */
template<class Conditional>
bool BayesChain<Conditional>::equals(const BayesChain& cbn, double tol) const {
const_iterator it1 = nodes_.begin(), it2 = cbn.nodes_.begin();
if(nodes_.size() != cbn.nodes_.size()) return false;
for(; it1 != nodes_.end(); it1++, it2++) {
const string& j1 = it1->first, j2 = it2->first;
boost::shared_ptr<Conditional> node1 = it1->second, node2 = it2->second;
if (j1 != j2) return false;
if (!node1->equals(*node2,tol))
return false;
}
return true;
}
/* ************************************************************************* */
} // namespace gtsam

23
cpp/BayesChain.h
View File

@ -8,10 +8,12 @@
#pragma once
#include <list>
#include <boost/shared_ptr.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/list.hpp>
#include "Testable.h"
namespace gtsam {
/**
@ -21,8 +23,25 @@ namespace gtsam {
* a ConditionalProbabilityTable, a ConditionalGaussian, or a SymbolicConditional.
*/
template<class Conditional>
class BayesChain {
class BayesChain: public Testable<BayesChain<Conditional> > {
protected:
/** nodes keys stored in topological sort order, i.e. from parents to children */
std::list<std::string> keys_;
/** nodes stored on key */
typedef typename std::map<std::string, boost::shared_ptr<Conditional> > Nodes;
Nodes nodes_;
typedef typename Nodes::const_iterator const_iterator;
public:
/** print */
void print(const std::string& s = "") const;
/** check equality */
bool equals(const BayesChain& other, double tol = 1e-9) const;
};
} /// namespace gtsam

18
cpp/Makefile.am
View File

@ -80,17 +80,14 @@ timeLinearFactor: timeLinearFactor.cpp
timeLinearFactor: CXXFLAGS += -I /opt/local/include
timeLinearFactor: LDFLAGS += -L.libs -lgtsam
# not the correct way, I'm sure: Kai ?
timeLinearFactorGraph: timeLinearFactorGraph.cpp
timeLinearFactorGraph: CXXFLAGS += -I /opt/local/include -I ..
timeLinearFactorGraph: LDFLAGS += SmallExample.o -L.libs -lgtsam -L../CppUnitLite -lCppUnitLite
# graphs
sources += LinearFactorGraph.cpp
sources += SymbolicBayesChain.cpp ChordalBayesNet.cpp
#sources += BayesChain.cpp SymbolicBayesChain.cpp
sources += ChordalBayesNet.cpp
sources += ConstrainedNonlinearFactorGraph.cpp ConstrainedLinearFactorGraph.cpp
check_PROGRAMS += testFactorgraph testLinearFactorGraph testNonlinearFactorGraph testNonlinearOptimizer
check_PROGRAMS += testSymbolicBayesChain testChordalBayesNet testBayesTree
check_PROGRAMS += testFactorgraph testLinearFactorGraph testNonlinearFactorGraph
check_PROGRAMS += testChordalBayesNet testNonlinearOptimizer
#check_PROGRAMS += testSymbolicBayesChain testBayesTree
check_PROGRAMS += testConstrainedNonlinearFactorGraph testConstrainedLinearFactorGraph
testFactorgraph_SOURCES = testFactorgraph.cpp
testLinearFactorGraph_SOURCES = $(example) testLinearFactorGraph.cpp
@ -112,6 +109,11 @@ testBayesTree_LDADD = libgtsam.la
testConstrainedNonlinearFactorGraph_LDADD = libgtsam.la
testConstrainedLinearFactorGraph_LDADD = libgtsam.la
# not the correct way, I'm sure: Kai ?
timeLinearFactorGraph: timeLinearFactorGraph.cpp
timeLinearFactorGraph: CXXFLAGS += -I /opt/local/include -I ..
timeLinearFactorGraph: LDFLAGS += SmallExample.o -L.libs -lgtsam -L../CppUnitLite -lCppUnitLite
# geometry
sources += Point2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
check_PROGRAMS += testPoint2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2

17
cpp/SymbolicBayesChain-inl.h
View File

@ -4,14 +4,21 @@
* @author Frank Dellaert
*/
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include "SymbolicBayesChain.h"
using namespace std;
using namespace gtsam;
namespace gtsam {
/* ************************************************************************* */
template<class Factor, class Config>
SymbolicBayesChain::SymbolicBayesChain(
const FactorGraph<Factor, Config>& factorGraph, const Ordering& ordering) {
}
/* ************************************************************************* */
template<class Factor, class Config>
SymbolicBayesChain::SymbolicBayesChain(const FactorGraph<Factor,Config>& factorGraph) {
}
/* ************************************************************************* */
} // namespace gtsam

43
cpp/SymbolicBayesChain.cpp
View File

@ -4,40 +4,29 @@
* @author Frank Dellaert
*/
#include <stdarg.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
#include "SymbolicBayesChain.h"
using namespace std;
using namespace gtsam;
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
namespace gtsam {
typedef pair<string,SymbolicConditional::shared_ptr> pp;
/* ************************************************************************* */
SymbolicBayesChain::SymbolicBayesChain(const std::map<std::string,
SymbolicConditional::shared_ptr>& nodes) {
BOOST_FOREACH(pp p, nodes) {
keys_.push_front(p.first);
nodes_.insert(p);
}
}
/* ************************************************************************* */
void SymbolicBayesChain::print(const string& s) const {
// BOOST_FOREACH(string key, keys) {
// const_iterator it = nodes.find(key);
// it->second->print("\nNode[" + key + "]");
// }
}
/* ************************************************************************* */
bool SymbolicBayesChain::equals(const SymbolicBayesChain& cbn, double tol) const {
// const_iterator it1 = nodes.begin(), it2 = cbn.nodes.begin();
//
// if(nodes.size() != cbn.nodes.size()) return false;
// for(; it1 != nodes.end(); it1++, it2++){
// const string& j1 = it1->first, j2 = it2->first;
// ConditionalGaussian::shared_ptr node1 = it1->second, node2 = it2->second;
// if (j1 != j2) return false;
// if (!node1->equals(*node2,tol))
// return false;
// }
// return true;
return false;
}
/* ************************************************************************* */
} // namespace gtsam

25
cpp/SymbolicBayesChain.h
View File

@ -15,37 +15,34 @@
#include "Testable.h"
#include "BayesChain.h"
#include "FactorGraph.h"
#include "SymbolicConditional.h"
namespace gtsam {
/**
* Conditional node for use in a symbolic Bayes chain
*/
class SymbolicConditional {
};
class Ordering;
/**
* Symbolic Bayes Chain, the (symbolic) result of eliminating a factor graph
*/
class SymbolicBayesChain: public BayesChain<SymbolicConditional> ,
public Testable<SymbolicBayesChain> {
class SymbolicBayesChain: public BayesChain<SymbolicConditional> {
public:
/**
* Construct from a map of nodes
*/
SymbolicBayesChain(const std::map<std::string,
SymbolicConditional::shared_ptr>& nodes);
/**
* Construct from any factor graph
*/
template<class Factor, class Config>
SymbolicBayesChain(const FactorGraph<Factor, Config>& factorGraph);
SymbolicBayesChain(const FactorGraph<Factor, Config>& factorGraph,
const Ordering& ordering);
/** Destructor */
virtual ~SymbolicBayesChain() {
}
/** print */
void print(const std::string& s = "") const;
/** check equality */
bool equals(const SymbolicBayesChain& other, double tol = 1e-9) const;
};
} /// namespace gtsam

53
cpp/SymbolicConditional.h Normal file
View File

@ -0,0 +1,53 @@
/**
* @file SymbolicConditional.h
* @brief Symbolic Conditional node for use in Bayes nets
* @author Frank Dellaert
*/
// \callgraph
#pragma once
#include "Testable.h"
namespace gtsam {
/**
* Conditional node for use in a Bayes nets
*/
class SymbolicConditional: Testable<SymbolicConditional> {
public:
typedef boost::shared_ptr<SymbolicConditional> shared_ptr;
/**
* No parents
*/
SymbolicConditional() {
}
/**
* Single parent
*/
SymbolicConditional(const std::string& key) {
}
/**
* Two parents
*/
SymbolicConditional(const std::string& key1, const std::string& key2) {
}
/** print */
void print(const std::string& s = "SymbolicConditional") const {
std::cout << s << std::endl;
}
/** check equality */
bool equals(const SymbolicConditional& other, double tol = 1e-9) const {
return false;
}
};
} /// namespace gtsam

7
cpp/testBayesTree.cpp
View File

@ -6,6 +6,7 @@
#include <CppUnitLite/TestHarness.h>
#include "BayesChain-inl.h"
#include "SymbolicBayesChain-inl.h"
#include "smallExample.h"
#include "BayesTree.h"
@ -16,7 +17,11 @@ using namespace gtsam;
TEST( BayesTree, constructor )
{
LinearFactorGraph factorGraph = createLinearFactorGraph();
SymbolicBayesChain symbolicBayesChain(factorGraph);
Ordering ordering;
ordering.push_back("x2");
ordering.push_back("l1");
ordering.push_back("x1");
SymbolicBayesChain symbolicBayesChain(factorGraph,ordering);
BayesTree<SymbolicConditional> bayesTree(symbolicBayesChain);
}

20
cpp/testSymbolicBayesChain.cpp
View File

@ -7,6 +7,7 @@
#include <CppUnitLite/TestHarness.h>
#include "smallExample.h"
#include "BayesChain-inl.h"
#include "SymbolicBayesChain-inl.h"
using namespace gtsam;
@ -14,8 +15,25 @@ using namespace gtsam;
/* ************************************************************************* */
TEST( SymbolicBayesChain, constructor )
{
// Create manually
SymbolicConditional::shared_ptr x2(new SymbolicConditional("x1","l1"));
SymbolicConditional::shared_ptr l1(new SymbolicConditional("x1"));
SymbolicConditional::shared_ptr x1(new SymbolicConditional());
map<string, SymbolicConditional::shared_ptr> nodes;
nodes.insert(make_pair("x2",x2));
nodes.insert(make_pair("l1",l1));
nodes.insert(make_pair("x1",x1));
SymbolicBayesChain expected(nodes);
// Create from a factor graph
Ordering ordering;
ordering.push_back("x2");
ordering.push_back("l1");
ordering.push_back("x1");
LinearFactorGraph factorGraph = createLinearFactorGraph();
SymbolicBayesChain symbolicChordalBayesNet(factorGraph);
SymbolicBayesChain actual(factorGraph,ordering);
//CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */