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 #pragma once
#include <list>
#include <iostream> #include <iostream>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <boost/serialization/serialization.hpp> #include <boost/serialization/serialization.hpp>
@ -213,8 +214,17 @@ namespace gtsam {
} }
}; };
// Conversion utilities
template<class Key> Symbol key2symbol(Key key) { template<class Key> Symbol key2symbol(Key key) {
return Symbol(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 } // namespace gtsam

66
cpp/Makefile.am
View File

@ -50,19 +50,18 @@ testMatrix_LDADD = libgtsam.la
# GTSAM basics # GTSAM basics
# The header files will be installed in ~/include/gtsam # The header files will be installed in ~/include/gtsam
headers = gtsam.h Value.h Testable.h Factor.h Conditional.h headers = gtsam.h Value.h Testable.h Factor.h Conditional.h
headers += Ordering.h Ordering-inl.h numericalDerivative.h headers += Ordering.h numericalDerivative.h
sources += Ordering.cpp sources += Ordering.cpp smallExample.cpp
example = smallExample.cpp
# Symbolic Inference # Symbolic Inference
headers += SymbolicConditional.h headers += SymbolicConditional.h
sources += SymbolicFactor.cpp SymbolicFactorGraph.cpp SymbolicBayesNet.cpp sources += SymbolicFactor.cpp SymbolicFactorGraph.cpp SymbolicBayesNet.cpp
check_PROGRAMS += testSymbolicFactor testSymbolicFactorGraph testSymbolicBayesNet check_PROGRAMS += testSymbolicFactor testSymbolicFactorGraph testSymbolicBayesNet
testSymbolicFactor_SOURCES = $(example) testSymbolicFactor.cpp testSymbolicFactor_SOURCES = testSymbolicFactor.cpp
testSymbolicFactor_LDADD = libgtsam.la testSymbolicFactor_LDADD = libgtsam.la
testSymbolicFactorGraph_SOURCES = $(example) testSymbolicFactorGraph.cpp testSymbolicFactorGraph_SOURCES = testSymbolicFactorGraph.cpp
testSymbolicFactorGraph_LDADD = libgtsam.la testSymbolicFactorGraph_LDADD = libgtsam.la
testSymbolicBayesNet_SOURCES = $(example) testSymbolicBayesNet.cpp testSymbolicBayesNet_SOURCES = testSymbolicBayesNet.cpp
testSymbolicBayesNet_LDADD = libgtsam.la testSymbolicBayesNet_LDADD = libgtsam.la
# Inference # Inference
@ -79,18 +78,18 @@ check_PROGRAMS += testBayesTree testISAM testGaussianISAM testGaussianISAM2
testGraph_SOURCES = testGraph.cpp testGraph_SOURCES = testGraph.cpp
testGraph_LDADD = libgtsam.la testGraph_LDADD = libgtsam.la
testFactorgraph_SOURCES = testFactorgraph.cpp testFactorgraph_SOURCES = testFactorgraph.cpp
testInference_SOURCES = $(example) testInference.cpp testInference_SOURCES = testInference.cpp
testFactorgraph_LDADD = libgtsam.la testFactorgraph_LDADD = libgtsam.la
testInference_LDADD = libgtsam.la testInference_LDADD = libgtsam.la
testOrdering_SOURCES = testOrdering.cpp testOrdering_SOURCES = testOrdering.cpp
testOrdering_LDADD = libgtsam.la testOrdering_LDADD = libgtsam.la
testBayesTree_SOURCES = $(example) testBayesTree.cpp testBayesTree_SOURCES = testBayesTree.cpp
testBayesTree_LDADD = libgtsam.la testBayesTree_LDADD = libgtsam.la
testGaussianISAM_SOURCES = $(example) testGaussianISAM.cpp testGaussianISAM_SOURCES = testGaussianISAM.cpp
testGaussianISAM_LDADD = libgtsam.la testGaussianISAM_LDADD = libgtsam.la
testGaussianISAM2_SOURCES = $(example) testGaussianISAM2.cpp testGaussianISAM2_SOURCES = testGaussianISAM2.cpp
testGaussianISAM2_LDADD = libgtsam.la testGaussianISAM2_LDADD = libgtsam.la
testISAM_SOURCES = $(example) testISAM.cpp testISAM_SOURCES = testISAM.cpp
testISAM_LDADD = libgtsam.la testISAM_LDADD = libgtsam.la
# Binary Inference # Binary Inference
@ -105,13 +104,13 @@ sources += NoiseModel.cpp Errors.cpp VectorConfig.cpp GaussianFactor.cpp Gaussia
check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testNoiseModel check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testNoiseModel
testVectorConfig_SOURCES = testVectorConfig.cpp testVectorConfig_SOURCES = testVectorConfig.cpp
testVectorConfig_LDADD = libgtsam.la testVectorConfig_LDADD = libgtsam.la
testGaussianFactor_SOURCES = $(example) testGaussianFactor.cpp testGaussianFactor_SOURCES = testGaussianFactor.cpp
testGaussianFactor_LDADD = libgtsam.la testGaussianFactor_LDADD = libgtsam.la
testGaussianFactorGraph_SOURCES = $(example) testGaussianFactorGraph.cpp testGaussianFactorGraph_SOURCES = testGaussianFactorGraph.cpp
testGaussianFactorGraph_LDADD = libgtsam.la testGaussianFactorGraph_LDADD = libgtsam.la
testGaussianConditional_SOURCES = $(example) testGaussianConditional.cpp testGaussianConditional_SOURCES = testGaussianConditional.cpp
testGaussianConditional_LDADD = libgtsam.la testGaussianConditional_LDADD = libgtsam.la
testGaussianBayesNet_SOURCES = $(example) testGaussianBayesNet.cpp testGaussianBayesNet_SOURCES = testGaussianBayesNet.cpp
testGaussianBayesNet_LDADD = libgtsam.la testGaussianBayesNet_LDADD = libgtsam.la
testNoiseModel_SOURCES = testNoiseModel.cpp testNoiseModel_SOURCES = testNoiseModel.cpp
testNoiseModel_LDADD = libgtsam.la testNoiseModel_LDADD = libgtsam.la
@ -120,11 +119,11 @@ testNoiseModel_LDADD = libgtsam.la
headers += iterative-inl.h SubgraphPreconditioner-inl.h headers += iterative-inl.h SubgraphPreconditioner-inl.h
sources += iterative.cpp BayesNetPreconditioner.cpp SubgraphPreconditioner.cpp sources += iterative.cpp BayesNetPreconditioner.cpp SubgraphPreconditioner.cpp
check_PROGRAMS += testIterative testBayesNetPreconditioner testSubgraphPreconditioner check_PROGRAMS += testIterative testBayesNetPreconditioner testSubgraphPreconditioner
testIterative_SOURCES = $(example) testIterative.cpp testIterative_SOURCES = testIterative.cpp
testIterative_LDADD = libgtsam.la testIterative_LDADD = libgtsam.la
testBayesNetPreconditioner_SOURCES = $(example) testBayesNetPreconditioner.cpp testBayesNetPreconditioner_SOURCES = testBayesNetPreconditioner.cpp
testBayesNetPreconditioner_LDADD = libgtsam.la testBayesNetPreconditioner_LDADD = libgtsam.la
testSubgraphPreconditioner_SOURCES = $(example) testSubgraphPreconditioner.cpp testSubgraphPreconditioner_SOURCES = testSubgraphPreconditioner.cpp
testSubgraphPreconditioner_LDADD = libgtsam.la testSubgraphPreconditioner_LDADD = libgtsam.la
# Nonlinear inference # Nonlinear inference
@ -132,11 +131,11 @@ headers += Key.h NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h
headers += NonlinearFactor.h headers += NonlinearFactor.h
check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testKey check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testKey
testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp testNonlinearFactor_SOURCES = testNonlinearFactor.cpp
testNonlinearFactor_LDADD = libgtsam.la testNonlinearFactor_LDADD = libgtsam.la
testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp testNonlinearFactorGraph_SOURCES = testNonlinearFactorGraph.cpp
testNonlinearFactorGraph_LDADD = libgtsam.la testNonlinearFactorGraph_LDADD = libgtsam.la
testNonlinearOptimizer_SOURCES = $(example) testNonlinearOptimizer.cpp testNonlinearOptimizer_SOURCES = testNonlinearOptimizer.cpp
testNonlinearOptimizer_LDADD = libgtsam.la testNonlinearOptimizer_LDADD = libgtsam.la
testKey_SOURCES = testKey.cpp testKey_SOURCES = testKey.cpp
testKey_LDADD = libgtsam.la testKey_LDADD = libgtsam.la
@ -150,9 +149,9 @@ testNonlinearConstraint_SOURCES = testNonlinearConstraint.cpp
testNonlinearConstraint_LDADD = libgtsam.la testNonlinearConstraint_LDADD = libgtsam.la
testNonlinearEquality_SOURCES = testNonlinearEquality.cpp testNonlinearEquality_SOURCES = testNonlinearEquality.cpp
testNonlinearEquality_LDADD = libgtsam.la testNonlinearEquality_LDADD = libgtsam.la
testSQP_SOURCES = $(example) testSQP.cpp testSQP_SOURCES = testSQP.cpp
testSQP_LDADD = libgtsam.la testSQP_LDADD = libgtsam.la
testSQPOptimizer_SOURCES = $(example) testSQPOptimizer.cpp testSQPOptimizer_SOURCES = testSQPOptimizer.cpp
testSQPOptimizer_LDADD = libgtsam.la testSQPOptimizer_LDADD = libgtsam.la
# geometry # geometry
@ -199,31 +198,31 @@ headers += LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h
headers += headers +=
sources += pose2SLAM.cpp sources += pose2SLAM.cpp
check_PROGRAMS += testPose2Factor testPose2Config testPose2SLAM testPose2Prior check_PROGRAMS += testPose2Factor testPose2Config testPose2SLAM testPose2Prior
testPose2Prior_SOURCES = $(example) testPose2Prior.cpp testPose2Prior_SOURCES = testPose2Prior.cpp
testPose2Prior_LDADD = libgtsam.la testPose2Prior_LDADD = libgtsam.la
testPose2Factor_SOURCES = $(example) testPose2Factor.cpp testPose2Factor_SOURCES = testPose2Factor.cpp
testPose2Factor_LDADD = libgtsam.la testPose2Factor_LDADD = libgtsam.la
testPose2Config_SOURCES = $(example) testPose2Config.cpp testPose2Config_SOURCES = testPose2Config.cpp
testPose2Config_LDADD = libgtsam.la testPose2Config_LDADD = libgtsam.la
testPose2SLAM_SOURCES = $(example) testPose2SLAM.cpp testPose2SLAM_SOURCES = testPose2SLAM.cpp
testPose2SLAM_LDADD = libgtsam.la testPose2SLAM_LDADD = libgtsam.la
# 2D SLAM using Bearing and Range # 2D SLAM using Bearing and Range
headers += headers +=
sources += planarSLAM.cpp sources += planarSLAM.cpp
check_PROGRAMS += testPlanarSLAM check_PROGRAMS += testPlanarSLAM
testPlanarSLAM_SOURCES = $(example) testPlanarSLAM.cpp testPlanarSLAM_SOURCES = testPlanarSLAM.cpp
testPlanarSLAM_LDADD = libgtsam.la testPlanarSLAM_LDADD = libgtsam.la
# 3D Pose constraints # 3D Pose constraints
headers += headers +=
sources += pose3SLAM.cpp sources += pose3SLAM.cpp
check_PROGRAMS += testPose3Factor testPose3Config testPose3SLAM check_PROGRAMS += testPose3Factor testPose3Config testPose3SLAM
testPose3Factor_SOURCES = $(example) testPose3Factor.cpp testPose3Factor_SOURCES = testPose3Factor.cpp
testPose3Factor_LDADD = libgtsam.la testPose3Factor_LDADD = libgtsam.la
testPose3Config_SOURCES = $(example) testPose3Config.cpp testPose3Config_SOURCES = testPose3Config.cpp
testPose3Config_LDADD = libgtsam.la testPose3Config_LDADD = libgtsam.la
testPose3SLAM_SOURCES = $(example) testPose3SLAM.cpp testPose3SLAM_SOURCES = testPose3SLAM.cpp
testPose3SLAM_LDADD = libgtsam.la testPose3SLAM_LDADD = libgtsam.la
# Cameras # Cameras
@ -244,7 +243,6 @@ testVSLAMGraph_LDADD = libgtsam.la
testVSLAMConfig_SOURCES = testVSLAMConfig.cpp testVSLAMConfig_SOURCES = testVSLAMConfig.cpp
testVSLAMConfig_LDADD = libgtsam.la testVSLAMConfig_LDADD = libgtsam.la
headers += smallExample.h
headers += $(sources:.cpp=.h) headers += $(sources:.cpp=.h)
# Timing tests # Timing tests
@ -252,9 +250,9 @@ noinst_PROGRAMS = timeGaussianFactor timeGaussianFactorGraph timeRot3
timeRot3_SOURCES = timeRot3.cpp timeRot3_SOURCES = timeRot3.cpp
timeRot3_LDADD = libgtsam.la timeRot3_LDADD = libgtsam.la
timeGaussianFactor_SOURCES = timeGaussianFactor.cpp timeGaussianFactor_SOURCES = timeGaussianFactor.cpp
timeGaussianFactor_LDADD = $(example) libgtsam.la timeGaussianFactor_LDADD = libgtsam.la
timeGaussianFactorGraph_SOURCES = timeGaussianFactorGraph.cpp timeGaussianFactorGraph_SOURCES = timeGaussianFactorGraph.cpp
timeGaussianFactorGraph_LDADD = $(example) libgtsam.la timeGaussianFactorGraph_LDADD = libgtsam.la
# create both dynamic and static libraries # create both dynamic and static libraries
AM_CXXFLAGS = -I$(boost) -fPIC 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 <iostream>
#include <stdexcept> #include <stdexcept>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include "Ordering.h" #include "Ordering.h"
using namespace std; using namespace std;
using namespace gtsam; 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 { void Ordering::print(const string& s) const {
@ -41,3 +27,13 @@ bool Ordering::equals(const Ordering &other, double tol) const {
return *this == other; 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 <list>
#include <string> #include <string>
#include <map>
#include "Testable.h" #include "Testable.h"
#include "graph.h"
#include "Key.h" #include "Key.h"
namespace gtsam { namespace gtsam {
@ -21,25 +19,18 @@ namespace gtsam {
*/ */
class Ordering: public std::list<Symbol>, public Testable<Ordering> { class Ordering: public std::list<Symbol>, public Testable<Ordering> {
public: public:
/** /** Default constructor creates empty ordering */
* Default constructor creates empty ordering Ordering() { }
*/
Ordering() {
}
/** /** Create from a single symbol */
* Create from a single string Ordering(Symbol key) { push_back(key); }
*/
Ordering(Symbol key) {
push_back(key);
}
/** /** Copy constructor */
* Copy constructor from string vector Ordering(const std::list<Symbol>& keys_in) : std::list<Symbol>(keys_in) {}
*/
Ordering(const std::list<Symbol>& keys_in) : // Testable
std::list<Symbol>(keys_in) { 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 * Remove a set of keys from an ordering
@ -47,22 +38,6 @@ namespace gtsam {
* @return a new ordering without the selected keys * @return a new ordering without the selected keys
*/ */
Ordering subtract(const Ordering& keys) const; 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 <boost/tuple/tuple.hpp>
#include "SubgraphPreconditioner.h" #include "SubgraphPreconditioner.h"
#include "Ordering-inl.h" #include "graph-inl.h"
#include "iterative-inl.h" #include "iterative-inl.h"
#include "FactorGraph-inl.h" #include "FactorGraph-inl.h"

30
cpp/graph-inl.h
View File

@ -17,6 +17,36 @@ using namespace std;
namespace gtsam { 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> template<class G, class F, class Key>
SDGraph<Key> toBoostGraph(const G& graph) { 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 * Convert the factor graph to an SDGraph
* G = Graph type * G = Graph type

29
cpp/testGraph.cpp
View File

@ -8,7 +8,8 @@
#include <iostream> #include <iostream>
#include <boost/shared_ptr.hpp> #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> #include <CppUnitLite/TestHarness.h>
@ -21,9 +22,33 @@
#include "FactorGraph-inl.h" #include "FactorGraph-inl.h"
using namespace std; using namespace std;
using namespace boost;
using namespace gtsam; 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 ) TEST( Graph, predecessorMap2Graph )
{ {

1
cpp/testIterative.cpp
View File

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

16
cpp/testKey.cpp
View File

@ -3,9 +3,13 @@
* @author Alex Cunningham * @author Alex Cunningham
*/ */
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include "Key.h" #include "Key.h"
using namespace std;
using namespace gtsam; using namespace gtsam;
class Pose3; class Pose3;
@ -50,6 +54,18 @@ TEST ( TypedLabledSymbol, basic_operations ) {
CHECK(key5 < key6); 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); } 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 += #include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
// Magically turn strings into Symbols
#define GTSAM_MAGIC_KEY #define GTSAM_MAGIC_KEY
#include "Ordering-inl.h" #include "Ordering.h"
#include "pose2SLAM.h" #include "pose2SLAM.h"
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace boost::assign;
/* ************************************************************************* */ /* ************************************************************************* */
// x1 -> x2 TEST ( Ordering, subtract )
// -> 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 ) {
Ordering init, delta; Ordering init, delta;
init += "a", "b", "c", "d", "e"; init += "a", "b", "c", "d", "e";
CHECK(assert_equal(init.subtract(delta), init)); 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 "NonlinearFactorGraph-inl.h"
#include "NonlinearOptimizer-inl.h" #include "NonlinearOptimizer-inl.h"
#include "Ordering-inl.h" #include "graph-inl.h"
#include "visualSLAM.h" #include "visualSLAM.h"
using namespace std; using namespace std;