NonlinearEquality factor is now working for generic configs. Currently, the feasible point must be specified in the form of a full config with the point in it, but this avoid needing to increase the requirements on the configs.

.cproject

@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<?xml version="1.0" encoding="UTF-8"?>
 <?fileVersion 4.0.0?>
 
 <cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
@@ -298,20 +298,6 @@
 </storageModule>
 <storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
 <buildTargets>
-<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-<buildCommand>make</buildCommand>
-<buildTarget>install</buildTarget>
-<stopOnError>true</stopOnError>
-<useDefaultCommand>true</useDefaultCommand>
-<runAllBuilders>true</runAllBuilders>
-</target>
-<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-<buildCommand>make</buildCommand>
-<buildTarget>check</buildTarget>
-<stopOnError>true</stopOnError>
-<useDefaultCommand>true</useDefaultCommand>
-<runAllBuilders>true</runAllBuilders>
-</target>
 <target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
 <buildArguments>-k</buildArguments>
@@ -322,6 +308,7 @@
 </target>
 <target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testSimpleCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -337,7 +324,6 @@
 </target>
 <target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testVSLAMFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -345,21 +331,22 @@
 </target>
 <target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testCalibratedCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
-<target name="testGaussianConditional.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
+<buildTarget>testConditionalGaussian.run</buildTarget>
-<buildTarget>testGaussianConditional.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
 <target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testPose2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -367,6 +354,7 @@
 </target>
 <target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testRot3.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -374,29 +362,29 @@
 </target>
 <target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testNonlinearOptimizer.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
-<target name="testGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<target name="testLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildTarget>testGaussianFactor.run</buildTarget>
+<buildArguments></buildArguments>
+<buildTarget>testLinearFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
-<target name="testGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<target name="testLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildTarget>testGaussianFactorGraph.run</buildTarget>
+<buildArguments></buildArguments>
+<buildTarget>testLinearFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
 <target name="testNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testNonlinearFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -404,6 +392,7 @@
 </target>
 <target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testPose3.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -411,7 +400,6 @@
 </target>
 <target name="testVectorConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testVectorConfig.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -419,7 +407,6 @@
 </target>
 <target name="testPoint2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testPoint2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -427,27 +414,31 @@
 </target>
 <target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testNonlinearFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
-<target name="timeGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<target name="timeLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildTarget>timeGaussianFactor.run</buildTarget>
+<buildArguments></buildArguments>
+<buildTarget>timeLinearFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
-<target name="timeGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<target name="timeLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildTarget>timeGaussianFactorGraph.run</buildTarget>
+<buildArguments></buildArguments>
+<buildTarget>timeLinearFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
 <target name="testGaussianBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testGaussianBayesNet.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -455,7 +446,6 @@
 </target>
 <target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testBayesTree.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -463,6 +453,7 @@
 </target>
 <target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testSymbolicBayesNet.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -470,7 +461,6 @@
 </target>
 <target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testSymbolicFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -478,6 +468,7 @@
 </target>
 <target name="testVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testVector.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -485,6 +476,7 @@
 </target>
 <target name="testMatrix.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testMatrix.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -492,23 +484,29 @@
 </target>
 <target name="testInference.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testInference.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
-<target name="testVSLAMFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<target name="testNonlinearEquality.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
 <buildArguments/>
-<buildTarget>testVSLAMFactorGraph.run</buildTarget>
+<buildTarget>testNonlinearEquality.run</buildTarget>
+<stopOnError>true</stopOnError>
+<useDefaultCommand>true</useDefaultCommand>
+<runAllBuilders>true</runAllBuilders>
+</target>
+<target name="testVSLAMGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
+<buildTarget>testVSLAMGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
 <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>install</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -516,7 +514,6 @@
 </target>
 <target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>clean</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -524,7 +521,22 @@
 </target>
 <target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
+<buildTarget>check</buildTarget>
+<stopOnError>true</stopOnError>
+<useDefaultCommand>true</useDefaultCommand>
+<runAllBuilders>true</runAllBuilders>
+</target>
+<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
+<buildTarget>install</buildTarget>
+<stopOnError>true</stopOnError>
+<useDefaultCommand>true</useDefaultCommand>
+<runAllBuilders>true</runAllBuilders>
+</target>
+<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>

cpp/Makefile.am

@@ -111,8 +111,8 @@ timeGaussianFactorGraph: LDFLAGS += smallExample.o -L.libs -lgtsam -L../CppUnitL
 # Nonlinear inference 
 headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
 headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h 
-sources += NonlinearFactor.cpp
+sources += NonlinearFactor.cpp 
-sources += NonlinearEquality.cpp
+sources += NonlinearEquality.h
 check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality
 testNonlinearFactor_SOURCES      = $(example) testNonlinearFactor.cpp
 testNonlinearFactor_LDADD        = libgtsam.la

cpp/NonlinearEquality.cpp

@@ -1,9 +0,0 @@
-/*
- * @file NonlinearEquality.cpp
- * @brief
- * @author alexgc
- */
-
-#include "NonlinearEquality.h"
-
-

cpp/NonlinearEquality.h

@@ -6,6 +6,7 @@
 
 #pragma once
 
+#include <iostream>
 #include "NonlinearFactor.h"
 
 namespace gtsam {
@@ -16,10 +17,74 @@ namespace gtsam {
  * Throws an error at linearization if the constraints are not met.
  */
 template<class Config>
-class NonlinearEquality : public NonlinearFactor<Config>{
+class NonlinearEquality : public NonlinearFactor<Config> {
+private:
+
+	// node to constrain
+	std::string key_;
+
+	// config containing the necessary feasible point
+	Config feasible_;
+
+	// dimension of the variable
+	size_t dim_;
+
 public:
-	NonlinearEquality();
+
-	virtual ~NonlinearEquality();
+	/**
+	 * Function that compares a value from a config with
+	 * another to determine whether a linearization point is
+	 * a feasible point.
+	 * @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);
+
+	/** 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) {
+
+	}
+
+	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 VectorConfig& c) const {
+		if (!compare_(key_, feasible_, c))
+			return repeat(dim_, 1.0/0.0); // 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 VectorConfig& 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;
+		}
+	}
 };
 
 }

cpp/testNonlinearEquality.cpp

@@ -3,14 +3,100 @@
  * @author Alex Cunningham
  */
 
-
 #include <CppUnitLite/TestHarness.h>
+#include "VectorConfig.h"
 #include "NonlinearEquality.h"
 
-TEST ( NonlinearEquality, construction ) {
+using namespace std;
+using namespace gtsam;
+
+typedef boost::shared_ptr<NonlinearEquality<VectorConfig> > shared_nle;
+
+bool vector_compare(const std::string& key,
+					const VectorConfig& feasible,
+					const VectorConfig& input) {
+	Vector feas, lin;
+	feas = feasible[key];
+	lin = input[key];
+	return equal_with_abs_tol(lin, feas, 1e-5);
+}
+
+TEST ( NonlinearEquality, linearization ) {
+	string key = "x";
+	Vector value = Vector_(2, 1.0, 2.0);
+	VectorConfig feasible, linearize;
+	feasible.insert(key, value);
+	linearize.insert(key, value);
+
+	// create a nonlinear equality constraint
+	shared_nle nle(new NonlinearEquality<VectorConfig>(key, feasible, 2, *vector_compare));
+
+	// check linearize
+	GaussianFactor expLF(key, eye(2), zero(2), 0.0);
+	GaussianFactor::shared_ptr actualLF = nle->linearize(linearize);
+	CHECK(assert_equal(*actualLF, expLF));
+}
+
+TEST ( NonlinearEquality, linearization_fail ) {
+	string key = "x";
+	Vector value = Vector_(2, 1.0, 2.0);
+	Vector wrong = Vector_(2, 3.0, 4.0);
+	VectorConfig feasible, bad_linearize;
+	feasible.insert(key, value);
+	bad_linearize.insert(key, wrong);
+
+	// create a nonlinear equality constraint
+	shared_nle nle(new NonlinearEquality<VectorConfig>(key, feasible, 2, *vector_compare));
+
+	// check linearize to ensure that it fails for bad linearization points
+	try {
+		GaussianFactor::shared_ptr actualLF = nle->linearize(bad_linearize);
+		CHECK(false);
+	} catch (std::invalid_argument) {
+		CHECK(true);
+	}
+}
+
+TEST ( NonlinearEquality, error ) {
+	string key = "x";
+	Vector value = Vector_(2, 1.0, 2.0);
+	Vector wrong = Vector_(2, 3.0, 4.0);
+	VectorConfig feasible, bad_linearize;
+	feasible.insert(key, value);
+	bad_linearize.insert(key, wrong);
+
+	// create a nonlinear equality constraint
+	shared_nle nle(new NonlinearEquality<VectorConfig>(key, feasible, 2, *vector_compare));
+
+	// check error function outputs
+	Vector actual = nle->error_vector(feasible);
+	CHECK(assert_equal(actual, zero(2)));
+
+	actual = nle->error_vector(bad_linearize);
+	CHECK(assert_equal(actual, repeat(2, 1.0/0.0)));
+}
+
+TEST ( NonlinearEquality, equals ) {
+	string key1 = "x";
+	Vector value1 = Vector_(2, 1.0, 2.0);
+	Vector value2 = Vector_(2, 3.0, 4.0);
+	VectorConfig feasible1, feasible2;
+	feasible1.insert(key1, value1);
+	feasible2.insert(key1, value2);
+
+	// create some constraints to compare
+	shared_nle nle1(new NonlinearEquality<VectorConfig>(key1, feasible1, 2, *vector_compare));
+	shared_nle nle2(new NonlinearEquality<VectorConfig>(key1, feasible1, 2, *vector_compare));
+	shared_nle nle3(new NonlinearEquality<VectorConfig>(key1, feasible2, 2, *vector_compare));
+
+	// verify
+	CHECK(nle1->equals(*nle2)); // basic equality = true
+	CHECK(nle2->equals(*nle1)); // test symmetry of equals()
+	CHECK(!nle1->equals(*nle3)); // test config
 
 }
 
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */