addConstraint

cpp/NonlinearEquality.h

@@ -12,6 +12,17 @@
 
 namespace gtsam {
 
+	/**
+	 * Template default compare function that assumes Congig.get yields a testable T
+	 */
+	template<class Config, class T>
+	bool compare(const std::string& key, const Config& feasible, const Config& input) {
+		const T& t1 = feasible.get(key);
+		const T& t2 = input.get(key);
+		return t1.equals(t2);
+	}
+
+
 	/**
 	 * An equality factor that forces either one variable to a constant,
 	 * or a set of variables to be equal to each other.
@@ -41,19 +52,28 @@ public:
 		 * @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);
+		bool (*compare_)(const std::string& key, const Config& feasible,
+				const Config& input);
 
 		/** Constructor */
-	NonlinearEquality(const std::string& key,
+		NonlinearEquality(const std::string& key, const Config& feasible,
-			const Config& feasible,
+				size_t dim, bool(*compare)(const std::string& key,
-			size_t dim,
+						const Config& feasible, const Config& input)) :
-			bool (*compare)(const std::string& key,
+			key_(key), dim_(dim), feasible_(feasible), compare_(compare) {
-							const Config& feasible,
-							const Config& input))
-	: key_(key), dim_(dim), feasible_(feasible), compare_(compare) {
 
 		}
 
+		/**
+		 * 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");
@@ -62,7 +82,8 @@ public:
 
 		/** 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);
+			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
@@ -80,13 +101,15 @@ public:
 		/** 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_ + "!");
+				throw std::invalid_argument("Linearization point not feasible for "
+						+ key_ + "!");
 			} else {
-			GaussianFactor::shared_ptr ret(new GaussianFactor(key_, eye(dim_), zero(dim_), 0.0));
+				GaussianFactor::shared_ptr ret(new GaussianFactor(key_, eye(dim_),
+						zero(dim_), 0.0));
 				return ret;
 			}
 		}
-};
+	}; // NonlinearEquality
 
-}
+} // namespace gtsam
 

cpp/Pose2Graph.h

@@ -8,6 +8,7 @@
 #pragma once
 
 #include "NonlinearFactorGraph.h"
+#include "NonlinearEquality.h"
 #include "Pose2Factor.h"
 #include "Pose2Config.h"
 
@@ -21,7 +22,8 @@ class Pose2Graph : public gtsam::NonlinearFactorGraph<Pose2Config> {
 	public:
 
 		/** default constructor is empty graph */
-  Pose2Graph() {}
+		Pose2Graph() {
+		}
 
 		/**
 		 * equals
@@ -36,11 +38,21 @@ public:
 			push_back(sharedFactor(new Pose2Factor(key1, key2, measured, covariance)));
 		}
 
+		/**
+		 *  Add an equality constraint on a pose
+		 *  @param key of pose
+		 *  @param pose which pose to constrain it to
+		 */
+		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) {}
+		void serialize(Archive & ar, const unsigned int version) {
+		}
 	};
 
 } // namespace gtsam

cpp/Pose3Graph.h

@@ -8,6 +8,7 @@
 #pragma once
 
 #include "NonlinearFactorGraph.h"
+#include "NonlinearEquality.h"
 #include "Pose3Factor.h"
 #include "Pose3Config.h"
 
@@ -21,7 +22,8 @@ class Pose3Graph : public gtsam::NonlinearFactorGraph<Pose3Config> {
 	public:
 
 		/** default constructor is empty graph */
-  Pose3Graph() {}
+		Pose3Graph() {
+		}
 
 		/**
 		 * equals
@@ -36,11 +38,21 @@ public:
 			push_back(sharedFactor(new Pose3Factor(key1, key2, measured, covariance)));
 		}
 
+		/**
+		 *  Add an equality constraint on a pose
+		 *  @param key of pose
+		 *  @param pose which pose to constrain it to
+		 */
+		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) {}
+		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);