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

Moved tree stuff to graph, removed ordering-inl.h

release/4.3a0
Frank Dellaert 2010-01-18 19:11:22 +00:00
parent 8e3a8f08c8
commit e523b148cf
13 changed files with 154 additions and 168 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cpp/Key.h
View File

@ -8,6 +8,7 @@
#pragma once
#include <list>
#include <iostream>
#include <boost/format.hpp>
#include <boost/serialization/serialization.hpp>
@ -213,8 +214,17 @@ namespace gtsam {
}
};
// Conversion utilities
template<class Key> Symbol key2symbol(Key key) {
return Symbol(key);
}
template<class Key> std::list<Symbol> keys2symbols(std::list<Key> keys) {
std::list<Symbol> symbols;
std::transform(keys.begin(), keys.end(), std::back_inserter(symbols), key2symbol<Key> );
return symbols;
}
} // namespace gtsam

66
cpp/Makefile.am
View File

@ -50,19 +50,18 @@ testMatrix_LDADD = libgtsam.la
# GTSAM basics
# The header files will be installed in ~/include/gtsam
headers = gtsam.h Value.h Testable.h Factor.h Conditional.h
headers += Ordering.h Ordering-inl.h numericalDerivative.h
sources += Ordering.cpp
example = smallExample.cpp
headers += Ordering.h numericalDerivative.h
sources += Ordering.cpp smallExample.cpp
# Symbolic Inference
headers += SymbolicConditional.h
sources += SymbolicFactor.cpp SymbolicFactorGraph.cpp SymbolicBayesNet.cpp
check_PROGRAMS += testSymbolicFactor testSymbolicFactorGraph testSymbolicBayesNet
testSymbolicFactor_SOURCES = $(example) testSymbolicFactor.cpp
testSymbolicFactor_SOURCES = testSymbolicFactor.cpp
testSymbolicFactor_LDADD = libgtsam.la
testSymbolicFactorGraph_SOURCES = $(example) testSymbolicFactorGraph.cpp
testSymbolicFactorGraph_SOURCES = testSymbolicFactorGraph.cpp
testSymbolicFactorGraph_LDADD = libgtsam.la
testSymbolicBayesNet_SOURCES = $(example) testSymbolicBayesNet.cpp
testSymbolicBayesNet_SOURCES = testSymbolicBayesNet.cpp
testSymbolicBayesNet_LDADD = libgtsam.la
# Inference
@ -79,18 +78,18 @@ check_PROGRAMS += testBayesTree testISAM testGaussianISAM testGaussianISAM2
testGraph_SOURCES = testGraph.cpp
testGraph_LDADD = libgtsam.la
testFactorgraph_SOURCES = testFactorgraph.cpp
testInference_SOURCES = $(example) testInference.cpp
testInference_SOURCES = testInference.cpp
testFactorgraph_LDADD = libgtsam.la
testInference_LDADD = libgtsam.la
testOrdering_SOURCES = testOrdering.cpp
testOrdering_LDADD = libgtsam.la
testBayesTree_SOURCES = $(example) testBayesTree.cpp
testBayesTree_SOURCES = testBayesTree.cpp
testBayesTree_LDADD = libgtsam.la
testGaussianISAM_SOURCES = $(example) testGaussianISAM.cpp
testGaussianISAM_SOURCES = testGaussianISAM.cpp
testGaussianISAM_LDADD = libgtsam.la
testGaussianISAM2_SOURCES = $(example) testGaussianISAM2.cpp
testGaussianISAM2_SOURCES = testGaussianISAM2.cpp
testGaussianISAM2_LDADD = libgtsam.la
testISAM_SOURCES = $(example) testISAM.cpp
testISAM_SOURCES = testISAM.cpp
testISAM_LDADD = libgtsam.la
# Binary Inference
@ -105,13 +104,13 @@ sources += NoiseModel.cpp Errors.cpp VectorConfig.cpp GaussianFactor.cpp Gaussia
check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testNoiseModel
testVectorConfig_SOURCES = testVectorConfig.cpp
testVectorConfig_LDADD = libgtsam.la
testGaussianFactor_SOURCES = $(example) testGaussianFactor.cpp
testGaussianFactor_SOURCES = testGaussianFactor.cpp
testGaussianFactor_LDADD = libgtsam.la
testGaussianFactorGraph_SOURCES = $(example) testGaussianFactorGraph.cpp
testGaussianFactorGraph_SOURCES = testGaussianFactorGraph.cpp
testGaussianFactorGraph_LDADD = libgtsam.la
testGaussianConditional_SOURCES = $(example) testGaussianConditional.cpp
testGaussianConditional_SOURCES = testGaussianConditional.cpp
testGaussianConditional_LDADD = libgtsam.la
testGaussianBayesNet_SOURCES = $(example) testGaussianBayesNet.cpp
testGaussianBayesNet_SOURCES = testGaussianBayesNet.cpp
testGaussianBayesNet_LDADD = libgtsam.la
testNoiseModel_SOURCES = testNoiseModel.cpp
testNoiseModel_LDADD = libgtsam.la
@ -120,11 +119,11 @@ testNoiseModel_LDADD = libgtsam.la
headers += iterative-inl.h SubgraphPreconditioner-inl.h
sources += iterative.cpp BayesNetPreconditioner.cpp SubgraphPreconditioner.cpp
check_PROGRAMS += testIterative testBayesNetPreconditioner testSubgraphPreconditioner
testIterative_SOURCES = $(example) testIterative.cpp
testIterative_SOURCES = testIterative.cpp
testIterative_LDADD = libgtsam.la
testBayesNetPreconditioner_SOURCES = $(example) testBayesNetPreconditioner.cpp
testBayesNetPreconditioner_SOURCES = testBayesNetPreconditioner.cpp
testBayesNetPreconditioner_LDADD = libgtsam.la
testSubgraphPreconditioner_SOURCES = $(example) testSubgraphPreconditioner.cpp
testSubgraphPreconditioner_SOURCES = testSubgraphPreconditioner.cpp
testSubgraphPreconditioner_LDADD = libgtsam.la
# Nonlinear inference
@ -132,11 +131,11 @@ headers += Key.h NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h
headers += NonlinearFactor.h
check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testKey
testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp
testNonlinearFactor_SOURCES = testNonlinearFactor.cpp
testNonlinearFactor_LDADD = libgtsam.la
testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp
testNonlinearFactorGraph_SOURCES = testNonlinearFactorGraph.cpp
testNonlinearFactorGraph_LDADD = libgtsam.la
testNonlinearOptimizer_SOURCES = $(example) testNonlinearOptimizer.cpp
testNonlinearOptimizer_SOURCES = testNonlinearOptimizer.cpp
testNonlinearOptimizer_LDADD = libgtsam.la
testKey_SOURCES = testKey.cpp
testKey_LDADD = libgtsam.la
@ -150,9 +149,9 @@ testNonlinearConstraint_SOURCES = testNonlinearConstraint.cpp
testNonlinearConstraint_LDADD = libgtsam.la
testNonlinearEquality_SOURCES = testNonlinearEquality.cpp
testNonlinearEquality_LDADD = libgtsam.la
testSQP_SOURCES = $(example) testSQP.cpp
testSQP_SOURCES = testSQP.cpp
testSQP_LDADD = libgtsam.la
testSQPOptimizer_SOURCES = $(example) testSQPOptimizer.cpp
testSQPOptimizer_SOURCES = testSQPOptimizer.cpp
testSQPOptimizer_LDADD = libgtsam.la
# geometry
@ -199,31 +198,31 @@ headers += LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h
headers +=
sources += pose2SLAM.cpp
check_PROGRAMS += testPose2Factor testPose2Config testPose2SLAM testPose2Prior
testPose2Prior_SOURCES = $(example) testPose2Prior.cpp
testPose2Prior_SOURCES = testPose2Prior.cpp
testPose2Prior_LDADD = libgtsam.la
testPose2Factor_SOURCES = $(example) testPose2Factor.cpp
testPose2Factor_SOURCES = testPose2Factor.cpp
testPose2Factor_LDADD = libgtsam.la
testPose2Config_SOURCES = $(example) testPose2Config.cpp
testPose2Config_SOURCES = testPose2Config.cpp
testPose2Config_LDADD = libgtsam.la
testPose2SLAM_SOURCES = $(example) testPose2SLAM.cpp
testPose2SLAM_SOURCES = testPose2SLAM.cpp
testPose2SLAM_LDADD = libgtsam.la
# 2D SLAM using Bearing and Range
headers +=
sources += planarSLAM.cpp
check_PROGRAMS += testPlanarSLAM
testPlanarSLAM_SOURCES = $(example) testPlanarSLAM.cpp
testPlanarSLAM_SOURCES = testPlanarSLAM.cpp
testPlanarSLAM_LDADD = libgtsam.la
# 3D Pose constraints
headers +=
sources += pose3SLAM.cpp
check_PROGRAMS += testPose3Factor testPose3Config testPose3SLAM
testPose3Factor_SOURCES = $(example) testPose3Factor.cpp
testPose3Factor_SOURCES = testPose3Factor.cpp
testPose3Factor_LDADD = libgtsam.la
testPose3Config_SOURCES = $(example) testPose3Config.cpp
testPose3Config_SOURCES = testPose3Config.cpp
testPose3Config_LDADD = libgtsam.la
testPose3SLAM_SOURCES = $(example) testPose3SLAM.cpp
testPose3SLAM_SOURCES = testPose3SLAM.cpp
testPose3SLAM_LDADD = libgtsam.la
# Cameras
@ -244,7 +243,6 @@ testVSLAMGraph_LDADD = libgtsam.la
testVSLAMConfig_SOURCES = testVSLAMConfig.cpp
testVSLAMConfig_LDADD = libgtsam.la
headers += smallExample.h
headers += $(sources:.cpp=.h)
# Timing tests
@ -252,9 +250,9 @@ noinst_PROGRAMS = timeGaussianFactor timeGaussianFactorGraph timeRot3
timeRot3_SOURCES = timeRot3.cpp
timeRot3_LDADD = libgtsam.la
timeGaussianFactor_SOURCES = timeGaussianFactor.cpp
timeGaussianFactor_LDADD = $(example) libgtsam.la
timeGaussianFactor_LDADD = libgtsam.la
timeGaussianFactorGraph_SOURCES = timeGaussianFactorGraph.cpp
timeGaussianFactorGraph_LDADD = $(example) libgtsam.la
timeGaussianFactorGraph_LDADD = libgtsam.la
# create both dynamic and static libraries
AM_CXXFLAGS = -I$(boost) -fPIC

48
cpp/Ordering-inl.h
View File

@ -1,48 +0,0 @@
/*
* Ordering-inl.h
*
* Created on: Jan 14, 2010
* Author: nikai
* Description: the inline file for Ordering
*/
#pragma once
#include "graph-inl.h"
#include "Ordering.h"
using namespace std;
namespace gtsam {
/* ************************************************************************* */
template <class Key>
class ordering_key_visitor : public boost::default_bfs_visitor {
public:
ordering_key_visitor(std::list<Key>& ordering_in) : ordering_(ordering_in) {}
template <typename Vertex, typename Graph> void discover_vertex(Vertex v, const Graph& g) const {
Key key = boost::get(boost::vertex_name, g, v);
ordering_.push_front(key);
}
std::list<Key>& ordering_;
};
/* ************************************************************************* */
template<class Key>
list<Key> predecessorMap2Keys(const PredecessorMap<Key>& p_map) {
typedef typename SGraph<Key>::Vertex SVertex;
SGraph<Key> g;
SVertex root;
std::map<Key, SVertex> key2vertex;
boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph<SGraph<Key>, SVertex, Key>(p_map);
// breadth first visit on the graph
std::list<Key> keys;
ordering_key_visitor<Key> vis(keys);
boost::breadth_first_search(g, root, boost::visitor(vis));
return keys;
}
}

24
cpp/Ordering.cpp
View File

@ -6,27 +6,13 @@
#include <iostream>
#include <stdexcept>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include "Ordering.h"
using namespace std;
using namespace gtsam;
using namespace boost::assign;
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
/* ************************************************************************* */
Ordering Ordering::subtract(const Ordering& keys) const {
Ordering newOrdering = *this;
BOOST_FOREACH(const Symbol& key, keys) {
newOrdering.remove(key);
}
return newOrdering;
}
/* ************************************************************************* */
void Ordering::print(const string& s) const {
@ -41,3 +27,13 @@ bool Ordering::equals(const Ordering &other, double tol) const {
return *this == other;
}
/* ************************************************************************* */
Ordering Ordering::subtract(const Ordering& keys) const {
Ordering newOrdering = *this;
BOOST_FOREACH(const Symbol& key, keys) {
newOrdering.remove(key);
}
return newOrdering;
}
/* ************************************************************************* */

45
cpp/Ordering.h
View File

@ -8,9 +8,7 @@
#include <list>
#include <string>
#include <map>
#include "Testable.h"
#include "graph.h"
#include "Key.h"
namespace gtsam {
@ -21,25 +19,18 @@ namespace gtsam {
*/
class Ordering: public std::list<Symbol>, public Testable<Ordering> {
public:
/**
* Default constructor creates empty ordering
*/
Ordering() {
}
/** Default constructor creates empty ordering */
Ordering() { }
/**
* Create from a single string
*/
Ordering(Symbol key) {
push_back(key);
}
/** Create from a single symbol */
Ordering(Symbol key) { push_back(key); }
/**
* Copy constructor from string vector
*/
Ordering(const std::list<Symbol>& keys_in) :
std::list<Symbol>(keys_in) {
}
/** Copy constructor */
Ordering(const std::list<Symbol>& keys_in) : std::list<Symbol>(keys_in) {}
// Testable
void print(const std::string& s = "Ordering") const;
bool equals(const Ordering &ord, double tol=0) const;
/**
* Remove a set of keys from an ordering
@ -47,22 +38,6 @@ namespace gtsam {
* @return a new ordering without the selected keys
*/
Ordering subtract(const Ordering& keys) const;
void print(const std::string& s = "Ordering") const;
/**
* check if two orderings are the same
* @param ordering
* @return bool
*/
bool equals(const Ordering &ord, double tol=0) const;
};
/**
* Generate a list of keys from a spanning tree represented by its predecessor map
*/
template<class Key>
std::list<Key> predecessorMap2Keys(const PredecessorMap<Key>& p_map);
}

2
cpp/SubgraphPreconditioner-inl.h
View File

@ -11,7 +11,7 @@
#include <boost/tuple/tuple.hpp>
#include "SubgraphPreconditioner.h"
#include "Ordering-inl.h"
#include "graph-inl.h"
#include "iterative-inl.h"
#include "FactorGraph-inl.h"

30
cpp/graph-inl.h
View File

@ -17,6 +17,36 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
template <class Key>
class ordering_key_visitor : public boost::default_bfs_visitor {
public:
ordering_key_visitor(std::list<Key>& ordering_in) : ordering_(ordering_in) {}
template <typename Vertex, typename Graph> void discover_vertex(Vertex v, const Graph& g) const {
Key key = boost::get(boost::vertex_name, g, v);
ordering_.push_front(key);
}
std::list<Key>& ordering_;
};
/* ************************************************************************* */
template<class Key>
list<Key> predecessorMap2Keys(const PredecessorMap<Key>& p_map) {
typedef typename SGraph<Key>::Vertex SVertex;
SGraph<Key> g;
SVertex root;
std::map<Key, SVertex> key2vertex;
boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph<SGraph<Key>, SVertex, Key>(p_map);
// breadth first visit on the graph
std::list<Key> keys;
ordering_key_visitor<Key> vis(keys);
boost::breadth_first_search(g, root, boost::visitor(vis));
return keys;
}
/* ************************************************************************* */
template<class G, class F, class Key>
SDGraph<Key> toBoostGraph(const G& graph) {

6
cpp/graph.h
View File

@ -49,6 +49,12 @@ namespace gtsam {
}
};
/**
* Generate a list of keys from a spanning tree represented by its predecessor map
*/
template<class Key>
std::list<Key> predecessorMap2Keys(const PredecessorMap<Key>& p_map);
/**
* Convert the factor graph to an SDGraph
* G = Graph type

29
cpp/testGraph.cpp
View File

@ -8,7 +8,8 @@
#include <iostream>
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
@ -21,9 +22,33 @@
#include "FactorGraph-inl.h"
using namespace std;
using namespace boost;
using namespace gtsam;
/* ************************************************************************* */
// x1 -> x2
// -> x3 -> x4
// -> x5
TEST ( Ordering, predecessorMap2Keys ) {
typedef TypedSymbol<Pose2,'x'> Key;
PredecessorMap<Key> p_map;
p_map.insert(1,1);
p_map.insert(2,1);
p_map.insert(3,1);
p_map.insert(4,3);
p_map.insert(5,1);
list<Key> expected;
expected += 4,5,3,2,1;//Key(4), Key(5), Key(3), Key(2), Key(1);
list<Key> actual = predecessorMap2Keys<Key>(p_map);
LONGS_EQUAL(expected.size(), actual.size());
list<Key>::const_iterator it1 = expected.begin();
list<Key>::const_iterator it2 = actual.begin();
for(; it1!=expected.end(); it1++, it2++)
CHECK(*it1 == *it2)
}
/* ************************************************************************* */
TEST( Graph, predecessorMap2Graph )
{

1
cpp/testIterative.cpp
View File

@ -18,7 +18,6 @@ using namespace boost::assign;
#include "smallExample.h"
#include "pose2SLAM.h"
#include "SubgraphPreconditioner.h"
#include "Ordering-inl.h"
#include "FactorGraph-inl.h"
#include "NonlinearFactorGraph-inl.h"
#include "iterative-inl.h"

16
cpp/testKey.cpp
View File

@ -3,9 +3,13 @@
* @author Alex Cunningham
*/
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include "Key.h"
using namespace std;
using namespace gtsam;
class Pose3;
@ -50,6 +54,18 @@ TEST ( TypedLabledSymbol, basic_operations ) {
CHECK(key5 < key6);
}
/* ************************************************************************* */
TEST ( Key, keys2symbols )
{
typedef TypedSymbol<int, 'x'> Key;
list<Symbol> expected;
expected += Key(1), Key(2), Key(3);
list<TypedSymbol<int, 'x'> > typeds;
typeds += 1, 2, 3;
CHECK(expected == keys2symbols(typeds));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

41
cpp/testOrdering.cpp
View File

@ -4,46 +4,22 @@
*/
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
// Magically turn strings into Symbols
#define GTSAM_MAGIC_KEY
#include "Ordering-inl.h"
#include "Ordering.h"
#include "pose2SLAM.h"
using namespace std;
using namespace gtsam;
using namespace boost::assign;
/* ************************************************************************* */
// x1 -> x2
// -> x3 -> x4
// -> x5
TEST ( Ordering, predecessorMap2Keys ) {
typedef TypedSymbol<Pose2,'x'> Key;
PredecessorMap<Key> p_map;
p_map.insert(1,1);
p_map.insert(2,1);
p_map.insert(3,1);
p_map.insert(4,3);
p_map.insert(5,1);
list<Key> expected;
expected += 4,5,3,2,1;//Key(4), Key(5), Key(3), Key(2), Key(1);
list<Key> actual = predecessorMap2Keys<Key>(p_map);
LONGS_EQUAL(expected.size(), actual.size());
list<Key>::const_iterator it1 = expected.begin();
list<Key>::const_iterator it2 = actual.begin();
for(; it1!=expected.end(); it1++, it2++)
CHECK(*it1 == *it2)
}
/* ************************************************************************* */
TEST ( Ordering, subtract ) {
TEST ( Ordering, subtract )
{
Ordering init, delta;
init += "a", "b", "c", "d", "e";
CHECK(assert_equal(init.subtract(delta), init));
@ -60,5 +36,8 @@ TEST ( Ordering, subtract ) {
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

2
cpp/testVSLAMGraph.cpp
View File

@ -15,7 +15,7 @@ using namespace boost;
#include "NonlinearFactorGraph-inl.h"
#include "NonlinearOptimizer-inl.h"
#include "Ordering-inl.h"
#include "graph-inl.h"
#include "visualSLAM.h"
using namespace std;