need to clean up templates and remove 2 redundant lines

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -449,10 +449,10 @@ public:
   /**
    * This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan)
    */
-  template<int D, int ZD> // D: camera dim, ZD: measurement dim
   void correctForMissingMeasurements(const Cameras& cameras, Vector& ue,
-      boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
+                                     boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
-      boost::optional<Matrix&> E = boost::none) const {
+                                     boost::optional<Matrix&> E = boost::none) const override
+  {
     // 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);
@@ -460,28 +460,18 @@ public:
       {
         if(Fs){ // delete influence of right point on jacobian Fs
           MatrixZD& Fi = Fs->at(i);
-          for(size_t ii=0; ii<D; ii++)
+          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(ZD * i + 1) = Matrix::Zero(1, E->cols());
+          E->row(ZDim * i + 1) = Matrix::Zero(1, E->cols());
 
         // set the corresponding entry of vector ue to zero
-        ue(ZD * i + 1) = 0.0;
+        ue(ZDim * i + 1) = 0.0;
       }
     }
   }
 
-  /**
-   * This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan)
-   * This is class implementation that directly uses the measurement and camera size without templates.
-   */
-  void correctForMissingMeasurements(const Cameras& cameras, Vector& ue,
-      boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
-      boost::optional<Matrix&> E = boost::none) const override {
-    correctForMissingMeasurements<Dim,ZDim>(cameras, ue, Fs, E);
-  }
-
   /** return the landmark */
     TriangulationResult point() const {
       return result_;

gtsam_unstable/slam/SmartStereoProjectionFactorPP.h

@@ -152,7 +152,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
   /// Compute F, E only (called below in both vanilla and SVD versions)
   /// Assumes the point has been computed
   /// Note E can be 2m*3 or 2m*2, in case point is degenerate
-  void computeJacobiansWithTriangulatedPoint(
+  void computeJacobiansAndCorrectForMissingMeasurements(
       FBlocks& Fs,
       Matrix& E, Vector& b, const Values& values) const {
     if (!result_) {
@@ -168,23 +168,20 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
         Pose3 body_P_cam = values.at<Pose3>(body_P_cam_keys_.at(i));
         StereoCamera camera(w_P_body.compose(body_P_cam, dPoseCam_dPoseBody, dPoseCam_dPoseExt), K_all_[i]);
         StereoPoint2 reprojectionError = StereoPoint2(camera.project(*result_, dProject_dPoseCam, Ei) - measured_.at(i));
-//        std::cout << "H0 \n" << dPoseCam_dPoseBody << std::endl;
-//        std::cout << "H1 \n" << dProject_dPoseCam << std::endl;
-//        std::cout << "H3 \n" << Ei << std::endl;
-//        std::cout << "H02 \n" << dPoseCam_dPoseExt << std::endl;
         Eigen::Matrix<double, ZDim, Dim> J; // 3 x 12
-//        std::cout << "H1 * H0 \n" << dProject_dPoseCam * dPoseCam_dPoseBody << std::endl;
-//        std::cout << "H1 * H02 \n" << dProject_dPoseCam * dPoseCam_dPoseExt << std::endl;
         J.block<ZDim,6>(0,0) = dProject_dPoseCam * dPoseCam_dPoseBody; // (3x6) * (6x6)
         J.block<ZDim,6>(0,6) = dProject_dPoseCam * dPoseCam_dPoseExt; // (3x6) * (6x6)
-//        std::cout << "J \n" << J << std::endl;
+        if(std::isnan(measured_.at(i).uR())) // if the right pixel is invalid
+        {
+          J.block<1,12>(1,0) = Matrix::Zero(1,12);
+          Ei.block<1,3>(1,0) = Matrix::Zero(1,3);
+          reprojectionError = StereoPoint2(reprojectionError.uL(), 0.0, reprojectionError.v() );
+        }
         Fs.push_back(J);
         size_t row = 3*i;
         b.segment<ZDim>(row) = - reprojectionError.vector();
         E.block<3,3>(row,0) = Ei;
       }
-      // correct for monocular measurements, where the right pixel measurement is nan
-      //Base::CorrectForMissingMeasurements(measured_, cameras, b, Fs, E);
     }
   }
 
@@ -204,11 +201,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
       throw std::runtime_error("SmartStereoProjectionHessianFactor: this->"
           "measured_.size() inconsistent with input");
 
-    std::cout << "triangulate" << std::endl;
     triangulateSafe(cameras(values));
 
     if (!result_) {
-      std::cout << "degenerate" << std::endl;
       // failed: return"empty" Hessian
       for(Matrix& m: Gs)
         m = Matrix::Zero(DimPose,DimPose);
@@ -218,12 +213,11 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
                                                                   Gs, gs, 0.0);
     }
 
-    std::cout << "params_.degeneracyMode" << params_.degeneracyMode <<  std::endl;
     // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
     FBlocks Fs;
     Matrix F, E;
     Vector b;
-    computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
+    computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values);
 
     // Whiten using noise model
     noiseModel_->WhitenSystem(E, b);
@@ -266,16 +260,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
         keyToSlotMap[keys_[k]] = k;
       }
 
-      std::cout << "linearize" << std::endl;
-//      for(size_t i=0; i<nrUniqueKeys;i++){
-//        std::cout <<"key: " << DefaultKeyFormatter(keys_[i]);
-//        std::cout <<"  key slot: " << keyToSlotMap[keys_[i]] << std::endl;
-//      }
-//      for(size_t i=0; i<nrNonuniqueKeys;i++){
-//        std::cout <<"key: " << DefaultKeyFormatter(nonuniqueKeys[i]);
-//        std::cout <<"  key slot: " << keyToSlotMap[nonuniqueKeys[i]] << std::endl;
-//      }
-
       // initialize matrix to zero
       augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, Matrix::Zero(6*nrUniqueKeys+1,6*nrUniqueKeys+1));
 
@@ -309,13 +293,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
         }
       }
       augmentedHessianUniqueKeys.updateDiagonalBlock(nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
-
-      //std::cout << "Matrix \n " << Matrix(augmentedHessianUniqueKeys.selfadjointView()) <<std::endl;
-      //std::cout << "sq norm " << b.squaredNorm() << std::endl;
     }
 
     return boost::make_shared<RegularHessianFactor<DimPose> >(keys_, augmentedHessianUniqueKeys);
-    //std::cout << "Matrix(augmentedHessian.selfadjointView()) \n" << Matrix(augmentedHessian.selfadjointView()) <<std::endl;
   }
 
   /**

gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp

@@ -862,7 +862,7 @@ TEST( SmartStereoProjectionFactorPP, 3poses_optimization_sameExtrinsicKey ) {
   EXPECT(assert_equal(expected, delta, 1e-4));
 }
 
-/* *************************************************************************
+/* *************************************************************************/
 TEST( SmartStereoProjectionFactorPP, monocular_multipleExtrinsicKeys ){
   // make a realistic calibration matrix
   double fov = 60; // degrees
@@ -979,7 +979,7 @@ TEST( SmartStereoProjectionFactorPP, monocular_multipleExtrinsicKeys ){
   tictoc_finishedIteration_();
 
   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
-  EXPECT(assert_equal(sensor_to_body,result.at<Pose3>(body_P_cam_key)));
+  EXPECT(assert_equal(sensor_to_body,result.at<Pose3>(body_P_cam_key), 1e-1));
 }
 
 /* *************************************************************************/