diff --git a/.cproject b/.cproject index 59c321651..79cf43d60 100644 --- a/.cproject +++ b/.cproject @@ -692,6 +692,14 @@ true true + +make + +testPose2Config.run +true +true +true + make install diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 58fdacdc0..170054092 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -197,9 +197,11 @@ check_PROGRAMS += testSimulated3D # Pose constraints headers += BetweenFactor.h Pose2Factor.h Pose2Prior.h Pose3Factor.h sources += Pose2Config.cpp Pose2Graph.cpp -check_PROGRAMS += testPose2Factor testPose2Graph testPose3Factor +check_PROGRAMS += testPose2Factor testPose2Config testPose2Graph testPose3Factor 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 testPose3Factor_SOURCES = $(example) testPose3Factor.cpp diff --git a/cpp/Pose2Config.cpp b/cpp/Pose2Config.cpp index d51b9eefd..f91f5ebb7 100644 --- a/cpp/Pose2Config.cpp +++ b/cpp/Pose2Config.cpp @@ -63,5 +63,22 @@ namespace gtsam { return newConfig; } + /* ************************************************************************* */ + // TODO: local version, should probably defined in LieConfig + static string symbol(char c, int index) { + stringstream ss; + ss << c << index; + return ss.str(); + } + + /* ************************************************************************* */ + Pose2Config pose2Circle(size_t n, double R, char c) { + Pose2Config x; + double theta = 0, dtheta = 2*M_PI/n; + for(size_t i=0;i + +#include +#include "Pose2Config.h" + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( Pose2Config, pose2Circle ) +{ + // expected is 4 poses tangent to circle with radius 1m + Pose2Config expected; + expected.insert("p0", Pose2( 1, 0, M_PI_2)); + expected.insert("p1", Pose2( 0, 1, - M_PI )); + expected.insert("p2", Pose2(-1, 0, - M_PI_2)); + expected.insert("p3", Pose2( 0, -1, 0 )); + + Pose2Config actual = pose2Circle(4,1.0,'p'); + CHECK(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */