Fixed NonlinearFactor2 equals and added some unit tests for equals

.cproject

@@ -300,6 +300,7 @@
 <buildTargets>
 <target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>install</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -307,6 +308,7 @@
 </target>
 <target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -314,7 +316,6 @@
 </target>
 <target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -322,6 +323,7 @@
 </target>
 <target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testSimpleCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -337,7 +339,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,6 +346,7 @@
 </target>
 <target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testCalibratedCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -352,7 +354,6 @@
 </target>
 <target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testConditionalGaussian.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -360,6 +361,7 @@
 </target>
 <target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testPose2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -375,6 +377,7 @@
 </target>
 <target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testRot3.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -382,7 +385,6 @@
 </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>
@@ -390,6 +392,7 @@
 </target>
 <target name="testLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testLinearFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -397,6 +400,7 @@
 </target>
 <target name="testConstrainedNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testConstrainedNonlinearFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -404,6 +408,7 @@
 </target>
 <target name="testLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testLinearFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -411,7 +416,6 @@
 </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>
@@ -419,6 +423,7 @@
 </target>
 <target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testPose3.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -426,7 +431,6 @@
 </target>
 <target name="testConstrainedLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testConstrainedLinearFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -434,7 +438,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>
@@ -442,15 +445,21 @@
 </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>
 <runAllBuilders>true</runAllBuilders>
 </target>
+<target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments/>
+<buildTarget>testNonlinearFactor.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>
@@ -458,7 +467,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>
@@ -466,7 +474,6 @@
 </target>
 <target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>

cpp/NonlinearFactor.cpp

@@ -19,7 +19,7 @@ NonlinearFactor1::NonlinearFactor1(const Vector& z,
                                    Vector (*h)(const Vector&),                      
                                    const string& key1,                   
                                    Matrix (*H)(const Vector&)) 
-  : NonlinearFactor<VectorConfig>(z, sigma), h_(h), key1_(key1), H_(H)
+  : NonlinearFactor<VectorConfig>(z, sigma), h_(h), key_(key1), H_(H)
 {
   keys_.push_front(key1);
 }
@@ -27,13 +27,16 @@ NonlinearFactor1::NonlinearFactor1(const Vector& z,
 /* ************************************************************************* */
 void NonlinearFactor1::print(const string& s) const {
 	cout << "NonLinearFactor1 " << s << endl;
-	NonlinearFactor<VectorConfig>::print(s);
+  cout << "h  : " << (void*)h_ << endl;
+  cout << "key: " << key_      << endl;
+  cout << "H  : " << (void*)H_ << endl;
+	NonlinearFactor<VectorConfig>::print("parent");
 }
 
 /* ************************************************************************* */
 LinearFactor::shared_ptr NonlinearFactor1::linearize(const VectorConfig& c) const {
 	// get argument 1 from config
-	Vector x1 = c[key1_];
+	Vector x1 = c[key_];
 
 	// Jacobian A = H(x1)/sigma
 	Matrix A = H_(x1) / sigma_;
@@ -41,7 +44,7 @@ LinearFactor::shared_ptr NonlinearFactor1::linearize(const VectorConfig& c) cons
 	// Right-hand-side b = error(c) = (z - h(x1))/sigma
 	Vector b = (z_ - h_(x1)) / sigma_;
 
-	LinearFactor::shared_ptr p(new LinearFactor(key1_, A, b));
+	LinearFactor::shared_ptr p(new LinearFactor(key_, A, b));
 	return p;
 }
 
@@ -53,7 +56,7 @@ bool NonlinearFactor1::equals(const Factor<VectorConfig>& f, double tol) const {
 	if (p == NULL) return false;
 	return NonlinearFactor<VectorConfig>::equals(*p, tol)
 	&& (h_   == p->h_) 
-	&& (key1_== p->key1_) 
+	&& (key_ == p->key_)
 	&& (H_   == p->H_);
 }
 
@@ -75,7 +78,12 @@ NonlinearFactor2::NonlinearFactor2(const Vector& z,
 /* ************************************************************************* */
 void NonlinearFactor2::print(const string& s) const {
 	cout << "NonLinearFactor2 " << s << endl;
-	NonlinearFactor<VectorConfig>::print(s);
+  cout << "h   : " << (void*)h_  << endl;
+  cout << "key1: " << key1_      << endl;
+  cout << "H2  : " << (void*)H2_ << endl;
+  cout << "key2: " << key2_      << endl;
+  cout << "H1  : " << (void*)H1_ << endl;
+	NonlinearFactor<VectorConfig>::print("parent");
 }
 
 /* ************************************************************************* */
@@ -102,9 +110,9 @@ bool NonlinearFactor2::equals(const Factor<VectorConfig>& f, double tol) const {
 	return NonlinearFactor<VectorConfig>::equals(*p, tol)
     && (h_    == p->h_)
     && (key1_ == p->key1_)
-    && (H2_   == p->H1_)
+    && (H1_   == p->H1_)
     && (key2_ == p->key2_)
-    && (H1_   == p->H2_);
+    && (H2_   == p->H2_);
 }
 
 /* ************************************************************************* */

cpp/NonlinearFactor.h

@@ -72,7 +72,7 @@ namespace gtsam {
     }
 
     /** Check if two NonlinearFactor objects are equal */
-    bool equals(const Factor<Config>& f, double tol) const {
+    bool equals(const Factor<Config>& f, double tol=1e-9) const {
     	const NonlinearFactor<Config>* p = dynamic_cast<const NonlinearFactor<Config>*> (&f);
     	if (p == NULL) return false;
       return equal_with_abs_tol(z_,p->z_,tol) && fabs(sigma_ - p->sigma_)<=tol;
@@ -123,7 +123,7 @@ namespace gtsam {
   class NonlinearFactor1 : public NonlinearFactor<VectorConfig> {
   private:
 
-		std::string key1_;
+		std::string key_;
 
   public:
 
@@ -144,7 +144,7 @@ namespace gtsam {
 
     /** error function */
     inline Vector error_vector(const VectorConfig& c) const {
-      return z_ - h_(c[key1_]);
+      return z_ - h_(c[key_]);
     }
 
     /** linearize a non-linearFactor1 to get a linearFactor1 */

cpp/smallExample.cpp

@@ -6,9 +6,6 @@
  * @author  Frank dellaert
  */
 
-
-
-/*STL/C++*/
 #include <iostream>
 #include <string>
 

cpp/testNonlinearFactor.cpp

@@ -13,12 +13,46 @@
 
 #include "Matrix.h"
 #include "smallExample.h"
+#include "Simulated2DMeasurement.h"
 
 using namespace std;
 using namespace gtsam;
 
 typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared_nlf;
 
+/* ************************************************************************* */
+TEST( NonLinearFactor, equals )
+{
+	double sigma = 1.0;
+
+	// create two nonlinear2 factors
+	Vector z3(2); z3(0) = 0. ; z3(1) = -1.;
+	Simulated2DMeasurement f0(z3, sigma, "x1", "l1");
+
+	// measurement between x2 and l1
+	Vector z4(2); z4(0)= -1.5 ; z4(1) = -1.;
+	Simulated2DMeasurement f1(z4, sigma, "x2", "l1");
+
+	CHECK(assert_equal(f0,f0));
+	CHECK(f0.equals(f0));
+	CHECK(!f0.equals(f1));
+	CHECK(!f1.equals(f0));
+}
+
+/* ************************************************************************* */
+TEST( NonLinearFactor, equals2 )
+{
+  // create a non linear factor graph
+  ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
+
+  // get two factors
+  shared_nlf f0 = fg[0], f1 = fg[1];
+
+  CHECK(f0->equals(*f0));
+  CHECK(!f0->equals(*f1));
+  CHECK(!f1->equals(*f0));
+}
+
 /* ************************************************************************* */
 TEST( NonLinearFactor, NonlinearFactor )
 {