From e0869719fa5b3ce867c2cfad1a5ec040ea18beb7 Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Tue, 2 Aug 2016 22:56:48 -0400
Subject: [PATCH] now SmartFactorBase doesn't know about stereoPoint2 (removed
 also header). The functionality to check if the right pixel is missing has
 been moved to SmartStereoProjectionFactor, removing the casting.

---
 gtsam/slam/SmartFactorBase.h                  | 29 +++++--------------
 .../slam/SmartStereoProjectionFactor.h        | 26 +++++++++++++++++
 2 files changed, 34 insertions(+), 21 deletions(-)

diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index 1d33d16a8..57a53daa3 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/gtsam/slam/SmartFactorBase.h
@@ -27,7 +27,6 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/linear/RegularHessianFactor.h>
 #include <gtsam/geometry/CameraSet.h>
-#include <gtsam/geometry/StereoPoint2.h>
 
 #include <boost/optional.hpp>
 #include <boost/serialization/optional.hpp>
@@ -221,29 +220,17 @@ public:
         Fs->at(i) = Fs->at(i) * J;
       }
     }
-
-    static const int Np = FixedDimension<POINT>::value; // 2 (Unit3) or 3 (Point3)
-
-    // when using stereo cameras, some of the measurements might be missing:
-    for(size_t i=0; i < cameras.size(); i++){
-      const StereoPoint2 * z3 = reinterpret_cast<const StereoPoint2*>(&measured_.at(i));
-      if(ZDim==3 && z3 && std::isnan(z3->uR())) // if it's a stereo point and the right pixel is invalid
-      {
-        if(Fs){ // delete influence of right point on jacobian Fs
-          MatrixZD& Fi = Fs->at(i);
-          for(size_t ii=0; ii<Dim; ii++)
-            Fi(1,ii) = 0.0;
-        }
-        if(E) // delete influence of right point on jacobian E
-          E->block<1, Np>(ZDim * i + 1, 0) = Matrix::Zero(1, Np);
-
-        // set to zero entry from vector ue
-        ue(ZDim * i + 1) = 0.0;
-      }
-    }
+    correctForMissingMeasurements(cameras, ue, Fs, E);
     return ue;
   }
 
+  /**
+   * This corrects the Jacobians for the case in which some pixel measurement is missing (nan)
+   * In practice, this does not do anything in the monocular case, but it is implemented in the stereo version
+   */
+  virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
+		  boost::optional<Matrix&> E = boost::none) const {}
+
   /**
    * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z]
    * Noise model applied
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
index 006ff7bdf..56b5d85de 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
@@ -516,6 +516,32 @@ public:
     }
   }
 
+  /**
+   * This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan)
+   */
+  virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue,
+      boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
+      boost::optional<Matrix&> E = boost::none) const
+  {
+    // when using stereo cameras, some of the measurements might be missing:
+    for(size_t i=0; i < cameras.size(); i++){
+      const StereoPoint2& z = measured_.at(i);
+      if(std::isnan(z.uR())) // if the right pixel is invalid
+      {
+        if(Fs){ // delete influence of right point on jacobian Fs
+          MatrixZD& Fi = Fs->at(i);
+          for(size_t ii=0; ii<Dim; ii++)
+            Fi(1,ii) = 0.0;
+        }
+        if(E) // delete influence of right point on jacobian E
+          E->row(ZDim * i + 1) = Matrix::Zero(1, E->cols());
+
+        // set the corresponding entry of vector ue to zero
+        ue(ZDim * i + 1) = 0.0;
+      }
+    }
+  }
+
   /** return the landmark */
     TriangulationResult point() const {
       return result_;