first implementation of smartStereo with possibly left-only pixel measurements

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;
   }
 

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -257,9 +257,11 @@ public:
       const StereoPoint2 zi = measured_[i];
       monoCameras.push_back(leftCamera_i);
       monoMeasured.push_back(Point2(zi.uL(),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,
           params_.triangulation);