From 369e69b0d2acb1750b285608c49a2fd25d1328fc Mon Sep 17 00:00:00 2001
From: Chris Beall <chrisbeall@gmail.com>
Date: Fri, 9 Aug 2013 15:28:57 +0000
Subject: [PATCH] actually set everything to zero to turn factor off during
 linearization

---
 gtsam_unstable/slam/SmartProjectionFactor.h | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h
index 9faf32cd1..f043c6873 100644
--- a/gtsam_unstable/slam/SmartProjectionFactor.h
+++ b/gtsam_unstable/slam/SmartProjectionFactor.h
@@ -161,8 +161,7 @@ namespace gtsam {
       // We triangulate the 3D position of the landmark
       boost::optional<Point3> point = triangulatePoint3(cameraPoses, measured_, *K_);
 
-      if (!point)
-        return HessianFactor::shared_ptr(new HessianFactor());
+
 
       if (debug) {
         std::cout << "point " << *point << std::endl;
@@ -178,6 +177,13 @@ namespace gtsam {
         js += ordering[k];
       }
 
+      // point is behind one of the cameras, turn factor off by setting everything to 0
+      if (!point) {
+        BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6,6);
+        BOOST_FOREACH(Vector& v, gs) v = zero(6);
+        return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f));
+      }
+
       // For debug only
       std::vector<Matrix> Gs1;
       std::vector<Vector> gs1;