pose2Circle

.cproject

@@ -692,6 +692,14 @@
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
+<target name="testPose2Config.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments/>
+<buildTarget>testPose2Config.run</buildTarget>
+<stopOnError>true</stopOnError>
+<useDefaultCommand>true</useDefaultCommand>
+<runAllBuilders>true</runAllBuilders>
+</target>
 <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
 <buildTarget>install</buildTarget>

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

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<n;i++, theta+=dtheta)
+			x.insert(symbol(c,i), Pose2(cos(theta), sin(theta), M_PI_2 + theta));
+		return x;
+	}
+
 	/* ************************************************************************* */
 } // namespace

cpp/Pose2Config.h

@@ -61,4 +61,13 @@ namespace gtsam {
    */
   Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta);
 
+  /**
+   * 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, char c = 'p');
+
 } // namespace

cpp/testPose2Config.cpp

@@ -0,0 +1,33 @@
+/**
+ *  @file  testPose2Config.cpp
+ *  @authors Frank Dellaert
+ **/
+
+#include <iostream>
+
+#include <CppUnitLite/TestHarness.h>
+#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);
+}
+/* ************************************************************************* */