rename Pose2Constraint using Pose2Factor

cpp/Makefile.am

@@ -126,10 +126,10 @@ headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h
 headers += SQPOptimizer.h SQPOptimizer-inl.h  
 headers += NonlinearConstraint.h NonlinearConstraint-inl.h
 headers += Pose2Config.h
-headers += Pose2Constraint.h
+headers += Pose2Factor.h
 sources += NonlinearFactor.cpp 
 sources += NonlinearEquality.h
-check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer testPose2Constraint
+check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer testPose2Factor
 testNonlinearFactor_SOURCES      = $(example) testNonlinearFactor.cpp
 testNonlinearFactor_LDADD        = libgtsam.la
 testNonlinearConstraint_SOURCES  = $(example) testNonlinearConstraint.cpp
@@ -144,8 +144,8 @@ testNonlinearEquality_SOURCES    = testNonlinearEquality.cpp
 testNonlinearEquality_LDADD      = libgtsam.la
 testSQP_SOURCES                  = $(example) testSQP.cpp
 testSQP_LDADD                    = libgtsam.la
-testPose2Constraint_SOURCES      = $(example) testPose2Constraint.cpp
+testPose2Factor_SOURCES      = $(example) testPose2Factor.cpp
-testPose2Constraint_LDADD        = libgtsam.la
+testPose2Factor_LDADD        = libgtsam.la
 
 # geometry
 sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp

cpp/Pose2Factor.h

@@ -1,5 +1,5 @@
 /**
- *  @file  Pose2Constraint.H
+ *  @file  Pose2Factor.H
  *  @authors Frank Dellaert, Viorela Ila
  **/
 
@@ -9,23 +9,31 @@
 #include "Pose2.h"
 #include "Matrix.h"
 #include "Pose2Config.h"
+#include "ostream"
 
 namespace gtsam {
 
-class Pose2Constraint : public Factor<Pose2Config> {
+class Pose2Factor : 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,
+	Pose2Factor(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 {}
+	void print(const std::string& name) const {
+		std::cout << name << std::endl;
+		std::cout << "Pose2Contraint"<< std::endl;
+		std::cout << "key1 "<< key1_<<std::endl;
+		std::cout << "key2 "<< key2_<<std::endl;
+		measured_.print("measured ");
+		gtsam::print(square_root_inverse_covariance_,"MeasurementCovariance");
+	}
 	bool equals(const Factor<Pose2Config>& expected, double tol) const {return false;}
 
 	/** implement functions needed to derive from Factor */

cpp/gtsam.h

@@ -168,7 +168,7 @@ class Pose2{
 	double y();
 	double theta();
 	size_t dim() const;
-	Pose2 exmap(const Vector& v) const;
+	Pose2 exmap(Vector v) const;
 	Vector vector() const;
 	Pose2 rotate(double theta) const;
 };
@@ -177,11 +177,11 @@ class Pose2Config{
 	Pose2 get(string key) const;
 };
 
-class Pose2Constraint {
+class Pose2Factor {
-	Pose2Constraint(string key1, string key2,
+	Pose2Factor(string key1, string key2,
-			const Pose2& measured, const Matrix& measurement_covariance);
+			const Pose2& measured, Matrix measurement_covariance);
 	void print(string name) const;
-	bool equals(const Pose2Constraint& expected, double tol) const;
+	bool equals(const Pose2Factor& expected, double tol) const;
 	double error(const Pose2Config& c) const;
 	size_t size() const;
 	GaussianFactor* linearize(const Pose2Config& config) const;

cpp/testPose2Factor.cpp

@@ -1,6 +1,6 @@
 /**
  *  @file  testPose2Constraint.cpp
- *  @brief Unit tests for Pose2Constraint Class
+ *  @brief Unit tests for Pose2Factor Class
  *  @authors Frank Dellaert, Viorela Ila
  **/
 
@@ -8,13 +8,13 @@
 #include <iostream>
 
 #include <CppUnitLite/TestHarness.h>
-#include "Pose2Constraint.h"
+#include "Pose2Factor.h"
 
 using namespace std;
 using namespace gtsam;
 
 
-TEST( Pose2Constraint, constructor )
+TEST( Pose2Factor, constructor )
 {
 	// create a factor between unknown poses p1 and p2
 	Pose2 measured(2,2,M_PI_2);
@@ -23,7 +23,7 @@ TEST( Pose2Constraint, constructor )
 			0.0, 0.25, 0.0,
 			0.0, 0.0, 0.01
 			);
-	Pose2Constraint constraint("p1","p2",measured, measurement_covariance);
+	Pose2Factor 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)
@@ -35,6 +35,7 @@ TEST( Pose2Constraint, constructor )
 	// Linearize
 	boost::shared_ptr<GaussianFactor> actual = constraint.linearize(config);
 
+
 	// expected
 	Matrix expectedH1 = Matrix_(3,3,
 			0.0,-1.0,2.1,
@@ -59,6 +60,7 @@ TEST( Pose2Constraint, constructor )
 
 	CHECK(assert_equal(expected,*actual));
 }
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;