add Pose2Graph

cpp/Makefile.am

@@ -125,11 +125,11 @@ 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 += Pose2Config.h
-headers += Pose2Factor.h
+headers += NonlinearEquality.h
+headers += Pose2Config.h Pose2Factor.h 
 sources += NonlinearFactor.cpp 
-sources += NonlinearEquality.h
-check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer testPose2Factor
+sources += Pose2Graph.cpp
+check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer testPose2Factor testPose2Graph
 testNonlinearFactor_SOURCES      = $(example) testNonlinearFactor.cpp
 testNonlinearFactor_LDADD        = libgtsam.la
 testNonlinearConstraint_SOURCES  = $(example) testNonlinearConstraint.cpp
@@ -144,8 +144,10 @@ testNonlinearEquality_SOURCES    = testNonlinearEquality.cpp
 testNonlinearEquality_LDADD      = libgtsam.la
 testSQP_SOURCES                  = $(example) testSQP.cpp
 testSQP_LDADD                    = libgtsam.la
-testPose2Factor_SOURCES      = $(example) testPose2Factor.cpp
-testPose2Factor_LDADD        = libgtsam.la
+testPose2Factor_SOURCES          = $(example) testPose2Factor.cpp
+testPose2Factor_LDADD       	 = libgtsam.la
+testPose2Graph_SOURCES      	 = $(example) testPose2Graph.cpp
+testPose2Graph_LDADD        	 = libgtsam.la
 
 # geometry
 sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp

cpp/Pose2Config.h

@@ -1,3 +1,5 @@
+#pragma once
+
 #include <map>
 #include "Pose2.h"
 
@@ -15,5 +17,8 @@ public:
 			throw std::invalid_argument("invalid key");
 		return it->second;
 	}
+	void insert(const std::string& name, const Pose2& val){
+		std::map<std::string, Pose2>::insert(make_pair(name, val));
+	}
 };
 } // namespace

cpp/Pose2Factor.h

@@ -2,8 +2,10 @@
  *  @file  Pose2Factor.H
  *  @authors Frank Dellaert, Viorela Ila
  **/
+#pragma once
 
 #include <map>
+#include "NonlinearFactor.h"
 #include "GaussianFactor.h"
 #include "VectorConfig.h"
 #include "Pose2.h"
@@ -13,13 +15,16 @@
 
 namespace gtsam {
 
-class Pose2Factor : public Factor<Pose2Config> {
+class Pose2Factor : public NonlinearFactor<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:
+
+	typedef boost::shared_ptr<Pose2Factor> shared_ptr; // shorthand for a smart pointer to a factor
+
 	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);
@@ -28,16 +33,21 @@ public:
 	/** implement functions needed for Testable */
 	void print(const std::string& name) const {
 		std::cout << name << std::endl;
-		std::cout << "Pose2Contraint"<< std::endl;
+		std::cout << "Factor "<< 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;}
+	bool equals(const NonlinearFactor<Pose2Config>& expected, double tol) const {return false;}
 
 	/** implement functions needed to derive from Factor */
-	double error(const Pose2Config& c) const {return 0;}
+	Vector error_vector(const Pose2Config& config) const {
+		//z-h
+		Pose2 p1 = config.get(key1_), p2 = config.get(key2_);
+		return (measured_ - between(p1,p2)).vector();
+	}
+
 	std::list<std::string> keys() const { std::list<std::string> l; return l; }
 	std::size_t size() const { return 2;}
 

cpp/Pose2Graph.cpp

@@ -0,0 +1,25 @@
+/**
+ * @file    Pose2Graph.cpp
+ * @brief   A factor graph for the 2D PoseSLAM problem
+ * @authors Frank Dellaert, Viorela Ila
+ */
+//#include "NonlinearOptimizer-inl.h"
+#include "FactorGraph-inl.h"
+#include "NonlinearFactorGraph-inl.h"
+#include "Pose2Graph.h"
+
+namespace gtsam {
+
+// explicit instantiation so all the code is there and we can link with it
+template class FactorGraph<NonlinearFactor<gtsam::Pose2Config> > ;
+template class NonlinearFactorGraph<Pose2Factor> ;
+//template class NonlinearOptimizer<Pose2Graph, Pose2Config> ;
+
+void Pose2Graph::print(const std::string& s) const {
+}
+
+bool Pose2Graph::equals(const Pose2Graph& p, double tol) const {
+	return false;
+}
+
+} // namespace gtsam

cpp/Pose2Graph.h

@@ -0,0 +1,47 @@
+/**
+ * @file    Pose2Graph.h
+ * @brief   A factor graph for the 2D PoseSLAM problem
+ * @author  Frank Dellaert
+ * @author  Viorela Ila
+ */
+
+#pragma once
+
+
+#include "NonlinearFactorGraph.h"
+#include "FactorGraph.h"
+#include "Pose2Factor.h"
+#include "Pose2Config.h"
+#include "Testable.h"
+
+namespace gtsam{
+
+/**
+ * Non-linear factor graph for visual SLAM
+ */
+class Pose2Graph : public gtsam::NonlinearFactorGraph<Pose2Config>{
+
+public:
+
+  /** default constructor is empty graph */
+  Pose2Graph() {}
+
+  /**
+   * print out graph
+   */
+  void print(const std::string& s = "") const;
+
+  /**
+   * equals
+   */
+  bool equals(const Pose2Graph& p, double tol=1e-9) const;
+
+
+private:
+	/** Serialization function */
+	friend class boost::serialization::access;
+	template<class Archive>
+	void serialize(Archive & ar, const unsigned int version) {}
+};
+
+} // namespace gtsam

cpp/gtsam.h

@@ -176,6 +176,7 @@ class Pose2{
 class Pose2Config{
 	Pose2Config();
 	Pose2 get(string key) const;
+	void insert(string name, const Pose2& val);
 };
 
 class Pose2Factor {
@@ -187,3 +188,11 @@ class Pose2Factor {
 	size_t size() const;
 	GaussianFactor* linearize(const Pose2Config& config) const;
 };
+
+class Pose2Graph{
+	Pose2Graph();
+	void print(string s) const;
+	bool equals(const Pose2Graph& p, double tol) const;
+	GaussianFactorGraph linearize(const Pose2Config& config) const;
+	void push_back(Pose2Factor* factor);
+};

cpp/testPose2Factor.cpp

@@ -9,6 +9,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 #include "Pose2Factor.h"
+#include "Pose2Graph.h"
 
 using namespace std;
 using namespace gtsam;
@@ -29,13 +30,17 @@ TEST( Pose2Factor, constructor )
 	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));
+	config.insert("p1",p1);
+	config.insert("p2",p2);
+	//Pose2 pose1;
+	//pose1=config.get("p1");
+	//pose1.print("pose1");
 
 	// Linearize
 	boost::shared_ptr<GaussianFactor> actual = constraint.linearize(config);
 
 
+
 	// expected
 	Matrix expectedH1 = Matrix_(3,3,
 			0.0,-1.0,2.1,

cpp/testPose2Graph.cpp

@@ -0,0 +1,36 @@
+
+/*STL/C++*/
+#include <iostream>
+
+#include <CppUnitLite/TestHarness.h>
+#include "Pose2Graph.h"
+
+using namespace std;
+using namespace gtsam;
+
+
+TEST( Pose2Graph, 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
+			);
+	Pose2Factor constraint("p1","p2",measured, measurement_covariance);
+	Pose2Graph graph;
+	graph.push_back(Pose2Factor::shared_ptr(new Pose2Factor("p1","p2",measured, measurement_covariance)));
+	// get the size of the graph
+	size_t actual = graph.size();
+	// verify
+	size_t expected = 1;
+	CHECK(actual == expected);
+
+}
+/* ************************************************************************* */
+int main() {
+	TestResult tr;
+	return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */