diff --git a/.cproject b/.cproject index 050d377b8..19fa7e086 100644 --- a/.cproject +++ b/.cproject @@ -468,7 +468,6 @@ make - testBayesTree.run true false @@ -476,6 +475,7 @@ make + testSymbolicBayesNet.run true false @@ -483,7 +483,6 @@ make - testSymbolicFactorGraph.run true false @@ -585,14 +584,6 @@ true true - -make --j2 -testPose2Constraint.run -true -true -true - make -j2 @@ -689,10 +680,10 @@ true true - + make -j2 -testPose2Graph.run +testPose2SLAM.run true true true @@ -723,7 +714,6 @@ make - testGraph.run true false diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 886538b81..2ea2b9d37 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -188,20 +188,21 @@ testSimulated3D_LDADD = libgtsam.la check_PROGRAMS += testSimulated3D # Pose SLAM headers -headers += BetweenFactor.h LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h +headers += BetweenFactor.h PriorFactor.h +headers += LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h -# 2D Pose constraints -headers += Pose2Prior.h -sources += Pose2Graph.cpp -check_PROGRAMS += testPose2Factor testPose2Config testPose2Graph testPose2Prior +# 2D Pose SLAM +headers += +sources += pose2SLAM.cpp +check_PROGRAMS += testPose2Factor testPose2Config testPose2SLAM testPose2Prior +testPose2Prior_SOURCES = $(example) testPose2Prior.cpp +testPose2Prior_LDADD = libgtsam.la testPose2Factor_SOURCES = $(example) testPose2Factor.cpp testPose2Factor_LDADD = libgtsam.la testPose2Config_SOURCES = $(example) testPose2Config.cpp testPose2Config_LDADD = libgtsam.la -testPose2Graph_SOURCES = $(example) testPose2Graph.cpp -testPose2Graph_LDADD = libgtsam.la -testPose2Prior_SOURCES = $(example) testPose2Prior.cpp -testPose2Prior_LDADD = libgtsam.la +testPose2SLAM_SOURCES = $(example) testPose2SLAM.cpp +testPose2SLAM_LDADD = libgtsam.la # 2D SLAM using Bearing and Range headers += diff --git a/cpp/Pose2Config.cpp b/cpp/Pose2Config.cpp deleted file mode 100644 index be1ba1133..000000000 --- a/cpp/Pose2Config.cpp +++ /dev/null @@ -1,30 +0,0 @@ -/** - * @file Pose2Config.cpp - * @brief Configuration of 2D poses - * @author Frank Dellaert - */ - -#include "Pose2Config.h" -#include "LieConfig-inl.h" - -using namespace std; - -namespace gtsam { - - /** Explicit instantiation of templated methods and functions */ - template class LieConfig,Pose2>; - template size_t dim(const Pose2Config& c); - template Pose2Config expmap(const Pose2Config& c, const Vector& delta); - template Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta); - - /* ************************************************************************* */ - Pose2Config pose2Circle(size_t n, double R) { - Pose2Config x; - double theta = 0, dtheta = 2*M_PI/n; - for(size_t i=0;i,Pose2> Pose2Config; - - /** - * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) - * @param n number of poses - * @param R radius of circle - * @param c character to use for keys - * @return circle of n 2D poses - */ - Pose2Config pose2Circle(size_t n, double R); - -} // namespace diff --git a/cpp/Pose2Factor.h b/cpp/Pose2Factor.h deleted file mode 100644 index 71aa707d8..000000000 --- a/cpp/Pose2Factor.h +++ /dev/null @@ -1,17 +0,0 @@ -/** - * @file Pose2Factor.H - * @authors Frank Dellaert, Viorela Ila - **/ - -#pragma once - -#include "BetweenFactor.h" -#include "Pose2.h" -#include "Pose2Config.h" - -namespace gtsam { - - /** This is just a typedef now */ - typedef BetweenFactor Pose2Factor; - -} /// namespace gtsam diff --git a/cpp/Pose2Graph.cpp b/cpp/Pose2Graph.cpp deleted file mode 100644 index 1ea49856c..000000000 --- a/cpp/Pose2Graph.cpp +++ /dev/null @@ -1,52 +0,0 @@ -/** - * @file Pose2Graph.cpp - * @brief A factor graph for the 2D PoseSLAM problem - * @authors Frank Dellaert, Viorela Ila - */ - -#include "Pose2Graph.h" -#include "FactorGraph-inl.h" -#include "NonlinearFactorGraph-inl.h" -#include "NonlinearOptimizer-inl.h" -#include "NonlinearEquality.h" -#include "graph-inl.h" -#include "LieConfig-inl.h" - -using namespace std; -using namespace gtsam; - -namespace gtsam { - - /** Explicit instantiation of templated methods and functions */ - template class LieConfig,Pose2>; - template size_t dim(const Pose2Config& c); - template Pose2Config expmap(const Pose2Config& c, const Vector& delta); - template Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta); - - // explicit instantiation so all the code is there and we can link with it - template class FactorGraph > ; - template class NonlinearFactorGraph ; - template class NonlinearEquality ; - template class NonlinearOptimizer; - - /* ************************************************************************* */ - Pose2Config pose2Circle(size_t n, double R) { - Pose2Config x; - double theta = 0, dtheta = 2*M_PI/n; - for(size_t i=0;i (key, pose))); - } - - /* ************************************************************************* */ - bool Pose2Graph::equals(const Pose2Graph& p, double tol) const { - return false; - } - -} // namespace gtsam diff --git a/cpp/Pose2Graph.h b/cpp/Pose2Graph.h deleted file mode 100644 index 986e07666..000000000 --- a/cpp/Pose2Graph.h +++ /dev/null @@ -1,74 +0,0 @@ -/** - * @file Pose2Graph.h - * @brief A factor graph for the 2D PoseSLAM problem - * @author Frank Dellaert - * @author Viorela Ila - */ - -#pragma once - -#include "NonlinearFactorGraph.h" -#include "Pose2.h" -#include "LieConfig.h" -#include "BetweenFactor.h" -#include "Key.h" - -namespace gtsam { - - /** - * Pose2Config is now simply a typedef - */ - typedef LieConfig,Pose2> Pose2Config; - - typedef BetweenFactor Pose2Factor; - - /** - * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) - * @param n number of poses - * @param R radius of circle - * @param c character to use for keys - * @return circle of n 2D poses - */ - Pose2Config pose2Circle(size_t n, double R); - - - /** - * Non-linear factor graph for visual SLAM - */ - class Pose2Graph: public gtsam::NonlinearFactorGraph { - - public: - - /** default constructor is empty graph */ - Pose2Graph() { - } - - /** - * equals - */ - bool equals(const Pose2Graph& p, double tol = 1e-9) const; - - /** - * Add a factor without having to do shared factor dance - */ - inline void add(const Pose2Config::Key& key1, const Pose2Config::Key& key2, - const Pose2& measured, const Matrix& covariance) { - push_back(sharedFactor(new Pose2Factor(key1, key2, measured, covariance))); - } - - /** - * Add an equality constraint on a pose - * @param key of pose - * @param pose which pose to constrain it to - */ - void addConstraint(const Pose2Config::Key& key, const Pose2& pose = Pose2()); - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - } - }; - -} // namespace gtsam diff --git a/cpp/Pose2Prior.h b/cpp/PriorFactor.h similarity index 85% rename from cpp/Pose2Prior.h rename to cpp/PriorFactor.h index 293e4f6cf..3eb381dc3 100644 --- a/cpp/Pose2Prior.h +++ b/cpp/PriorFactor.h @@ -1,17 +1,12 @@ /** - * @file Pose2Prior.h - * @authors Michael Kaess + * @file PriorFactor.h + * @authors Frank Dellaert **/ #pragma once -#include +#include #include "NonlinearFactor.h" -#include "GaussianFactor.h" #include "Pose2.h" -#include "Matrix.h" -#include "Key.h" -#include "Pose2Graph.h" -#include "ostream" namespace gtsam { @@ -68,7 +63,4 @@ namespace gtsam { } }; - /** This is just a typedef now */ - typedef PriorFactor Pose2Prior; - } /// namespace gtsam diff --git a/cpp/pose2SLAM.cpp b/cpp/pose2SLAM.cpp new file mode 100644 index 000000000..1944dc715 --- /dev/null +++ b/cpp/pose2SLAM.cpp @@ -0,0 +1,50 @@ +/** + * @file pose2SLAM.cpp + * @brief: bearing/range measurements in 2D plane + * @authors Frank Dellaert + **/ + +#include "pose2SLAM.h" +#include "LieConfig-inl.h" +#include "NonlinearFactorGraph-inl.h" +#include "NonlinearOptimizer-inl.h" + +// Use pose2SLAM namespace for specific SLAM instance +namespace gtsam { + + using namespace pose2SLAM; + INSTANTIATE_LIE_CONFIG(Key, Pose2) + INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) + INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) + + namespace pose2SLAM { + + /* ************************************************************************* */ + Config circle(size_t n, double R) { + Config x; + double theta = 0, dtheta = 2 * M_PI / n; + for (size_t i = 0; i < n; i++, theta += dtheta) + x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta)); + return x; + } + + /* ************************************************************************* */ + void Graph::addConstraint(const Key& i, const Pose2& p) { + sharedFactor factor(new Constraint(i, p)); + push_back(factor); + } + + void Graph::addPrior(const Key& i, const Pose2& p, const Matrix& cov) { + sharedFactor factor(new Prior(i, p, cov)); + push_back(factor); + } + + void Graph::add(const Key& i, const Key& j, const Pose2& z, const Matrix& cov) { + sharedFactor factor(new Odometry(i, j, z, cov)); + push_back(factor); + } + /* ************************************************************************* */ + + } // pose2SLAM + +} // gtsam diff --git a/cpp/pose2SLAM.h b/cpp/pose2SLAM.h new file mode 100644 index 000000000..c1cbdf310 --- /dev/null +++ b/cpp/pose2SLAM.h @@ -0,0 +1,63 @@ +/** + * @file pose2SLAM.h + * @brief: bearing/range measurements in 2D plane + * @authors Frank Dellaert + **/ + +#pragma once + +#include "Key.h" +#include "Pose2.h" +#include "LieConfig.h" +#include "PriorFactor.h" +#include "BetweenFactor.h" +#include "NonlinearEquality.h" +#include "NonlinearFactorGraph.h" +#include "NonlinearOptimizer.h" + +namespace gtsam { + + // Use pose2SLAM namespace for specific SLAM instance + namespace pose2SLAM { + + // Keys and Config + typedef Symbol Key; + typedef LieConfig Config; + + /** + * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) + * @param n number of poses + * @param R radius of circle + * @param c character to use for keys + * @return circle of n 2D poses + */ + Config circle(size_t n, double R); + + // Factors + typedef PriorFactor Prior; + typedef BetweenFactor Odometry; + typedef NonlinearEquality Constraint; + + // Graph + struct Graph: public NonlinearFactorGraph { + void addConstraint(const Key& i, const Pose2& p); + void addPrior(const Key& i, const Pose2& p, const Matrix& cov); + void add(const Key& i, const Key& j, const Pose2& z, const Matrix& cov); + }; + + // Optimizer + typedef NonlinearOptimizer Optimizer; + + } // pose2SLAM + + /** + * Backwards compatibility + */ + typedef pose2SLAM::Prior Pose2Prior; + typedef pose2SLAM::Odometry Pose2Factor; + typedef pose2SLAM::Constraint Pose2Constraint; + typedef pose2SLAM::Config Pose2Config; + typedef pose2SLAM::Graph Pose2Graph; + +} // namespace gtsam + diff --git a/cpp/testGraph.cpp b/cpp/testGraph.cpp index 1f7c5e6a0..b481c9a34 100644 --- a/cpp/testGraph.cpp +++ b/cpp/testGraph.cpp @@ -12,7 +12,7 @@ #include -#include "Pose2Graph.h" +#include "pose2SLAM.h" #include "LieConfig-inl.h" #include "graph-inl.h" @@ -43,8 +43,8 @@ TEST( Graph, composePoses ) { Pose2Graph graph; Matrix cov = eye(3); - graph.push_back(boost::shared_ptr(new Pose2Factor(1,2, Pose2(2.0, 0.0, 0.0), cov))); - graph.push_back(boost::shared_ptr(new Pose2Factor(2,3, Pose2(3.0, 0.0, 0.0), cov))); + graph.add(1,2, Pose2(2.0, 0.0, 0.0), cov); + graph.add(2,3, Pose2(3.0, 0.0, 0.0), cov); PredecessorMap tree; tree.insert(1,2); diff --git a/cpp/testIterative.cpp b/cpp/testIterative.cpp index 0a1b4ad50..d8dfe6805 100644 --- a/cpp/testIterative.cpp +++ b/cpp/testIterative.cpp @@ -12,8 +12,7 @@ using namespace boost::assign; #include "Ordering.h" #include "iterative.h" #include "smallExample.h" -#include "Pose2Graph.h" -#include "Pose2Prior.h" +#include "pose2SLAM.h" using namespace std; using namespace gtsam; @@ -101,8 +100,6 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint ) /* ************************************************************************* */ TEST( Iterative, conjugateGradientDescent_soft_constraint ) { - typedef Pose2Config::Key Key; - Pose2Config config; config.insert(1, Pose2(0.,0.,0.)); config.insert(2, Pose2(1.5,0.,0.)); @@ -110,8 +107,8 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) Pose2Graph graph; Matrix cov = eye(3); Matrix cov2 = eye(3) * 1e-10; - graph.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(1.,0.,0.), cov))); - graph.push_back(boost::shared_ptr(new Pose2Prior(Key(1), Pose2(0.,0.,0.), cov2))); + graph.add(1,2, Pose2(1.,0.,0.), cov); + graph.addPrior(1, Pose2(0.,0.,0.), cov2); VectorConfig zeros; zeros.insert("x1",zero(3)); diff --git a/cpp/testOrdering.cpp b/cpp/testOrdering.cpp index eb1aa735a..57b6158df 100644 --- a/cpp/testOrdering.cpp +++ b/cpp/testOrdering.cpp @@ -6,7 +6,7 @@ #include // for operator += #include #include "Ordering-inl.h" -#include "Pose2Graph.h" +#include "pose2SLAM.h" using namespace std; diff --git a/cpp/testPose2Config.cpp b/cpp/testPose2Config.cpp index 0a4694e24..0be4145e6 100644 --- a/cpp/testPose2Config.cpp +++ b/cpp/testPose2Config.cpp @@ -6,7 +6,7 @@ #include #include -#include "Pose2Graph.h" +#include "pose2SLAM.h" using namespace std; using namespace gtsam; @@ -21,7 +21,7 @@ TEST( Pose2Config, pose2Circle ) expected.insert(2, Pose2(-1, 0, - M_PI_2)); expected.insert(3, Pose2( 0, -1, 0 )); - Pose2Config actual = pose2Circle(4,1.0); + Pose2Config actual = pose2SLAM::circle(4,1.0); CHECK(assert_equal(expected,actual)); } @@ -37,7 +37,7 @@ TEST( Pose2Config, expmap ) // Note expmap coordinates are in local coordinates, so shifting to right requires thought !!! Vector delta = Vector_(12, 0.0,-0.1,0.0, -0.1,0.0,0.0, 0.0,0.1,0.0, 0.1,0.0,0.0); - Pose2Config actual = expmap(pose2Circle(4,1.0),delta); + Pose2Config actual = expmap(pose2SLAM::circle(4,1.0),delta); CHECK(assert_equal(expected,actual)); } diff --git a/cpp/testPose2Factor.cpp b/cpp/testPose2Factor.cpp index 9c07c967a..ad6ab5829 100644 --- a/cpp/testPose2Factor.cpp +++ b/cpp/testPose2Factor.cpp @@ -6,7 +6,7 @@ #include #include "numericalDerivative.h" -#include "Pose2Graph.h" +#include "pose2SLAM.h" using namespace std; using namespace gtsam; diff --git a/cpp/testPose2Prior.cpp b/cpp/testPose2Prior.cpp index c2c4c2eae..992c1c9da 100644 --- a/cpp/testPose2Prior.cpp +++ b/cpp/testPose2Prior.cpp @@ -6,7 +6,7 @@ #include #include "numericalDerivative.h" -#include "Pose2Prior.h" +#include "pose2SLAM.h" using namespace std; using namespace gtsam; diff --git a/cpp/testPose2Graph.cpp b/cpp/testPose2SLAM.cpp similarity index 98% rename from cpp/testPose2Graph.cpp rename to cpp/testPose2SLAM.cpp index 6d32f172c..f9d22c2d4 100644 --- a/cpp/testPose2Graph.cpp +++ b/cpp/testPose2SLAM.cpp @@ -14,7 +14,7 @@ using namespace boost::assign; #include "NonlinearOptimizer-inl.h" #include "FactorGraph-inl.h" #include "Ordering.h" -#include "Pose2Graph.h" +#include "pose2SLAM.h" using namespace std; using namespace gtsam; @@ -115,7 +115,7 @@ TEST(Pose2Graph, optimize) { TEST(Pose2Graph, optimizeCircle) { // Create a hexagon of poses - Pose2Config hexagon = pose2Circle(6,1.0); + Pose2Config hexagon = pose2SLAM::circle(6,1.0); Pose2 p0 = hexagon[0], p1 = hexagon[1]; // create a Pose graph with one equality constraint and one measurement diff --git a/myconfigure b/myconfigure index 3ce66169c..12658b16c 100755 --- a/myconfigure +++ b/myconfigure @@ -1,4 +1,4 @@ ./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ # for maximum performance on Intel Core2 platform: -#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS="-O3 -march=nocona -mtune=generic -DNDEBUG" --disable-static \ No newline at end of file +#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS=" -g -ftree-vectorize O3 -march=nocona -mtune=generic -DNDEBUG" --disable-static \ No newline at end of file