From f2bec78a58281ee52b9c58f03343cac048995179 Mon Sep 17 00:00:00 2001
From: Luca <lcarlone@mit.edu>
Date: Sun, 24 Jul 2016 18:27:20 -0400
Subject: [PATCH] first implementation of smartStereo with possibly left-only
 pixel measurements

---
 gtsam/slam/SmartFactorBase.h                  | 22 +++++++++++++++++++
 .../slam/SmartStereoProjectionFactor.h        |  6 +++--
 2 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index 93d2ff218..fad49a339 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/gtsam/slam/SmartFactorBase.h
@@ -27,6 +27,7 @@
 #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>
@@ -210,6 +211,27 @@ 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 < Fs->size(); i++){
+      if(ZDim == 3){ // it's a stereo point ..
+        Z z3 = measured_.at(i);
+        if(isnan(z3.vector()[1])){ // .. and the right pixel is invalid
+          // delete influence of right point on jacobian Fs
+          std::cout << "unwhitenedError:: isnan(z3->uR()" << z3.vector() << std::endl;
+          MatrixZD& Fi = Fs->at(i);
+          for(size_t ii=0; ii<Dim; ii++)
+            Fi(1,ii) = 0.0;
+
+          // 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; // this should not matter anyway
+        }
+      }
+    }
     return ue;
   }
 
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
index 9d9e65af6..e2b35ad82 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
@@ -257,8 +257,10 @@ public:
       const StereoPoint2 zi = measured_[i];
       monoCameras.push_back(leftCamera_i);
       monoMeasured.push_back(Point2(zi.uL(),zi.v()));
-      monoCameras.push_back(rightCamera_i);
-      monoMeasured.push_back(Point2(zi.uR(),zi.v()));
+      if(!isnan(zi.uR())){ // if right point is valid
+        monoCameras.push_back(rightCamera_i);
+        monoMeasured.push_back(Point2(zi.uR(),zi.v()));
+      }
     }
     if (retriangulate)
       result_ = gtsam::triangulateSafe(monoCameras, monoMeasured,