addConstraint

cpp/NonlinearEquality.h

@@ -12,81 +12,104 @@
 
 namespace gtsam {
 
-/**
+	/**
- * An equality factor that forces either one variable to a constant,
+	 * Template default compare function that assumes Congig.get yields a testable T
- * or a set of variables to be equal to each other.
+	 */
- * Throws an error at linearization if the constraints are not met.
+	template<class Config, class T>
- */
+	bool compare(const std::string& key, const Config& feasible, const Config& input) {
-template<class Config>
+		const T& t1 = feasible.get(key);
-class NonlinearEquality : public NonlinearFactor<Config> {
+		const T& t2 = input.get(key);
-private:
+		return t1.equals(t2);
+	}
 
-	// node to constrain
-	std::string key_;
-
-	// config containing the necessary feasible point
-	Config feasible_;
-
-	// dimension of the variable
-	size_t dim_;
-
-public:
 
 	/**
-	 * Function that compares a value from a config with
+	 * An equality factor that forces either one variable to a constant,
-	 * another to determine whether a linearization point is
+	 * or a set of variables to be equal to each other.
-	 * a feasible point.
+	 * Throws an error at linearization if the constraints are not met.
-	 * @param key is the identifier for the key
-	 * @param feasible is the value which is constrained
-	 * @param input is the config to be tested for feasibility
-	 * @return true if the linearization point is feasible
 	 */
-	bool (*compare_)(const std::string& key, const Config& feasible, const Config& input);
+	template<class Config>
+	class NonlinearEquality: public NonlinearFactor<Config> {
+	private:
 
-	/** Constructor */
+		// node to constrain
-	NonlinearEquality(const std::string& key,
+		std::string key_;
-			const Config& feasible,
-			size_t dim,
-			bool (*compare)(const std::string& key,
-							const Config& feasible,
-							const Config& input))
-	: key_(key), dim_(dim), feasible_(feasible), compare_(compare) {
 
-	}
+		// config containing the necessary feasible point
+		Config feasible_;
 
-	void print(const std::string& s = "") const {
+		// dimension of the variable
-		std::cout << "Constraint: " << s << " on [" << key_ << "]\n";
+		size_t dim_;
-		feasible_.print("Feasible Point");
-		std::cout << "Variable Dimension: " << dim_ << std::endl;
-	}
 
-	/** Check if two factors are equal */
+	public:
-	bool equals(const Factor<Config>& f, double tol=1e-9) const {
-		const NonlinearEquality<Config>* p = dynamic_cast<const NonlinearEquality<Config>*> (&f);
-		if (p == NULL) return false;
-		if (key_ != p->key_) return false;
-		if (!compare_(key_, feasible_, p->feasible_)) return false; // only check the relevant value
-		return dim_ == p->dim_;
-	}
 
-	/** error function */
+		/**
-	inline Vector error_vector(const Config& c) const {
+		 * Function that compares a value from a config with
-		if (!compare_(key_, feasible_, c))
+		 * another to determine whether a linearization point is
-			return repeat(dim_, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
+		 * a feasible point.
-		else
+		 * @param key is the identifier for the key
-			return zero(dim_); // set error to zero if equal
+		 * @param feasible is the value which is constrained
-	}
+		 * @param input is the config to be tested for feasibility
+		 * @return true if the linearization point is feasible
+		 */
+		bool (*compare_)(const std::string& key, const Config& feasible,
+				const Config& input);
+
+		/** Constructor */
+		NonlinearEquality(const std::string& key, const Config& feasible,
+				size_t dim, bool(*compare)(const std::string& key,
+						const Config& feasible, const Config& input)) :
+			key_(key), dim_(dim), feasible_(feasible), compare_(compare) {
 
-	/** linearize a nonlinear constraint into a linear constraint */
-	boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
-		if (!compare_(key_, feasible_, c)) {
-			throw std::invalid_argument("Linearization point not feasible for " + key_ + "!");
-		} else {
-			GaussianFactor::shared_ptr ret(new GaussianFactor(key_, eye(dim_), zero(dim_), 0.0));
-			return ret;
 		}
-	}
-};
 
-}
+		/**
+		 * Constructor with default compare
+		 * Needs class T to have dim()
+		 * and Config to have insert and get
+		 */
+		template <class T>
+		NonlinearEquality(const std::string& key, const T& feasible) :
+			key_(key), dim_(dim(feasible)), compare_(compare<Config,T>) {
+			feasible_.insert(key,feasible);
+		}
+
+		void print(const std::string& s = "") const {
+			std::cout << "Constraint: " << s << " on [" << key_ << "]\n";
+			feasible_.print("Feasible Point");
+			std::cout << "Variable Dimension: " << dim_ << std::endl;
+		}
+
+		/** Check if two factors are equal */
+		bool equals(const Factor<Config>& f, double tol = 1e-9) const {
+			const NonlinearEquality<Config>* p =
+					dynamic_cast<const NonlinearEquality<Config>*> (&f);
+			if (p == NULL) return false;
+			if (key_ != p->key_) return false;
+			if (!compare_(key_, feasible_, p->feasible_)) return false; // only check the relevant value
+			return dim_ == p->dim_;
+		}
+
+		/** error function */
+		inline Vector error_vector(const Config& c) const {
+			if (!compare_(key_, feasible_, c))
+				return repeat(dim_, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
+			else
+				return zero(dim_); // set error to zero if equal
+		}
+
+		/** linearize a nonlinear constraint into a linear constraint */
+		boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
+			if (!compare_(key_, feasible_, c)) {
+				throw std::invalid_argument("Linearization point not feasible for "
+						+ key_ + "!");
+			} else {
+				GaussianFactor::shared_ptr ret(new GaussianFactor(key_, eye(dim_),
+						zero(dim_), 0.0));
+				return ret;
+			}
+		}
+	}; // NonlinearEquality
+
+} // namespace gtsam
 

cpp/Pose2Graph.h

@@ -8,39 +8,51 @@
 #pragma once
 
 #include "NonlinearFactorGraph.h"
+#include "NonlinearEquality.h"
 #include "Pose2Factor.h"
 #include "Pose2Config.h"
 
-namespace gtsam{
+namespace gtsam {
 
-/**
+	/**
- * Non-linear factor graph for visual SLAM
+	 * Non-linear factor graph for visual SLAM
- */
+	 */
-class Pose2Graph : public gtsam::NonlinearFactorGraph<Pose2Config> {
+	class Pose2Graph: public gtsam::NonlinearFactorGraph<Pose2Config> {
 
-public:
+	public:
 
-  /** default constructor is empty graph */
+		/** default constructor is empty graph */
-  Pose2Graph() {}
+		Pose2Graph() {
+		}
 
-  /**
+		/**
-   * equals
+		 * equals
-   */
+		 */
-  bool equals(const Pose2Graph& p, double tol=1e-9) const;
+		bool equals(const Pose2Graph& p, double tol = 1e-9) const;
 
-  /**
+		/**
-   * Add a factor without having to do shared factor dance
+		 * Add a factor without having to do shared factor dance
-   */
+		 */
-  inline void add(const std::string& key1, const std::string& key2,
+		inline void add(const std::string& key1, const std::string& key2,
-			const Pose2& measured, const Matrix& covariance) {
+				const Pose2& measured, const Matrix& covariance) {
-    push_back(sharedFactor(new Pose2Factor(key1, key2, measured, covariance)));
+			push_back(sharedFactor(new Pose2Factor(key1, key2, measured, covariance)));
-  }
+		}
 
-private:
+		/**
-	/** Serialization function */
+		 *  Add an equality constraint on a pose
-	friend class boost::serialization::access;
+		 *  @param key of pose
-	template<class Archive>
+		 *  @param pose which pose to constrain it to
-	void serialize(Archive & ar, const unsigned int version) {}
+		 */
-};
+		inline void addConstraint(const std::string& key, const Pose2& pose =	Pose2()) {
+			push_back(sharedFactor(new NonlinearEquality<Pose2Config>(key, pose)));
+		}
+
+	private:
+		/** Serialization function */
+		friend class boost::serialization::access;
+		template<class Archive>
+		void serialize(Archive & ar, const unsigned int version) {
+		}
+	};
 
 } // namespace gtsam

cpp/Pose3Graph.h

@@ -8,39 +8,51 @@
 #pragma once
 
 #include "NonlinearFactorGraph.h"
+#include "NonlinearEquality.h"
 #include "Pose3Factor.h"
 #include "Pose3Config.h"
 
-namespace gtsam{
+namespace gtsam {
 
-/**
+	/**
- * Non-linear factor graph for visual SLAM
+	 * Non-linear factor graph for visual SLAM
- */
+	 */
-class Pose3Graph : public gtsam::NonlinearFactorGraph<Pose3Config> {
+	class Pose3Graph: public gtsam::NonlinearFactorGraph<Pose3Config> {
 
-public:
+	public:
 
-  /** default constructor is empty graph */
+		/** default constructor is empty graph */
-  Pose3Graph() {}
+		Pose3Graph() {
+		}
 
-  /**
+		/**
-   * equals
+		 * equals
-   */
+		 */
-  bool equals(const Pose3Graph& p, double tol=1e-9) const;
+		bool equals(const Pose3Graph& p, double tol = 1e-9) const;
 
-  /**
+		/**
-   * Add a factor without having to do shared factor dance
+		 * Add a factor without having to do shared factor dance
-   */
+		 */
-  inline void add(const std::string& key1, const std::string& key2,
+		inline void add(const std::string& key1, const std::string& key2,
-			const Pose3& measured, const Matrix& covariance) {
+				const Pose3& measured, const Matrix& covariance) {
-    push_back(sharedFactor(new Pose3Factor(key1, key2, measured, covariance)));
+			push_back(sharedFactor(new Pose3Factor(key1, key2, measured, covariance)));
-  }
+		}
 
-private:
+		/**
-	/** Serialization function */
+		 *  Add an equality constraint on a pose
-	friend class boost::serialization::access;
+		 *  @param key of pose
-	template<class Archive>
+		 *  @param pose which pose to constrain it to
-	void serialize(Archive & ar, const unsigned int version) {}
+		 */
-};
+		inline void addConstraint(const std::string& key, const Pose3& pose =	Pose3()) {
+			push_back(sharedFactor(new NonlinearEquality<Pose3Config> (key, pose)));
+		}
+
+	private:
+		/** Serialization function */
+		friend class boost::serialization::access;
+		template<class Archive>
+		void serialize(Archive & ar, const unsigned int version) {
+		}
+	};
 
 } // namespace gtsam

cpp/testPose2Graph.cpp

@@ -11,7 +11,6 @@ using namespace boost::assign;
 #include <CppUnitLite/TestHarness.h>
 
 #include "NonlinearOptimizer-inl.h"
-#include "NonlinearEquality.h"
 #include "Ordering.h"
 #include "Pose2Config.h"
 #include "Pose2Graph.h"
@@ -81,22 +80,12 @@ TEST( Pose2Graph, linerization )
 	CHECK(assert_equal(lfg_expected, lfg_linearized));
 }
 
-/* ************************************************************************* */
-bool poseCompare(const std::string& key,
-    const gtsam::Pose2Config& feasible,
-    const gtsam::Pose2Config& input) {
-  return feasible.get(key).equals(input.get(key));
-}
-
 /* ************************************************************************* */
 TEST(Pose2Graph, optimize) {
 
 	// create a Pose graph with one equality constraint and one measurement
   Pose2Graph fg;
-  Pose2Config feasible;
+  fg.addConstraint("p0", Pose2(0,0,0));
-  feasible.insert("p0", Pose2(0,0,0));
-  fg.push_back(Pose2Graph::sharedFactor(
-      new NonlinearEquality<Pose2Config>("p0", feasible, dim(Pose2()), poseCompare)));
   fg.add("p0", "p1", Pose2(1,2,M_PI_2), covariance);
 
   // Create initial config
@@ -118,7 +107,6 @@ TEST(Pose2Graph, optimize) {
   expected.insert("p0", Pose2(0,0,0));
   expected.insert("p1", Pose2(1,2,M_PI_2));
   CHECK(assert_equal(expected, *optimizer.config()));
-
 }
 
 /* ************************************************************************* */
@@ -131,10 +119,7 @@ TEST(Pose2Graph, optimizeCircle) {
 
 	// create a Pose graph with one equality constraint and one measurement
   Pose2Graph fg;
-  Pose2Config feasible;
+  fg.addConstraint("p0", p0);
-  feasible.insert("p0", p0);
-  fg.push_back(Pose2Graph::sharedFactor(
-      new NonlinearEquality<Pose2Config>("p0", feasible, dim(Pose2()), poseCompare)));
   Pose2 delta = between(p0,p1);
   fg.add("p0", "p1", delta, covariance);
   fg.add("p1", "p2", delta, covariance);

cpp/testPose3Graph.cpp

@@ -23,13 +23,6 @@ using namespace gtsam;
 // common measurement covariance
 static Matrix covariance = eye(6);
 
-/* ************************************************************************* */
-bool poseCompare(const std::string& key,
-    const gtsam::Pose3Config& feasible,
-    const gtsam::Pose3Config& input) {
-  return feasible.get(key).equals(input.get(key));
-}
-
 /* ************************************************************************* */
 // test optimization with 6 poses arranged in a hexagon and a loop closure
 TEST(Pose3Graph, optimizeCircle) {
@@ -40,10 +33,7 @@ TEST(Pose3Graph, optimizeCircle) {
 
 	// create a Pose graph with one equality constraint and one measurement
   Pose3Graph fg;
-  Pose3Config feasible;
+  fg.addConstraint("p0", p0);
-  feasible.insert("p0", p0);
-  fg.push_back(Pose3Graph::sharedFactor(
-      new NonlinearEquality<Pose3Config>("p0", feasible, dim(Pose3()), poseCompare)));
   Pose3 delta = between(p0,p1);
   fg.add("p0", "p1", delta, covariance);
   fg.add("p1", "p2", delta, covariance);