BetweenFactor is a template that models the measurement between two Lie types e.g. Pose2 or Pose3. Pose2Factor and Pose3Factor are now simply typedefs.

cpp/BetweenFactor.h

@@ -0,0 +1,91 @@
+/**
+ *  @file  BetweenFactor.h
+ *  @authors Frank Dellaert, Viorela Ila
+ **/
+#pragma once
+
+#include <ostream>
+
+#include "NonlinearFactor.h"
+#include "GaussianFactor.h"
+#include "Lie.h"
+#include "Matrix.h"
+
+namespace gtsam {
+
+	/**
+	 * A class for a measurement predicted by "between(config[key1],config[key2])"
+	 * T is the Lie group type, Config where the T's are gotten from
+	 */
+	template<class T, class Config>
+	class BetweenFactor: public NonlinearFactor<Config> {
+
+	private:
+
+		T measured_; /** The measurement */
+		std::string key1_, key2_; /** The keys of the two poses, order matters */
+		std::list<std::string> keys_; /** The keys as a list */
+		Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
+
+	public:
+
+		// shorthand for a smart pointer to a factor
+		typedef typename boost::shared_ptr<BetweenFactor> shared_ptr;
+
+		/** Constructor */
+		BetweenFactor(const std::string& key1, const std::string& key2,
+				const T& measured, const Matrix& measurement_covariance) :
+			key1_(key1), key2_(key2), measured_(measured) {
+			square_root_inverse_covariance_ = inverse_square_root(
+					measurement_covariance);
+			keys_.push_back(key1);
+			keys_.push_back(key2);
+		}
+
+		/** implement functions needed for Testable */
+
+		/** print */
+		void print(const std::string& name) const {
+			std::cout << name << 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");
+		}
+
+		/** equals */
+		bool equals(const NonlinearFactor<Config>& expected, double tol) const {
+			return key1_ == expected.key1_ && key2_ == expected.key2_
+					&& measured_.equals(expected.measured_, tol);
+		}
+
+		/** implement functions needed to derive from Factor */
+
+		/** vector of errors */
+		Vector error_vector(const Config& config) const {
+			//z-h
+			T p1 = config.get(key1_), p2 = config.get(key2_);
+			return -logmap(between(p1, p2), measured_);
+		}
+
+		/** keys as a list */
+		inline std::list<std::string> keys() const { return keys_;}
+
+		/** number of variables attached to this factor */
+		inline std::size_t size() const { return 2;}
+
+		/** linearize */
+		boost::shared_ptr<GaussianFactor> linearize(const Config& config) const {
+			T p1 = config.get(key1_), p2 = config.get(key2_);
+			Vector b = -logmap(between(p1, p2), measured_);
+			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/Makefile.am

@@ -193,8 +193,8 @@ testSimulated3D_LDADD = libgtsam.la
 check_PROGRAMS += testSimulated3D 
 
 # Pose constraints
-headers += Pose2Factor.h Pose2Prior.h
+headers += BetweenFactor.h Pose2Factor.h Pose2Prior.h Pose3Factor.h 
-sources += Pose2Config.cpp Pose2Graph.cpp Pose3Factor.cpp 
+sources += Pose2Config.cpp Pose2Graph.cpp 
 check_PROGRAMS += testPose2Factor testPose2Graph testPose3Factor
 testPose2Factor_SOURCES = $(example) testPose2Factor.cpp
 testPose2Factor_LDADD   = libgtsam.la

cpp/Pose2Factor.h

@@ -2,71 +2,16 @@
  *  @file  Pose2Factor.H
  *  @authors Frank Dellaert, Viorela Ila
  **/
+
 #pragma once
 
-#include <map>
+#include "BetweenFactor.h"
-#include "NonlinearFactor.h"
-#include "GaussianFactor.h"
-#include "VectorConfig.h"
 #include "Pose2.h"
-#include "Matrix.h"
 #include "Pose2Config.h"
-#include "ostream"
 
 namespace gtsam {
 
-class Pose2Factor : public NonlinearFactor<Pose2Config> {
+	/** This is just a typedef now */
-private:
+	typedef BetweenFactor<Pose2, Pose2Config> Pose2Factor;
-	std::string key1_, key2_; /** The keys of the two poses, order matters */
-	Pose2 measured_;
-	Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
-	std::list<std::string> keys_;
-
-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);
-		keys_.push_back(key1);
-		keys_.push_back(key2);
-	}
-
-	/** implement functions needed for Testable */
-	void print(const std::string& name) const {
-		std::cout << name << 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 NonlinearFactor<Pose2Config>& expected, double tol) const {return false;}
-
-	/** implement functions needed to derive from Factor */
-	Vector error_vector(const Pose2Config& config) const {
-		//z-h
-		Pose2 p1 = config.get(key1_), p2 = config.get(key2_);
-		return -logmap(between(p1,p2), 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 p1 = config.get(key1_), p2 = config.get(key2_);
-		Vector b = -logmap(between(p1,p2), measured_);
-		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/Pose3Factor.cpp

@@ -1,86 +0,0 @@
-/**
- * @file    UrbanOdometryFactor.cpp
- * @brief   A non-linear factor specialized to the Visual SLAM problem
- * @author  Frank Dellaert
- * @author  Viorela Ila
- */
-
-#include "Pose3Factor.h"
-
-using namespace std;
-namespace gtsam {
-
-/* ************************************************************************* *
-Pose3Factor::Pose3Factor() {
-	/// Arbitrary values
-	robotPoseNumber_ = 1;
-	robotPoseName_ = symbol('x',robotPoseNumber_);
-	keys_.push_back(robotPoseName_);
-}
-/* ************************************************************************* *
-Pose3Factor::Pose3Factor(const Point2& z, double sigma, int cn, int ln)
-: NonlinearFactor<Pose3Config>(z.vector(), sigma)
-  {
-	robotPoseNumber_ = cn;
-	landmarkNumber_    = ln;
-	robotPoseName_ = symbol('x',robotPoseNumber_);
-	landmarkName_    = symbol('l',landmarkNumber_);
-	keys_.push_back(robotPoseName_);
-	keys_.push_back(landmarkName_);
-  }
-
-/* ************************************************************************* *
-void Pose3Factor::print(const std::string& s) const {
-  printf("%s %s %s\n", s.c_str(), robotPoseName_.c_str(), landmarkName_.c_str());
-  gtsam::print(this->z_, s+".z");
-}
-
-/* ************************************************************************* *
-bool Pose3Factor::equals(const Pose3Factor& p, double tol) const {
-  if (&p == NULL) return false;
-  if (robotPoseNumber_ != p.robotPoseNumber_ || landmarkNumber_ != p.landmarkNumber_) return false;
-  if (!equal_with_abs_tol(this->z_,p.z_,tol)) return false;
-  return true;
-}
-
-/* ************************************************************************* *
-// TODO Implement transformPoint2_from
-// the difference here that we have a 2d point in a 3D coordinate
-Vector Pose3Factor::predict(const Pose3Config& c) const {
-  Pose3 pose = c.cameraPose(robotPoseNumber_);
-  Point2 landmark = c.landmarkPoint(landmarkNumber_);
-  return transformPoint2_from(SimpleCamera(*K_,pose), landmark).vector();
-}
-
-/* ************************************************************************* *
-Vector Pose3Factor::error_vector(const Pose3Config& c) const {
-  // Right-hand-side b = (z - h(x))/sigma
-  Point2 h = predict(c);
-  return (this->z_ - h);
-}
-
-/* ************************************************************************* *
-GaussianFactor::shared_ptr Pose3Factor::linearize(const Pose3Config& c) const
-{
-  // get arguments from config
-  Pose3 pose = c.cameraPose(robotPoseNumber_);
-  Point3 landmark = c.landmarkPoint(landmarkNumber_);
-
-  SimpleCamera camera(*K_,pose);
-
-  // Jacobians
-  Matrix Dh1 = Dproject_pose(camera, landmark);
-  Matrix Dh2 = Dproject_point(camera, landmark);
-
-  // Right-hand-side b = (z - h(x))
-  Vector h = project(camera, landmark).vector();
-  Vector b = this->z_ - h;
-
-  // Make new linearfactor, divide by sigma
-  GaussianFactor::shared_ptr
-    p(new GaussianFactor(robotPoseName_, Dh1, landmarkName_, Dh2, b, this->sigma_));
-  return p;
-}
-
-/* ************************************************************************* */
-} // namespace gtsam

cpp/Pose3Factor.h

@@ -1,88 +1,29 @@
 /**
- * @file    Pose3Factor.h
+ *  @file  Pose3Factor.H
- * @brief   A Nonlinear Factor, specialized for Urban application
+ *  @authors Frank Dellaert, Viorela Ila
- * @author  Frank Dellaert
+ **/
- * @author  Viorela Ila
- */
 
 #pragma once
 
 #include <map>
-#include "NonlinearFactor.h"
+#include "BetweenFactor.h"
-#include "GaussianFactor.h"
-#include "VectorConfig.h"
 #include "Pose3.h"
-#include "Matrix.h"
 
 namespace gtsam {
 
-	typedef VectorConfig Config;
+	class Pose3Config: public std::map<std::string, Pose3> {
-
-	class Pose3Factor: public NonlinearFactor<Config> {
-	private:
-		std::string key1_, key2_; /** The keys of the two poses, order matters */
-		Pose3 measured_;
-		Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
 
 	public:
 
-		typedef boost::shared_ptr<Pose3Factor> shared_ptr; // shorthand for a smart pointer to a factor
+		const Pose3& get(std::string key) const {
-
+			const_iterator it = find(key);
-		Pose3Factor(const std::string& key1, const std::string& key2,
+			if (it == end()) throw std::invalid_argument("invalid key");
-				const Pose3& measured, const Matrix& measurement_covariance) :
+			return it->second;
-			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 {
-			std::cout << name << 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 NonlinearFactor<Config>& expected, double tol) const {
-			return false;
-		}
-
-		/** implement functions needed to derive from Factor */
-		Vector error_vector(const Config& config) const {
-			//z-h
-			Pose3 p1 = config.get(key1_), p2 = config.get(key2_);
-			// todo: removed vector() from Pose3, so emulating it here!  This is incorrect!
-			Pose3 betw = between(p1,p2);
-			Vector r_diff = logmap(measured_.rotation()) - logmap(betw.rotation());
-			Vector t_diff = logmap(measured_.translation()) - logmap(betw.translation());
-			return concatVectors(2, &r_diff, &t_diff);
-		}
-
-		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 Config& config) const {
-			Pose3 p1 = config.get(key1_), p2 = config.get(key2_);
-            Pose3 betw = between(p1,p2);
-            // todo: removed vector() from Pose3, so emulating it here!  This is incorrect!
-            Vector r_diff = logmap(measured_.rotation()) - logmap(betw.rotation());
-            Vector t_diff = logmap(measured_.translation()) - logmap(betw.translation());
-			Vector b = concatVectors(2, &r_diff, &t_diff);
-			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
+	/** This is just a typedef now */
+	typedef BetweenFactor<Pose3, Pose3Config> Pose3Factor;
 
+} /// namespace gtsam

cpp/testPose3Factor.cpp

@@ -11,8 +11,12 @@
 using namespace std;
 using namespace gtsam;
 
+/* ************************************************************************* */
 TEST( Pose3Factor, constructor )
 {
+	Pose3 measured;
+	Matrix measurement_covariance;
+	Pose3Factor("x1", "x2", measured, measurement_covariance);
 }
 
 /* ************************************************************************* */