From c3c5c0d80c802d1aba45f6c353470a1587d3e28d Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sat, 21 Feb 2015 16:39:39 +0100
Subject: [PATCH] Equals

---
 gtsam/slam/RegularImplicitSchurFactor.h | 19 +++++++++++++++----
 1 file changed, 15 insertions(+), 4 deletions(-)

diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h
index 75b2d44ba..95a61dcfc 100644
--- a/gtsam/slam/RegularImplicitSchurFactor.h
+++ b/gtsam/slam/RegularImplicitSchurFactor.h
@@ -96,11 +96,17 @@ public:
 
   /// equals
   bool equals(const GaussianFactor& lf, double tol) const {
-    if (!dynamic_cast<const RegularImplicitSchurFactor*>(&lf))
-      return false;
-    else {
+    const This* f = dynamic_cast<const This*>(&lf);
+    if (!f)
       return false;
+    for (size_t pos = 0; pos < size(); ++pos) {
+      if (keys_[pos] != f->keys_[pos]) return false;
+      if (Fblocks_[pos].first != f->Fblocks_[pos].first) return false;
+      if (!equal_with_abs_tol(Fblocks_[pos].second,f->Fblocks_[pos].second,tol)) return false;
     }
+    return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol)
+        && equal_with_abs_tol(E_, f->E_, tol)
+        && equal_with_abs_tol(b_, f->b_, tol);
   }
 
   /// Degrees of freedom of camera
@@ -460,7 +466,12 @@ public:
 
 
 };
-// RegularImplicitSchurFactor
+// end class RegularImplicitSchurFactor
+
+// traits
+template<size_t D> struct traits<RegularImplicitSchurFactor<D> > : public Testable<
+    RegularImplicitSchurFactor<D> > {
+};
 
 }