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

Ordering is now a list and Testable

release/4.3a0
Frank Dellaert 2009-10-31 15:24:22 +00:00
parent e1f14b34c3
commit fd5f43092f
12 changed files with 75 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
cpp/BayesChain-inl.h
View File

@ -30,14 +30,14 @@ namespace gtsam {
/* ************************************************************************* */
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;
if(size() != cbn.size()) return false;
if(keys_ != cbn.keys_) return false;
string key;
boost::shared_ptr<Conditional> node;
FOREACH_PAIR( key, node, nodes_) {
const_iterator cg = cbn.nodes_.find(key);
if (cg == nodes_.end()) return false;
if (!equals_star(node,cg->second,tol)) return false;
}
return true;
}

5
cpp/LinearFactorGraph.cpp
View File

@ -15,7 +15,6 @@
#include "ChordalBayesNet.h"
#include "FactorGraph-inl.h"
#include "LinearFactorGraph.h"
#include "SymbolicBayesChain-inl.h"
using namespace std;
using namespace gtsam;
@ -23,10 +22,6 @@ using namespace gtsam;
// Explicitly instantiate so we don't have to include everywhere
template class FactorGraph<LinearFactor>;
// explicitly instantiate conversion from LinearFG to SymbolicFG
template SymbolicBayesChain::SymbolicBayesChain
(FactorGraph<LinearFactor> const&, Ordering const&);
/* ************************************************************************* */
LinearFactorGraph::LinearFactorGraph(const ChordalBayesNet& CBN)
{

30
cpp/Ordering.cpp
View File

@ -5,39 +5,23 @@
*/
#include <iostream>
#include <string.h> //Added for linux compatibility
#include <boost/foreach.hpp>
#include "Ordering.h"
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
void Ordering::print() const {
std::cout << "Ordering:" << std::endl;
printf("Ordering size: %d \n", (int)this->size());
for(size_t i = 0; i < this->size(); i++)
cout << (*this)[i] << " ";
void Ordering::print(const string& s) const {
cout << s;
BOOST_FOREACH(string key, *this)
cout << " " << key;
cout << endl;
}
/* ************************************************************************* */
bool Ordering::equals(Ordering &ord){
if(this->size() != ord.size())
return false;
vector<std::string>::iterator key;
vector<std::string>::iterator ord_key;
for(key = this->begin(); key != this->end(); key++){
for(ord_key = ord.begin(); ord_key != ord.end(); ord_key++){
if(strcmp((*key).c_str(), (*ord_key).c_str()) == 0)
break;
if(key == this->end())
return false;
}
}
return true;
bool Ordering::equals(const Ordering &other, double tol) const {
return *this == other;
}
/* ************************************************************************* */

14
cpp/Ordering.h
View File

@ -6,8 +6,9 @@
#pragma once
#include <vector>
#include <list>
#include <string>
#include "Testable.h"
namespace gtsam {
@ -15,7 +16,7 @@ namespace gtsam {
* @class Ordering
* @brief ordering of indices for eliminating a factor graph
*/
class Ordering: public std::vector<std::string> {
class Ordering: public std::list<std::string>, public Testable<Ordering> {
public:
/**
* Default constructor creates empty ordering
@ -25,20 +26,19 @@ namespace gtsam {
/**
* Copy constructor from string vector
* TODO: should take reference?
*/
Ordering(std::vector<std::string> strings_in) :
std::vector<std::string>(strings_in) {
Ordering(const std::list<std::string>& strings_in) :
std::list<std::string>(strings_in) {
}
void print() const;
void print(const std::string& s = "Ordering") const;
/**
* check if two orderings are the same
* @param ordering
* @return bool
*/
bool equals(Ordering &ord);
bool equals(const Ordering &ord, double tol=0) const;
};
}

24
cpp/SymbolicBayesChain-inl.h
View File

@ -1,24 +0,0 @@
/**
* @file SymbolicBayesChain-inl.h
* @brief Template definitions for SymbolicBayesChain
* @author Frank Dellaert
*/
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include "SymbolicBayesChain.h"
using namespace std;
namespace gtsam {
/* ************************************************************************* */
template<class Factor>
SymbolicBayesChain::SymbolicBayesChain(
const FactorGraph<Factor>& factorGraph, const Ordering& ordering) {
}
/* ************************************************************************* */
} // namespace gtsam

7
cpp/SymbolicBayesChain.h
View File

@ -41,13 +41,6 @@ namespace gtsam {
SymbolicBayesChain(const std::map<std::string,
SymbolicConditional::shared_ptr>& nodes);
/**
* Construct from any factor graph
*/
template<class Factor>
SymbolicBayesChain(const FactorGraph<Factor>& factorGraph,
const Ordering& ordering);
/** Destructor */
virtual ~SymbolicBayesChain() {
}

6
cpp/testBayesTree.cpp
View File

@ -5,15 +5,15 @@
*/
#include <boost/assign/list_inserter.hpp> // for 'insert()'
#include <boost/assign/std/vector.hpp> // for operator +=
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include "SymbolicBayesChain.h"
#include "BayesTree-inl.h"
//using namespace std;
using namespace gtsam;
using namespace boost::assign;
// Conditionals for ASIA example from the tutorial with A and D evidence
SymbolicConditional::shared_ptr

6
cpp/testLinearFactor.cpp
View File

@ -8,6 +8,9 @@
#include <iostream>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include "Matrix.h"
@ -450,8 +453,7 @@ TEST( LinearFactor, matrix )
// render with a given ordering
Ordering ord;
ord.push_back("x1");
ord.push_back("x2");
ord += "x1","x2";
Matrix A; Vector b;
boost::tie(A,b) = lf->matrix(ord);

37
cpp/testLinearFactorGraph.cpp
View File

@ -4,13 +4,15 @@
* @author Christian Potthast
**/
/*STL/C++*/
#include <string.h>
#include <iostream>
using namespace std;
#include <CppUnitLite/TestHarness.h>
#include <string.h>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include "Matrix.h"
#include "smallExample.h"
@ -271,16 +273,14 @@ TEST( LinearFactorGraph, eliminateAll )
ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian(d3, R3, "l1", A21, "x1", A22));
ChordalBayesNet expected;
expected.insert("x1", cg1);
expected.insert("l1", cg2);
expected.insert("x2", cg3);
expected.insert("l1", cg2);
expected.insert("x1", cg1);
// Check one ordering
LinearFactorGraph fg1 = createLinearFactorGraph();
Ordering ord1;
ord1.push_back("x2");
ord1.push_back("l1");
ord1.push_back("x1");
ord1 += "x2","l1","x1";
ChordalBayesNet::shared_ptr actual = fg1.eliminate(ord1);
CHECK(assert_equal(expected,*actual,tol));
}
@ -310,9 +310,7 @@ TEST( LinearFactorGraph, copying )
// now eliminate the copy
Ordering ord1;
ord1.push_back("x2");
ord1.push_back("l1");
ord1.push_back("x1");
ord1 += "x2","l1","x1";
ChordalBayesNet::shared_ptr actual1 = copy.eliminate(ord1);
// Create the same graph, but not by copying
@ -330,9 +328,7 @@ TEST( LinearFactorGraph, matrix )
// render with a given ordering
Ordering ord;
ord.push_back("x2");
ord.push_back("l1");
ord.push_back("x1");
ord += "x2","l1","x1";
Matrix A; Vector b;
boost::tie(A,b) = fg.matrix(ord);
@ -361,10 +357,7 @@ TEST( LinearFactorGraph, CONSTRUCTOR_ChordalBayesNet )
// render with a given ordering
Ordering ord;
ord.push_back("x2");
ord.push_back("l1");
ord.push_back("x1");
ord += "x2","l1","x1";
ChordalBayesNet::shared_ptr CBN = fg.eliminate(ord);
LinearFactorGraph fg2(*CBN);
ChordalBayesNet::shared_ptr CBN2 = fg2.eliminate(ord);
@ -375,11 +368,11 @@ TEST( LinearFactorGraph, CONSTRUCTOR_ChordalBayesNet )
/* ************************************************************************* */
TEST( LinearFactorGraph, GET_ORDERING)
{
Ordering expected;
expected += "l1","x1","x2";
LinearFactorGraph fg = createLinearFactorGraph();
Ordering ord = fg.getOrdering();
CHECK(ord[0] == string("l1"));
CHECK(ord[1] == string("x1"));
CHECK(ord[2] == string("x2"));
Ordering actual = fg.getOrdering();
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */

16
cpp/testNonlinearOptimizer.cpp
View File

@ -7,10 +7,14 @@
#include <iostream>
using namespace std;
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include "Matrix.h"
#include "smallExample.h"
// template definitions
#include "NonlinearFactorGraph-inl.h"
#include "NonlinearOptimizer-inl.h"
@ -43,27 +47,21 @@ TEST( NonlinearOptimizer, delta )
// Check one ordering
Ordering ord1;
ord1.push_back("x2");
ord1.push_back("l1");
ord1.push_back("x1");
ord1 += "x2","l1","x1";
Optimizer optimizer1(fg, ord1, initial);
VectorConfig actual1 = optimizer1.linearizeAndOptimizeForDelta();
CHECK(assert_equal(actual1,expected));
// Check another
Ordering ord2;
ord2.push_back("x1");
ord2.push_back("x2");
ord2.push_back("l1");
ord2 += "x1","x2","l1";
Optimizer optimizer2(fg, ord2, initial);
VectorConfig actual2 = optimizer2.linearizeAndOptimizeForDelta();
CHECK(assert_equal(actual2,expected));
// And yet another...
Ordering ord3;
ord3.push_back("l1");
ord3.push_back("x1");
ord3.push_back("x2");
ord3 += "l1","x1","x2";
Optimizer optimizer3(fg, ord3, initial);
VectorConfig actual3 = optimizer3.linearizeAndOptimizeForDelta();
CHECK(assert_equal(actual3,expected));

22
cpp/testSymbolicBayesChain.cpp
View File

@ -5,13 +5,14 @@
*/
#include <boost/assign/list_inserter.hpp> // for 'insert()'
#include <boost/assign/std/vector.hpp> // for operator +=
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include "smallExample.h"
#include "SymbolicBayesChain.h"
#include "SymbolicFactorGraph.h"
using namespace std;
using namespace gtsam;
@ -21,21 +22,24 @@ TEST( SymbolicBayesChain, constructor )
{
// Create manually
SymbolicConditional::shared_ptr
x2(new SymbolicConditional("x1", "l1")),
x2(new SymbolicConditional("l1", "x1")),
l1(new SymbolicConditional("x1")),
x1(new SymbolicConditional());
map<string, SymbolicConditional::shared_ptr> nodes;
insert(nodes)("x2", x2)("l1", l1)("x1", x1);
SymbolicBayesChain expected(nodes);
SymbolicBayesChain expected;
expected.insert("x2",x2);
expected.insert("l1",l1);
expected.insert("x1",x1);
// Create from a factor graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
SymbolicFactorGraph fg(factorGraph);
// eliminate it
Ordering ordering;
ordering += "x2","l1","x1";
LinearFactorGraph factorGraph = createLinearFactorGraph();
SymbolicBayesChain actual(factorGraph, ordering);
CHECK(assert_equal(expected, actual));
SymbolicBayesChain::shared_ptr actual = fg.eliminate(ordering);
//bayesChain.ordering();
CHECK(assert_equal(expected, *actual));
}
/* ************************************************************************* */

21
cpp/testSymbolicFactorGraph.cpp
View File

@ -4,6 +4,9 @@
* @author Frank Dellaert
*/
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include "smallExample.h"
@ -19,7 +22,7 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph )
// construct expected symbolic graph
SymbolicFactorGraph expected;
list<string> f1_keys; f1_keys.push_back("x1");
list<string> f1_keys; f1_keys += "x1";
SymbolicFactor::shared_ptr f1(new SymbolicFactor(f1_keys));
expected.push_back(f1);
@ -120,14 +123,14 @@ TEST( LinearFactorGraph, eliminateOne )
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"));
SymbolicConditional::shared_ptr x2(new SymbolicConditional("l1", "x1"));
SymbolicConditional::shared_ptr l1(new SymbolicConditional("x1"));
SymbolicConditional::shared_ptr x1(new SymbolicConditional());
SymbolicBayesChain expected;
expected.insert("x1", c1);
expected.insert("l1", c2);
expected.insert("x2", c3);
expected.insert("x2", x2);
expected.insert("l1", l1);
expected.insert("x1", x1);
// create a test graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
@ -135,9 +138,7 @@ TEST( LinearFactorGraph, eliminate )
// eliminate it
Ordering ordering;
ordering.push_back("x2");
ordering.push_back("l1");
ordering.push_back("x1");
ordering += "x2","l1","x1";
SymbolicBayesChain::shared_ptr actual = fg.eliminate(ordering);
CHECK(assert_equal(expected,*actual));