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);
+}
+/* ************************************************************************* */