add Pose2Constraint class

cpp/Makefile.am

@@ -121,9 +121,10 @@ headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
 headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h 
 headers += SQPOptimizer.h SQPOptimizer-inl.h  
 headers += NonlinearConstraint.h NonlinearConstraint-inl.h
+headers += Pose2Constraint.h
 sources += NonlinearFactor.cpp 
 sources += NonlinearEquality.h
-check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer
+check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer testPose2Constraint
 testNonlinearFactor_SOURCES      = $(example) testNonlinearFactor.cpp
 testNonlinearFactor_LDADD        = libgtsam.la
 testNonlinearConstraint_SOURCES  = $(example) testNonlinearConstraint.cpp
@@ -138,6 +139,8 @@ testNonlinearEquality_SOURCES    = testNonlinearEquality.cpp
 testNonlinearEquality_LDADD      = libgtsam.la
 testSQP_SOURCES                  = $(example) testSQP.cpp
 testSQP_LDADD                    = libgtsam.la
+testPose2Constraint_SOURCES      = $(example) testPose2Constraint.cpp
+testPose2Constraint_LDADD        = libgtsam.la
 
 # geometry
 sources += Point2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp

cpp/Pose2Constraint.h

@@ -0,0 +1,57 @@
+/**
+ *  @file  Pose2Constraint.H
+ *  @authors Frank Dellaert, Viorela Ila
+ **/
+
+#include <map>
+#include "GaussianFactor.h"
+#include "VectorConfig.h"
+#include "Pose2.h"
+#include "Matrix.h"
+
+namespace gtsam {
+
+class Pose2Config : public std::map<std::string,Pose2> {
+public:
+	Pose2 get(std::string key) const {
+		std::map<std::string,Pose2>::const_iterator it = find(key);
+		if (it==end()) throw std::invalid_argument("invalid key");
+		return it->second;
+	}
+};
+
+class Pose2Constraint : public Factor<Pose2Config> {
+private:
+	std::string key1_, key2_; /** The keys of the two poses, order matters */
+	Pose2 measured_;
+	Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
+
+public:
+	Pose2Constraint(const std::string& key1, const std::string& key2,
+			const Pose2& measured, const Matrix& measurement_covariance): key1_(key1),key2_(key2),measured_(measured) {
+		square_root_inverse_covariance_ = inverse_square_root(measurement_covariance);
+	}
+
+	/** implement functions needed for Testable */
+	void print(const std::string& name) const {}
+	bool equals(const Factor<Pose2Config>& expected, double tol) const {return false;}
+
+	/** implement functions needed to derive from Factor */
+	double error(const Pose2Config& c) const {return 0;}
+	std::list<std::string> keys() const { std::list<std::string> l; return l; }
+	std::size_t size() const { return 2;}
+
+	/** linearize */
+	boost::shared_ptr<GaussianFactor> linearize(const Pose2Config& config) const {
+		Pose2 p1 = config.get(key1_), p2 = config.get(key2_);
+		Vector b = (measured_ - between(p1,p2)).vector();
+		Matrix H1 = Dbetween1(p1,p2);
+		Matrix H2 = Dbetween2(p1,p2);
+		boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(
+				key1_, square_root_inverse_covariance_ * H1,
+				key2_, square_root_inverse_covariance_ * H2,
+				b, 1.0));
+		return linearized;
+	}
+};
+} /// namespace gtsam

cpp/gtsam.h

@@ -158,3 +158,16 @@ class Simulated2DMeasurement {
   void print(string s) const;
 };
 
+class Pose2Config{
+	Pose2 get(string key) const;
+};
+
+class Pose2Constraint {
+	Pose2Constraint(string key1, string key2,
+			const Pose2& measured, const Matrix& measurement_covariance);
+	void print(string name) const;
+	bool equals(const Pose2Constraint& expected, double tol) const;
+	double error(const Pose2Config& c) const;
+	size_t size() const;
+	GaussianFactor* linearize(const Pose2Config& config) const;
+};

cpp/testNonlinearFactor.cpp

@@ -172,100 +172,6 @@ TEST( NonlinearFactor, size )
 	CHECK(factor3->size() == 2);
 }
 
-/* ************************************************************************* */
-
-#include <map>
-#include "Pose2.h"
-class Pose2Config : public map<string,Pose2> {
-public:
-	Pose2 get(string key) const {
-		map<string,Pose2>::const_iterator it = find(key);
-		if (it==end()) throw std::invalid_argument("invalid key");
-		return it->second;
-	}
-};
-
-class Pose2Constraint : public Factor<Pose2Config> {
-private:
-	std::string key1_, key2_; /** The keys of the two poses, order matters */
-	Pose2 measured_;
-	Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
-
-public:
-	Pose2Constraint(const string& key1, const string& key2,
-			const Pose2& measured, const Matrix& measurement_covariance): key1_(key1),key2_(key2),measured_(measured) {
-		square_root_inverse_covariance_ = inverse_square_root(measurement_covariance);
-	}
-
-	/** implement functions needed for Testable */
-	void print(const std::string& name) const {}
-	bool equals(const Factor<Pose2Config>& expected, double tol) const {return false;}
-
-  /** implement functions needed to derive from Factor */
-	double error(const Pose2Config& c) const {return 0;}
-  std::list<std::string> keys() const { list<string> l; return l; }
-  std::size_t size() const { return 2;}
-
-  /** linearize */
-  boost::shared_ptr<GaussianFactor> linearize(const Pose2Config& config) const {
-  	Pose2 p1 = config.get(key1_), p2 = config.get(key2_);
-  	Vector b = (measured_ - between(p1,p2)).vector();
-  	Matrix H1 = Dbetween1(p1,p2);
-  	Matrix H2 = Dbetween2(p1,p2);
-  	boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(
-						key1_, square_root_inverse_covariance_ * H1,
-						key2_, square_root_inverse_covariance_ * H2,
-						b, 1.0));
-  	return linearized;
-  }
-};
-
-TEST( Pose2Constraint, constructor )
-{
-	// create a factor between unknown poses p1 and p2
-	Pose2 measured(2,2,M_PI_2);
-	Matrix measurement_covariance = Matrix_(3,3,
-			0.25, 0.0, 0.0,
-			0.0, 0.25, 0.0,
-			0.0, 0.0, 0.01
-			);
-	Pose2Constraint constraint("p1","p2",measured, measurement_covariance);
-
-	// Choose a linearization point
-	Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
-	Pose2 p2(-1,4.1,M_PI);  // robot at (-1,4) looking at negative (ground truth is at 4.1,2)
-	Pose2Config config;
-	config.insert(make_pair("p1",p1));
-	config.insert(make_pair("p2",p2));
-
-	// Linearize
-	boost::shared_ptr<GaussianFactor> actual = constraint.linearize(config);
-
-	// expected
-	Matrix expectedH1 = Matrix_(3,3,
-			0.0,-1.0,2.1,
-			1.0,0.0,-2.1,
-			0.0,0.0,-1.0
-			);
-	Matrix expectedH2 = Matrix_(3,3,
-			 0.0,1.0,0.0,
-			-1.0,0.0,0.0,
-			 0.0,0.0,1.0
-			 );
-	// we need the minus signs as inverse_square_root uses SVD and sign is simply arbitrary (still a ssquare root!)
-	Matrix square_root_inverse_covariance = Matrix_(3,3,
-			-2.0, 0.0, 0.0,
-			0.0, -2.0, 0.0,
-			0.0, 0.0, -10.0
-			);
-	GaussianFactor expected(
-			"p1", square_root_inverse_covariance*expectedH1,
-			"p2", square_root_inverse_covariance*expectedH2,
-			Vector_(3,-0.1,-0.1,0.0), 1.0);
-
-	CHECK(assert_equal(expected,*actual));
-}
-
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

cpp/testPose2Constraint.cpp

@@ -0,0 +1,67 @@
+/**
+ *  @file  testPose2Constraint.cpp
+ *  @brief Unit tests for Pose2Constraint Class
+ *  @authors Frank Dellaert, Viorela Ila
+ **/
+
+/*STL/C++*/
+#include <iostream>
+
+#include <CppUnitLite/TestHarness.h>
+#include "Pose2Constraint.h"
+
+using namespace std;
+using namespace gtsam;
+
+
+TEST( Pose2Constraint, constructor )
+{
+	// create a factor between unknown poses p1 and p2
+	Pose2 measured(2,2,M_PI_2);
+	Matrix measurement_covariance = Matrix_(3,3,
+			0.25, 0.0, 0.0,
+			0.0, 0.25, 0.0,
+			0.0, 0.0, 0.01
+			);
+	Pose2Constraint constraint("p1","p2",measured, measurement_covariance);
+
+	// Choose a linearization point
+	Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
+	Pose2 p2(-1,4.1,M_PI);  // robot at (-1,4) looking at negative (ground truth is at 4.1,2)
+	Pose2Config config;
+	config.insert(make_pair("p1",p1));
+	config.insert(make_pair("p2",p2));
+
+	// Linearize
+	boost::shared_ptr<GaussianFactor> actual = constraint.linearize(config);
+
+	// expected
+	Matrix expectedH1 = Matrix_(3,3,
+			0.0,-1.0,2.1,
+			1.0,0.0,-2.1,
+			0.0,0.0,-1.0
+			);
+	Matrix expectedH2 = Matrix_(3,3,
+			 0.0,1.0,0.0,
+			-1.0,0.0,0.0,
+			 0.0,0.0,1.0
+			 );
+	// we need the minus signs as inverse_square_root uses SVD and sign is simply arbitrary (still a ssquare root!)
+	Matrix square_root_inverse_covariance = Matrix_(3,3,
+			-2.0, 0.0, 0.0,
+			0.0, -2.0, 0.0,
+			0.0, 0.0, -10.0
+			);
+	GaussianFactor expected(
+			"p1", square_root_inverse_covariance*expectedH1,
+			"p2", square_root_inverse_covariance*expectedH2,
+			Vector_(3,-0.1,-0.1,0.0), 1.0);
+
+	CHECK(assert_equal(expected,*actual));
+}
+/* ************************************************************************* */
+int main() {
+	TestResult tr;
+	return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */