diff --git a/.cproject b/.cproject index 79cf43d60..7eaf0065f 100644 --- a/.cproject +++ b/.cproject @@ -340,6 +340,7 @@ make +-j2 testVSLAMFactor.run true true @@ -355,6 +356,7 @@ make +-j2 testGaussianConditional.run true true @@ -378,6 +380,7 @@ make +-j2 testNonlinearOptimizer.run true true @@ -401,6 +404,7 @@ make +-j2 testNonlinearFactorGraph.run true true @@ -416,6 +420,7 @@ make +-j2 testVectorConfig.run true true @@ -423,6 +428,7 @@ make +-j2 testPoint2.run true true @@ -462,6 +468,7 @@ make + testBayesTree.run true false @@ -469,7 +476,6 @@ make - testSymbolicBayesNet.run true false @@ -477,6 +483,7 @@ make + testSymbolicFactorGraph.run true false @@ -500,6 +507,7 @@ make +-j2 testVSLAMGraph.run true true @@ -507,6 +515,7 @@ make +-j2 testNonlinearEquality.run true true @@ -522,6 +531,7 @@ make +-j2 testNonlinearConstraint.run true true @@ -561,6 +571,7 @@ make +-j2 testControlGraph.run true true @@ -576,6 +587,7 @@ make +-j2 testPose2Constraint.run true true @@ -599,6 +611,7 @@ make +-j2 testPose3Factor.run true true @@ -606,6 +619,7 @@ make +-j2 testUrbanGraph.run true true @@ -613,6 +627,7 @@ make +-j2 testUrbanConfig.run true true @@ -620,6 +635,7 @@ make +-j2 testUrbanMeasurement.run true true @@ -635,6 +651,7 @@ make +-j2 testIterative.run true true @@ -642,6 +659,7 @@ make +-j2 testGaussianISAM2.run true true @@ -665,6 +683,7 @@ make +-j2 testPose2Factor.run true true @@ -672,6 +691,7 @@ make +-j2 timeRot3.run true true @@ -679,6 +699,7 @@ make +-j2 install true true @@ -686,7 +707,6 @@ make - testPose2Graph.run true true @@ -694,14 +714,30 @@ make - testPose2Config.run true true true + +make +-j2 +testPose3Config.run +true +true +true + + +make +-j2 +testPose3Graph.run +true +true +true + make +-j2 install true true @@ -709,6 +745,7 @@ make +-j2 clean true true @@ -716,6 +753,7 @@ make +-j2 check true true diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 170054092..5587cfd69 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -194,18 +194,27 @@ testSimulated3D_SOURCES = testSimulated3D.cpp testSimulated3D_LDADD = libgtsam.la check_PROGRAMS += testSimulated3D -# Pose constraints -headers += BetweenFactor.h Pose2Factor.h Pose2Prior.h Pose3Factor.h +# 2D Pose constraints +headers += BetweenFactor.h Pose2Factor.h Pose2Prior.h sources += Pose2Config.cpp Pose2Graph.cpp -check_PROGRAMS += testPose2Factor testPose2Config testPose2Graph testPose3Factor +check_PROGRAMS += testPose2Factor testPose2Config testPose2Graph 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 + +# 3D Pose constraints +headers += Pose3Factor.h +sources += Pose3Config.cpp Pose3Graph.cpp +check_PROGRAMS += testPose3Factor testPose3Config testPose3Graph testPose3Factor_SOURCES = $(example) testPose3Factor.cpp testPose3Factor_LDADD = libgtsam.la +testPose3Config_SOURCES = $(example) testPose3Config.cpp +testPose3Config_LDADD = libgtsam.la +testPose3Graph_SOURCES = $(example) testPose3Graph.cpp +testPose3Graph_LDADD = libgtsam.la # Cameras sources += CalibratedCamera.cpp SimpleCamera.cpp diff --git a/cpp/Pose3Config.cpp b/cpp/Pose3Config.cpp new file mode 100644 index 000000000..b307959f2 --- /dev/null +++ b/cpp/Pose3Config.cpp @@ -0,0 +1,40 @@ +/** + * @file Pose3Config.cpp + * @brief Configuration of 3D poses + * @author Frank Dellaert + */ + +#include "Pose3Config.h" +#include "LieConfig-inl.h" + +using namespace std; + +namespace gtsam { + + /** Explicit instantiation of templated methods and functions */ + template class LieConfig; + template size_t dim(const LieConfig& c); + template LieConfig expmap(const LieConfig& c, const Vector& delta); + template LieConfig expmap(const LieConfig& c, const VectorConfig& delta); + + /* ************************************************************************* */ + // TODO: local version, should probably defined in LieConfig + static string symbol(char c, int index) { + stringstream ss; + ss << c << index; + return ss.str(); + } + + /* ************************************************************************* */ + Pose3Config pose3Circle(size_t n, double R, char c) { + Pose3Config x; + double theta = 0, dtheta = 2*M_PI/n; + // Vehicle at p0 is looking towards y axis + Rot3 R0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); + for (size_t i = 0; i < n; i++, theta += dtheta) + x.insert(symbol(c,i), Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta),sin(theta),0))); + return x; + } + + /* ************************************************************************* */ +} // namespace diff --git a/cpp/Pose3Config.h b/cpp/Pose3Config.h new file mode 100644 index 000000000..655bc4cbe --- /dev/null +++ b/cpp/Pose3Config.h @@ -0,0 +1,29 @@ +/** + * @file Pose3Config.cpp + * @brief Configuration of 3D poses + * @author Frank Dellaert + */ + +#pragma once + +#include "Pose3.h" +#include "LieConfig.h" + +namespace gtsam { + + /** + * Pose3Config is now simply a typedef + */ + typedef LieConfig Pose3Config; + + /** + * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0,0) + * The convention used is the Navlab/Aerospace convention: X-forward,Y-right,Z-down + * @param n number of poses + * @param R radius of circle + * @param c character to use for keys + * @return circle of n 2D poses + */ + Pose3Config pose3Circle(size_t n, double R, char c = 'p'); + +} // namespace diff --git a/cpp/Pose3Factor.h b/cpp/Pose3Factor.h index 7c386438a..03d891f79 100644 --- a/cpp/Pose3Factor.h +++ b/cpp/Pose3Factor.h @@ -5,18 +5,11 @@ #pragma once -#include -#include "Pose3.h" -#include "LieConfig.h" +#include "Pose3Config.h" #include "BetweenFactor.h" namespace gtsam { - /** - * A config specifically for 3D poses - */ - typedef LieConfig Pose3Config; - /** * A Factor for 3D pose measurements * This is just a typedef now diff --git a/cpp/Pose3Graph.cpp b/cpp/Pose3Graph.cpp new file mode 100644 index 000000000..ad2a52369 --- /dev/null +++ b/cpp/Pose3Graph.cpp @@ -0,0 +1,22 @@ +/** + * @file Pose3Graph.cpp + * @brief A factor graph for the 2D PoseSLAM problem + * @authors Frank Dellaert, Viorela Ila + */ +//#include "NonlinearOptimizer-inl.h" +#include "FactorGraph-inl.h" +#include "NonlinearFactorGraph-inl.h" +#include "Pose3Graph.h" + +namespace gtsam { + +// explicit instantiation so all the code is there and we can link with it +template class FactorGraph > ; +template class NonlinearFactorGraph ; +//template class NonlinearOptimizer ; + +bool Pose3Graph::equals(const Pose3Graph& p, double tol) const { + return false; +} + +} // namespace gtsam diff --git a/cpp/Pose3Graph.h b/cpp/Pose3Graph.h new file mode 100644 index 000000000..d6d4fba51 --- /dev/null +++ b/cpp/Pose3Graph.h @@ -0,0 +1,46 @@ +/** + * @file Pose3Graph.h + * @brief A factor graph for the 3D PoseSLAM problem + * @author Frank Dellaert + * @author Viorela Ila + */ + +#pragma once + +#include "NonlinearFactorGraph.h" +#include "Pose3Factor.h" +#include "Pose3Config.h" + +namespace gtsam{ + +/** + * Non-linear factor graph for visual SLAM + */ +class Pose3Graph : public gtsam::NonlinearFactorGraph { + +public: + + /** default constructor is empty graph */ + Pose3Graph() {} + + /** + * equals + */ + bool equals(const Pose3Graph& p, double tol=1e-9) const; + + /** + * Add a factor without having to do shared factor dance + */ + inline void add(const std::string& key1, const std::string& key2, + const Pose3& measured, const Matrix& covariance) { + push_back(sharedFactor(new Pose3Factor(key1, key2, measured, covariance))); + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) {} +}; + +} // namespace gtsam diff --git a/cpp/testPose3Config.cpp b/cpp/testPose3Config.cpp new file mode 100644 index 000000000..9500cb5fc --- /dev/null +++ b/cpp/testPose3Config.cpp @@ -0,0 +1,61 @@ +/** + * @file testPose3Config.cpp + * @authors Frank Dellaert + **/ + +#include + +#include +#include "Pose3Config.h" + +using namespace std; +using namespace gtsam; + +// The world coordinate system has z pointing up, y north, x east +// The vehicle has X forward, Y right, Z down +Rot3 R1(Point3( 0, 1, 0), Point3( 1, 0, 0), Point3(0, 0, -1)); +Rot3 R2(Point3(-1, 0, 0), Point3( 0, 1, 0), Point3(0, 0, -1)); +Rot3 R3(Point3( 0,-1, 0), Point3(-1, 0, 0), Point3(0, 0, -1)); +Rot3 R4(Point3( 1, 0, 0), Point3( 0,-1, 0), Point3(0, 0, -1)); + +/* ************************************************************************* */ +TEST( Pose3Config, pose3Circle ) +{ + // expected is 4 poses tangent to circle with radius 1m + Pose3Config expected; + expected.insert("p0", Pose3(R1, Point3( 1, 0, 0))); + expected.insert("p1", Pose3(R2, Point3( 0, 1, 0))); + expected.insert("p2", Pose3(R3, Point3(-1, 0, 0))); + expected.insert("p3", Pose3(R4, Point3( 0,-1, 0))); + + Pose3Config actual = pose3Circle(4,1.0,'p'); + CHECK(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST( Pose3Config, expmap ) +{ + // expected is circle shifted to East + Pose3Config expected; + expected.insert("p0", Pose3(R1, Point3( 1.1, 0, 0))); + expected.insert("p1", Pose3(R2, Point3( 0.1, 1, 0))); + expected.insert("p2", Pose3(R3, Point3(-0.9, 0, 0))); + expected.insert("p3", Pose3(R4, Point3( 0.1,-1, 0))); + + // Note expmap coordinates are in global coordinates with non-compose expmap + // so shifting to East requires little thought, different from with Pose2 !!! + Vector delta = Vector_(24, + 0.0,0.0,0.0, 0.1, 0.0, 0.0, + 0.0,0.0,0.0, 0.1, 0.0, 0.0, + 0.0,0.0,0.0, 0.1, 0.0, 0.0, + 0.0,0.0,0.0, 0.1, 0.0, 0.0); + Pose3Config actual = expmap(pose3Circle(4,1.0,'p'),delta); + CHECK(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/cpp/testPose3Factor.cpp b/cpp/testPose3Factor.cpp index 86948c8a4..ecf4df66c 100644 --- a/cpp/testPose3Factor.cpp +++ b/cpp/testPose3Factor.cpp @@ -7,6 +7,7 @@ #include #include using namespace boost::assign; + #include #include "Pose3Factor.h" #include "LieConfig-inl.h" @@ -37,13 +38,6 @@ TEST( Pose3Factor, error ) CHECK(assert_equal(expected,actual)); } -/* ************************************************************************* */ -TEST( Pose3Factor, simple_circle ) -{ - Pose3Config expected, actual; - CHECK(assert_equal(expected,actual)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/cpp/testPose3Graph.cpp b/cpp/testPose3Graph.cpp new file mode 100644 index 000000000..d5a98d4f3 --- /dev/null +++ b/cpp/testPose3Graph.cpp @@ -0,0 +1,87 @@ +/** + * @file testPose3Graph.cpp + * @authors Frank Dellaert, Viorela Ila + **/ + +#include + +#include +#include +#include +using namespace boost::assign; + +#include + +#include "NonlinearOptimizer-inl.h" +#include "NonlinearEquality.h" +#include "Ordering.h" +#include "Pose3Graph.h" + +using namespace std; +using namespace gtsam; + +// common measurement covariance +static Matrix covariance = eye(6); + +/* ************************************************************************* */ +bool poseCompare(const std::string& key, + const gtsam::Pose3Config& feasible, + const gtsam::Pose3Config& input) { + return feasible.get(key).equals(input.get(key)); +} + +/* ************************************************************************* */ +// test optimization with 6 poses arranged in a hexagon and a loop closure +TEST(Pose3Graph, optimizeCircle) { + + // Create a hexagon of poses + Pose3Config hexagon = pose3Circle(6,1.0,'p'); + Pose3 p0 = hexagon["p0"], p1 = hexagon["p1"]; + + // create a Pose graph with one equality constraint and one measurement + Pose3Graph fg; + Pose3Config feasible; + feasible.insert("p0", p0); + fg.push_back(Pose3Graph::sharedFactor( + new NonlinearEquality("p0", feasible, dim(Pose3()), poseCompare))); + Pose3 delta = between(p0,p1); + fg.add("p0", "p1", delta, covariance); + fg.add("p1", "p2", delta, covariance); + fg.add("p2", "p3", delta, covariance); + fg.add("p3", "p4", delta, covariance); + fg.add("p4", "p5", delta, covariance); + fg.add("p5", "p0", delta, covariance); + + // Create initial config + boost::shared_ptr initial(new Pose3Config()); + initial->insert("p0", p0); + initial->insert("p1", expmap(hexagon["p1"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert("p2", expmap(hexagon["p2"],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); + initial->insert("p3", expmap(hexagon["p3"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert("p4", expmap(hexagon["p4"],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); + initial->insert("p5", expmap(hexagon["p5"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + + // Choose an ordering and optimize + Ordering ordering; + ordering += "p0","p1","p2","p3","p4","p5"; + typedef NonlinearOptimizer Optimizer; + Optimizer optimizer(fg, ordering, initial); + Optimizer::verbosityLevel verbosity = Optimizer::SILENT; +// Optimizer::verbosityLevel verbosity = Optimizer::ERROR; + optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity); + + Pose3Config actual = *optimizer.config(); + + // Check with ground truth + CHECK(assert_equal(hexagon, actual,1e-5)); + + // Check loop closure + CHECK(assert_equal(delta,between(actual["p5"],actual["p0"]),1e-5)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */