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

Changed names and moved two LinearFactorGraph functions to FactorGraph 

Added SymbolicFactorGraph compiulation unit and unit tests
Added symbolic combine constructor
release/4.3a0
Frank Dellaert 2009-10-29 05:39:13 +00:00
parent f0c23a2828
commit a513ae0287
13 changed files with 278 additions and 174 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -492,6 +492,14 @@
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>

2
cpp/ConstrainedLinearFactorGraph.cpp
View File

@ -107,7 +107,7 @@ ConstrainedConditionalGaussian::shared_ptr ConstrainedLinearFactorGraph::elimina
ConstrainedConditionalGaussian::shared_ptr ccg = constraint->eliminate(key);
// perform a change of variables on the linear factors in the separator
vector<LinearFactor::shared_ptr> separator = find_factors_and_remove(key);
vector<LinearFactor::shared_ptr> separator = findAndRemoveFactors(key);
BOOST_FOREACH(LinearFactor::shared_ptr factor, separator) {
// store the block matrices
map<string, Matrix> blocks;

17
cpp/FactorGraph-inl.h
View File

@ -171,13 +171,13 @@ list<int> FactorGraph<Factor>::factors(const string& key) const {
/* ************************************************************************* */
template<class Factor>
vector<boost::shared_ptr<Factor> >
FactorGraph<Factor>::find_factors_and_remove(const string& key) {
FactorGraph<Factor>::findAndRemoveFactors(const string& key) {
vector<boost::shared_ptr<Factor> > found;
Indices::iterator it = indices_.find(key);
if (it == indices_.end())
throw(std::invalid_argument
("FactorGraph::find_factors_and_remove invalid key: " + key));
("FactorGraph::findAndRemoveFactors invalid key: " + key));
list<int> *indices_ptr; // pointer to indices list in indices_ map
indices_ptr = &(it->second);
@ -190,5 +190,18 @@ FactorGraph<Factor>::find_factors_and_remove(const string& key) {
return found;
}
/* ************************************************************************* */
/* find factors and remove them from the factor graph: O(n) */
/* ************************************************************************* */
template<class Factor>
boost::shared_ptr<Factor>
FactorGraph<Factor>::removeAndCombineFactors(const string& key)
{
typedef typename boost::shared_ptr<Factor> shared_factor;
vector<shared_factor> found = findAndRemoveFactors(key);
shared_factor new_factor(new Factor(found));
return new_factor;
}
/* ************************************************************************* */
}

9
cpp/FactorGraph.h
View File

@ -93,7 +93,14 @@ namespace gtsam {
* from the factor graph
* @param key the key for the given node
*/
std::vector<shared_factor> find_factors_and_remove(const std::string& key);
std::vector<shared_factor> findAndRemoveFactors(const std::string& key);
/**
* extract and combine all the factors that involve a given node
* @param key the key for the given node
* @return the combined linear factor
*/
shared_factor removeAndCombineFactors(const std::string& key);
private:

14
cpp/LinearFactorGraph.cpp
View File

@ -13,6 +13,7 @@
#include <colamd/colamd.h>
#include "ChordalBayesNet.h"
#include "FactorGraph-inl.h"
#include "LinearFactorGraph.h"
using namespace std;
@ -47,17 +48,6 @@ set<string> LinearFactorGraph::find_separator(const string& key) const
return separator;
}
/* ************************************************************************* */
/* find factors and remove them from the factor graph: O(n) */
/* ************************************************************************* */
boost::shared_ptr<LinearFactor>
LinearFactorGraph::combine_factors(const string& key)
{
vector<LinearFactor::shared_ptr> found = find_factors_and_remove(key);
boost::shared_ptr<LinearFactor> lf(new LinearFactor(found));
return lf;
}
/* ************************************************************************* */
/* eliminate one node from the linear factor graph */
/* ************************************************************************* */
@ -65,7 +55,7 @@ ConditionalGaussian::shared_ptr LinearFactorGraph::eliminate_one(const string& k
{
// combine the factors of all nodes connected to the variable to be eliminated
// if no factors are connected to key, returns an empty factor
boost::shared_ptr<LinearFactor> joint_factor = combine_factors(key);
boost::shared_ptr<LinearFactor> joint_factor = removeAndCombineFactors(key);
// eliminate that joint factor
try {

11
cpp/LinearFactorGraph.h
View File

@ -67,17 +67,6 @@ namespace gtsam {
*/
std::set<std::string> find_separator(const std::string& key) const;
/**
* extract and combine all the factors that involve a given node
* NOTE: the combined factor will be depends on a system-dependent
* ordering of the input set of factors. Do not rely on this order
* when using the function.
* @param key the key for the given node
* @return the combined linear factor
*/
boost::shared_ptr<LinearFactor>
combine_factors(const std::string& key);
/**
* eliminate one node yielding a ConditionalGaussian
* Eliminates the factors from the factor graph through find_factors_and_remove

10
cpp/Makefile.am
View File

@ -81,15 +81,16 @@ timeLinearFactor: CXXFLAGS += -I /opt/local/include
timeLinearFactor: LDFLAGS += -L.libs -lgtsam
# graphs
sources += LinearFactorGraph.cpp
sources += SymbolicBayesChain.cpp
sources += ChordalBayesNet.cpp
sources += SymbolicFactorGraph.cpp LinearFactorGraph.cpp
sources += SymbolicBayesChain.cpp ChordalBayesNet.cpp
sources += ConstrainedNonlinearFactorGraph.cpp ConstrainedLinearFactorGraph.cpp
check_PROGRAMS += testFactorgraph testLinearFactorGraph testNonlinearFactorGraph
check_PROGRAMS += testFactorgraph testSymbolicFactorGraph
check_PROGRAMS += testLinearFactorGraph testNonlinearFactorGraph
check_PROGRAMS += testChordalBayesNet testNonlinearOptimizer
check_PROGRAMS += testSymbolicBayesChain testBayesTree
check_PROGRAMS += testConstrainedNonlinearFactorGraph testConstrainedLinearFactorGraph
testFactorgraph_SOURCES = testFactorgraph.cpp
testSymbolicFactorGraph_SOURCES = $(example) testSymbolicFactorGraph.cpp
testLinearFactorGraph_SOURCES = $(example) testLinearFactorGraph.cpp
testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp
testNonlinearOptimizer_SOURCES = $(example) testNonlinearOptimizer.cpp
@ -100,6 +101,7 @@ testConstrainedNonlinearFactorGraph_SOURCES = $(example) testConstrainedNonlinea
testConstrainedLinearFactorGraph_SOURCES = $(example) testConstrainedLinearFactorGraph.cpp
testFactorgraph_LDADD = libgtsam.la
testSymbolicFactorGraph_LDADD = libgtsam.la
testLinearFactorGraph_LDADD = libgtsam.la
testNonlinearFactorGraph_LDADD = libgtsam.la
testNonlinearOptimizer_LDADD = libgtsam.la

47
cpp/SymbolicFactorGraph.cpp Normal file
View File

@ -0,0 +1,47 @@
/*
* SymbolicFactorGraph.cpp
*
* Created on: Oct 29, 2009
* Author: Frank Dellaert
*/
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include "SymbolicFactorGraph.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_;
}
/* ************************************************************************* */
}

76
cpp/SymbolicFactorGraph.h Normal file
View File

@ -0,0 +1,76 @@
/*
* SymbolicFactorGraph.h
*
* Created on: Oct 29, 2009
* Author: Frank Dellaert
*/
#ifndef SYMBOLICFACTORGRAPH_H_
#define SYMBOLICFACTORGRAPH_H_
#include <string>
#include <list>
#include "FactorGraph.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_;
}
};
/** Symbolic Factor Graph */
class SymbolicFactorGraph: public FactorGraph<SymbolicFactor> {
public:
SymbolicFactorGraph() {
}
template<class Factor>
SymbolicFactorGraph(const FactorGraph<Factor>& fg) {
for (size_t i = 0; i < fg.size(); i++) {
boost::shared_ptr<Factor> f = fg[i];
std::list<std::string> keys = f->keys();
SymbolicFactor::shared_ptr factor(new SymbolicFactor(keys));
push_back(factor);
}
}
};
}
#endif /* SYMBOLICFACTORGRAPH_H_ */

1
cpp/testConstrainedNonlinearFactorGraph.cpp
View File

@ -7,6 +7,7 @@
#include <CppUnitLite/TestHarness.h>
#include "FactorGraph-inl.h"
#include "ConstrainedNonlinearFactorGraph.h"
#include "smallExample.h"

24
cpp/testLinearFactorGraph.cpp
View File

@ -63,20 +63,13 @@ TEST( LinearFactorGraph, find_separator )
}
/* ************************************************************************* */
// Note: This test fails on different systems due to a dependency on the ordering
// of LinearFactor::shared_ptr in STL sets. Because the ordering is based
// on pointer values instead of factor values, the ordering is different
// between separate systems.
TEST( LinearFactorGraph, combine_factors_x1 )
{
// create a small example for a linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
// combine all factors
LinearFactor::shared_ptr actual = fg.combine_factors("x1");
//FIXME: this expected value must be constructed in the order that
// the factors are stored in a set - which differs between systems
LinearFactor::shared_ptr actual = fg.removeAndCombineFactors("x1");
// the expected linear factor
Matrix Al1 = Matrix_(6,2,
@ -128,10 +121,7 @@ TEST( LinearFactorGraph, combine_factors_x2 )
LinearFactorGraph fg = createLinearFactorGraph();
// combine all factors
LinearFactor::shared_ptr actual = fg.combine_factors("x2");
// FIXME: change the ordering of the expected to match whatever the real
// ordering is and constructed the expected with the correct order
LinearFactor::shared_ptr actual = fg.removeAndCombineFactors("x2");
// the expected linear factor
Matrix Al1 = Matrix_(4,2,
@ -476,7 +466,7 @@ TEST( LinearFactorGraph, factor_lookup)
}
/* ************************************************************************* */
TEST( LinearFactorGraph, find_factors_and_remove )
TEST( LinearFactorGraph, findAndRemoveFactors )
{
// create the graph
LinearFactorGraph fg = createLinearFactorGraph();
@ -487,7 +477,7 @@ TEST( LinearFactorGraph, find_factors_and_remove )
LinearFactor::shared_ptr f2 = fg[2];
// call the function
vector<LinearFactor::shared_ptr> factors = fg.find_factors_and_remove("x1");
vector<LinearFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
// Check the factors
CHECK(f0==factors[0]);
@ -499,7 +489,7 @@ TEST( LinearFactorGraph, find_factors_and_remove )
}
/* ************************************************************************* */
TEST( LinearFactorGraph, find_factors_and_remove__twice )
TEST( LinearFactorGraph, findAndRemoveFactors_twice )
{
// create the graph
LinearFactorGraph fg = createLinearFactorGraph();
@ -510,14 +500,14 @@ TEST( LinearFactorGraph, find_factors_and_remove__twice )
LinearFactor::shared_ptr f2 = fg[2];
// call the function
vector<LinearFactor::shared_ptr> factors = fg.find_factors_and_remove("x1");
vector<LinearFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
// Check the factors
CHECK(f0==factors[0]);
CHECK(f1==factors[1]);
CHECK(f2==factors[2]);
factors = fg.find_factors_and_remove("x1");
factors = fg.findAndRemoveFactors("x1");
CHECK(factors.size() == 0);
// CHECK if the factors are deleted from the factor graph

126
cpp/testSymbolicBayesChain.cpp
View File

@ -11,135 +11,9 @@
#include "BayesChain-inl.h"
#include "SymbolicBayesChain-inl.h"
namespace gtsam {
/** Symbolic Factor */
class SymbolicFactor: public Testable<SymbolicFactor> {
private:
std::list<std::string> keys_;
public:
SymbolicFactor(std::list<std::string> keys) :
keys_(keys) {
}
typedef boost::shared_ptr<SymbolicFactor> shared_ptr;
/** print */
void print(const std::string& s = "SymbolicFactor") const {
cout << s << " ";
BOOST_FOREACH(string key, keys_) cout << key << " ";
cout << endl;
}
/** check equality */
bool equals(const SymbolicFactor& other, double tol = 1e-9) const {
return keys_ == other.keys_;
}
/**
* Find all variables
* @return The set of all variable keys
*/
std::list<std::string> keys() const {
return keys_;
}
};
/** Symbolic Factor Graph */
class SymbolicFactorGraph: public FactorGraph<SymbolicFactor> {
public:
SymbolicFactorGraph() {}
template<class Factor>
SymbolicFactorGraph(const FactorGraph<Factor>& fg) {
for (size_t i = 0; i < fg.size(); i++) {
boost::shared_ptr<Factor> f = fg[i];
std::list<std::string> keys = f->keys();
SymbolicFactor::shared_ptr factor(new SymbolicFactor(keys));
push_back(factor);
}
}
};
}
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST( SymbolicFactorGraph, symbolicFactorGraph )
{
// construct expected symbolic graph
SymbolicFactorGraph expected;
list<string> f1_keys; f1_keys.push_back("x1");
SymbolicFactor::shared_ptr f1(new SymbolicFactor(f1_keys));
expected.push_back(f1);
list<string> f2_keys; f2_keys.push_back("x1"); f2_keys.push_back("x2");
SymbolicFactor::shared_ptr f2(new SymbolicFactor(f2_keys));
expected.push_back(f2);
list<string> f3_keys; f3_keys.push_back("l1"); f3_keys.push_back("x1");
SymbolicFactor::shared_ptr f3(new SymbolicFactor(f3_keys));
expected.push_back(f3);
list<string> f4_keys; f4_keys.push_back("l1"); f4_keys.push_back("x2");
SymbolicFactor::shared_ptr f4(new SymbolicFactor(f4_keys));
expected.push_back(f4);
// construct it from the factor graph graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
SymbolicFactorGraph actual(factorGraph);
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( SymbolicFactorGraph, find_factors_and_remove )
{
// construct it from the factor graph graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
SymbolicFactorGraph actual(factorGraph);
SymbolicFactor::shared_ptr f1 = actual[0];
SymbolicFactor::shared_ptr f3 = actual[2];
actual.find_factors_and_remove("x2");
// construct expected graph after find_factors_and_remove
SymbolicFactorGraph expected;
SymbolicFactor::shared_ptr null;
expected.push_back(f1);
expected.push_back(null);
expected.push_back(f3);
expected.push_back(null);
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( SymbolicFactorGraph, factor_lookup)
{
// create a test graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
SymbolicFactorGraph fg(factorGraph);
// ask for all factor indices connected to x1
list<int> x1_factors = fg.factors("x1");
int x1_indices[] = { 0, 1, 2 };
list<int> x1_expected(x1_indices, x1_indices + 3);
CHECK(x1_factors==x1_expected);
// ask for all factor indices connected to x2
list<int> x2_factors = fg.factors("x2");
int x2_indices[] = { 1, 3 };
list<int> x2_expected(x2_indices, x2_indices + 2);
CHECK(x2_factors==x2_expected);
}
/* ************************************************************************* */
TEST( SymbolicBayesChain, constructor )
{

107
cpp/testSymbolicFactorGraph.cpp Normal file
View File

@ -0,0 +1,107 @@
/**
* @file testSymbolicBayesChain.cpp
* @brief Unit tests for a symbolic Bayes chain
* @author Frank Dellaert
*/
#include <CppUnitLite/TestHarness.h>
#include "smallExample.h"
#include "FactorGraph-inl.h"
#include "SymbolicFactorGraph.h"
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST( SymbolicFactorGraph, symbolicFactorGraph )
{
// construct expected symbolic graph
SymbolicFactorGraph expected;
list<string> f1_keys; f1_keys.push_back("x1");
SymbolicFactor::shared_ptr f1(new SymbolicFactor(f1_keys));
expected.push_back(f1);
list<string> f2_keys; f2_keys.push_back("x1"); f2_keys.push_back("x2");
SymbolicFactor::shared_ptr f2(new SymbolicFactor(f2_keys));
expected.push_back(f2);
list<string> f3_keys; f3_keys.push_back("l1"); f3_keys.push_back("x1");
SymbolicFactor::shared_ptr f3(new SymbolicFactor(f3_keys));
expected.push_back(f3);
list<string> f4_keys; f4_keys.push_back("l1"); f4_keys.push_back("x2");
SymbolicFactor::shared_ptr f4(new SymbolicFactor(f4_keys));
expected.push_back(f4);
// construct it from the factor graph graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
SymbolicFactorGraph actual(factorGraph);
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( SymbolicFactorGraph, findAndRemoveFactors )
{
// construct it from the factor graph graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
SymbolicFactorGraph actual(factorGraph);
SymbolicFactor::shared_ptr f1 = actual[0];
SymbolicFactor::shared_ptr f3 = actual[2];
actual.findAndRemoveFactors("x2");
// construct expected graph after find_factors_and_remove
SymbolicFactorGraph expected;
SymbolicFactor::shared_ptr null;
expected.push_back(f1);
expected.push_back(null);
expected.push_back(f3);
expected.push_back(null);
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( SymbolicFactorGraph, factors)
{
// create a test graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
SymbolicFactorGraph fg(factorGraph);
// ask for all factor indices connected to x1
list<int> x1_factors = fg.factors("x1");
int x1_indices[] = { 0, 1, 2 };
list<int> x1_expected(x1_indices, x1_indices + 3);
CHECK(x1_factors==x1_expected);
// ask for all factor indices connected to x2
list<int> x2_factors = fg.factors("x2");
int x2_indices[] = { 1, 3 };
list<int> x2_expected(x2_indices, x2_indices + 2);
CHECK(x2_factors==x2_expected);
}
/* ************************************************************************* */
TEST( LinearFactorGraph, removeAndCombineFactors )
{
// create a test graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
SymbolicFactorGraph fg(factorGraph);
// combine all factors connected to x1
SymbolicFactor::shared_ptr actual = fg.removeAndCombineFactors("x1");
list<string> keys; keys.push_back("l1"); keys.push_back("x1"); keys.push_back("x2");
SymbolicFactor expected(keys);
// check if the two factors are the same
CHECK(assert_equal(expected,*actual));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */