uncommented urban In Makefile.am as it prevents linking; added Pose2Prior; ISAM2/planar now working

cpp/Makefile.am

@@ -178,7 +178,7 @@ testSimulated3D_LDADD = libgtsam.la
 check_PROGRAMS += testSimulated3D 
 
 # Pose constraints
-headers += Pose2Config.h Pose2Factor.h
+headers += Pose2Config.h Pose2Factor.h Pose2Prior.h
 sources += Pose3Factor.cpp Pose2Graph.cpp
 check_PROGRAMS += testPose2Factor testPose2Graph testPose3Factor
 testPose2Factor_SOURCES = $(example) testPose2Factor.cpp
@@ -189,16 +189,16 @@ testPose3Factor_SOURCES = $(example) testPose3Factor.cpp
 testPose3Factor_LDADD   = libgtsam.la
 
 # Urban
-sources += UrbanFactor.cpp UrbanConfig.cpp UrbanMeasurement.cpp UrbanOdometry.cpp UrbanGraph.cpp
+#sources += UrbanFactor.cpp UrbanConfig.cpp UrbanMeasurement.cpp UrbanOdometry.cpp UrbanGraph.cpp
-check_PROGRAMS += testUrbanConfig testUrbanMeasurement testUrbanOdometry testUrbanGraph
+#check_PROGRAMS += testUrbanConfig testUrbanMeasurement testUrbanOdometry testUrbanGraph
-testUrbanConfig_SOURCES       = $(example) testUrbanConfig.cpp
+#testUrbanConfig_SOURCES       = $(example) testUrbanConfig.cpp
-testUrbanConfig_LDADD         = libgtsam.la
+#testUrbanConfig_LDADD         = libgtsam.la
-testUrbanMeasurement_SOURCES  = $(example) testUrbanMeasurement.cpp
+#testUrbanMeasurement_SOURCES  = $(example) testUrbanMeasurement.cpp
-testUrbanMeasurement_LDADD    = libgtsam.la
+#testUrbanMeasurement_LDADD    = libgtsam.la
-testUrbanOdometry_SOURCES     = $(example) testUrbanOdometry.cpp
+#testUrbanOdometry_SOURCES     = $(example) testUrbanOdometry.cpp
-testUrbanOdometry_LDADD       = libgtsam.la
+#testUrbanOdometry_LDADD       = libgtsam.la
-testUrbanGraph_SOURCES        = $(example) testUrbanGraph.cpp
+#testUrbanGraph_SOURCES        = $(example) testUrbanGraph.cpp
-testUrbanGraph_LDADD          = libgtsam.la
+#testUrbanGraph_LDADD          = libgtsam.la
 
 # Robot Control example system
 sources += ControlConfig.cpp ControlPoint.cpp ControlGraph.cpp

cpp/Pose2Prior.h

@@ -0,0 +1,67 @@
+/**
+ *  @file  Pose2Prior.h
+ *  @authors Michael Kaess
+ **/
+#pragma once
+
+#include <map>
+#include "NonlinearFactor.h"
+#include "GaussianFactor.h"
+#include "Pose2.h"
+#include "Matrix.h"
+#include "Pose2Config.h"
+#include "ostream"
+
+namespace gtsam {
+
+class Pose2Prior : public NonlinearFactor<Pose2Config> {
+private:
+	std::string key_;
+	Pose2 measured_;
+	Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
+	std::list<std::string> keys_;
+
+public:
+
+	typedef boost::shared_ptr<Pose2Prior> shared_ptr; // shorthand for a smart pointer to a factor
+
+	Pose2Prior(const std::string& key, const Pose2& measured, const Matrix& measurement_covariance) :
+			  key_(key),measured_(measured) {
+		square_root_inverse_covariance_ = inverse_square_root(measurement_covariance);
+		keys_.push_back(key);
+	}
+
+	/** implement functions needed for Testable */
+	void print(const std::string& name) const {
+		std::cout << name << std::endl;
+		std::cout << "Factor "<< std::endl;
+		std::cout << "key "<< key_<<std::endl;
+		measured_.print("measured ");
+		gtsam::print(square_root_inverse_covariance_,"MeasurementCovariance");
+	}
+	bool equals(const NonlinearFactor<Pose2Config>& expected, double tol) const {return false;}
+
+	/** implement functions needed to derive from Factor */
+	Vector error_vector(const Pose2Config& config) const {
+		Pose2 p = config.get(key_);
+		return -p.log(measured_);
+	}
+
+	std::list<std::string> keys() const { return keys_; }
+	std::size_t size() const { return keys_.size(); }
+
+	/** linearize */
+	boost::shared_ptr<GaussianFactor> linearize(const Pose2Config& config) const {
+		Pose2 p = config.get(key_);
+		Vector b = -p.log(measured_);
+		Matrix H(3,3);
+		H(0,0)=1; H(0,1)=0; H(0,2)=0;
+		H(1,0)=0; H(1,1)=1; H(1,2)=0;
+		H(2,0)=0; H(2,1)=0; H(2,2)=1;
+		boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(
+				key_, square_root_inverse_covariance_ * H,
+				b, 1.0));
+		return linearized;
+	}
+};
+} /// namespace gtsam