diff --git a/cpp/Key.h b/cpp/Key.h index 168274fbd..cc60d4812 100644 --- a/cpp/Key.h +++ b/cpp/Key.h @@ -8,6 +8,7 @@ #pragma once +#include #include #include #include @@ -213,8 +214,17 @@ namespace gtsam { } }; + // Conversion utilities + template Symbol key2symbol(Key key) { return Symbol(key); } + + template std::list keys2symbols(std::list keys) { + std::list symbols; + std::transform(keys.begin(), keys.end(), std::back_inserter(symbols), key2symbol ); + return symbols; + } + } // namespace gtsam diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 9859a193c..4ed82e481 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -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 diff --git a/cpp/Ordering-inl.h b/cpp/Ordering-inl.h deleted file mode 100644 index e8c89a19d..000000000 --- a/cpp/Ordering-inl.h +++ /dev/null @@ -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 ordering_key_visitor : public boost::default_bfs_visitor { -public: - ordering_key_visitor(std::list& ordering_in) : ordering_(ordering_in) {} - template void discover_vertex(Vertex v, const Graph& g) const { - Key key = boost::get(boost::vertex_name, g, v); - ordering_.push_front(key); - } - std::list& ordering_; -}; - -/* ************************************************************************* */ -template -list predecessorMap2Keys(const PredecessorMap& p_map) { - - typedef typename SGraph::Vertex SVertex; - - SGraph g; - SVertex root; - std::map key2vertex; - boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph, SVertex, Key>(p_map); - - // breadth first visit on the graph - std::list keys; - ordering_key_visitor vis(keys); - boost::breadth_first_search(g, root, boost::visitor(vis)); - return keys; -} - -} diff --git a/cpp/Ordering.cpp b/cpp/Ordering.cpp index 02ac56cd8..9ba79e609 100644 --- a/cpp/Ordering.cpp +++ b/cpp/Ordering.cpp @@ -6,27 +6,13 @@ #include #include -#include // for operator += #include #include #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; +} + +/* ************************************************************************* */ diff --git a/cpp/Ordering.h b/cpp/Ordering.h index 2eafba1dc..0d7e54240 100644 --- a/cpp/Ordering.h +++ b/cpp/Ordering.h @@ -8,9 +8,7 @@ #include #include -#include #include "Testable.h" -#include "graph.h" #include "Key.h" namespace gtsam { @@ -21,25 +19,18 @@ namespace gtsam { */ class Ordering: public std::list, public Testable { 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& keys_in) : - std::list(keys_in) { - } + /** Copy constructor */ + Ordering(const std::list& keys_in) : std::list(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 - std::list predecessorMap2Keys(const PredecessorMap& p_map); - } diff --git a/cpp/SubgraphPreconditioner-inl.h b/cpp/SubgraphPreconditioner-inl.h index cac25f24c..9563330a6 100644 --- a/cpp/SubgraphPreconditioner-inl.h +++ b/cpp/SubgraphPreconditioner-inl.h @@ -11,7 +11,7 @@ #include #include "SubgraphPreconditioner.h" -#include "Ordering-inl.h" +#include "graph-inl.h" #include "iterative-inl.h" #include "FactorGraph-inl.h" diff --git a/cpp/graph-inl.h b/cpp/graph-inl.h index 8a927c656..9a57c6ebb 100644 --- a/cpp/graph-inl.h +++ b/cpp/graph-inl.h @@ -17,6 +17,36 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +template +class ordering_key_visitor : public boost::default_bfs_visitor { +public: + ordering_key_visitor(std::list& ordering_in) : ordering_(ordering_in) {} + template void discover_vertex(Vertex v, const Graph& g) const { + Key key = boost::get(boost::vertex_name, g, v); + ordering_.push_front(key); + } + std::list& ordering_; +}; + +/* ************************************************************************* */ +template +list predecessorMap2Keys(const PredecessorMap& p_map) { + + typedef typename SGraph::Vertex SVertex; + + SGraph g; + SVertex root; + std::map key2vertex; + boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph, SVertex, Key>(p_map); + + // breadth first visit on the graph + std::list keys; + ordering_key_visitor vis(keys); + boost::breadth_first_search(g, root, boost::visitor(vis)); + return keys; +} + /* ************************************************************************* */ template SDGraph toBoostGraph(const G& graph) { diff --git a/cpp/graph.h b/cpp/graph.h index 45e30a6a3..f899b2789 100644 --- a/cpp/graph.h +++ b/cpp/graph.h @@ -49,6 +49,12 @@ namespace gtsam { } }; + /** + * Generate a list of keys from a spanning tree represented by its predecessor map + */ + template + std::list predecessorMap2Keys(const PredecessorMap& p_map); + /** * Convert the factor graph to an SDGraph * G = Graph type diff --git a/cpp/testGraph.cpp b/cpp/testGraph.cpp index bc1df1260..68100bee4 100644 --- a/cpp/testGraph.cpp +++ b/cpp/testGraph.cpp @@ -8,7 +8,8 @@ #include #include -#include +#include // for operator += +using namespace boost::assign; #include @@ -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 Key; + PredecessorMap 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 expected; + expected += 4,5,3,2,1;//Key(4), Key(5), Key(3), Key(2), Key(1); + + list actual = predecessorMap2Keys(p_map); + LONGS_EQUAL(expected.size(), actual.size()); + + list::const_iterator it1 = expected.begin(); + list::const_iterator it2 = actual.begin(); + for(; it1!=expected.end(); it1++, it2++) + CHECK(*it1 == *it2) +} + /* ************************************************************************* */ TEST( Graph, predecessorMap2Graph ) { diff --git a/cpp/testIterative.cpp b/cpp/testIterative.cpp index 56abd4ed3..37569b47f 100644 --- a/cpp/testIterative.cpp +++ b/cpp/testIterative.cpp @@ -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" diff --git a/cpp/testKey.cpp b/cpp/testKey.cpp index 168b345cf..9669eabd8 100644 --- a/cpp/testKey.cpp +++ b/cpp/testKey.cpp @@ -3,9 +3,13 @@ * @author Alex Cunningham */ +#include // for operator += +using namespace boost::assign; + #include #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 Key; + list expected; + expected += Key(1), Key(2), Key(3); + + list > typeds; + typeds += 1, 2, 3; + CHECK(expected == keys2symbols(typeds)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/cpp/testOrdering.cpp b/cpp/testOrdering.cpp index 9998e206b..acada7984 100644 --- a/cpp/testOrdering.cpp +++ b/cpp/testOrdering.cpp @@ -4,46 +4,22 @@ */ #include // for operator += +using namespace boost::assign; + #include +// 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 Key; - PredecessorMap 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 expected; - expected += 4,5,3,2,1;//Key(4), Key(5), Key(3), Key(2), Key(1); - - list actual = predecessorMap2Keys(p_map); - LONGS_EQUAL(expected.size(), actual.size()); - - list::const_iterator it1 = expected.begin(); - list::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)); @@ -58,7 +34,10 @@ TEST ( Ordering, subtract ) { expected2 += "a", "c", "d"; CHECK(assert_equal(init.subtract(delta), expected2)); } - + /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/cpp/testVSLAMGraph.cpp b/cpp/testVSLAMGraph.cpp index 795a76790..818adfcbb 100644 --- a/cpp/testVSLAMGraph.cpp +++ b/cpp/testVSLAMGraph.cpp @@ -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;