good progress - still need to work on TriangulatePoint3

gtsam/geometry/PinholeCamera.h

@@ -318,6 +318,11 @@ public:
     return K_.K() * P;
   }
 
+  /// for Nonlinear Triangulation
+  Vector defaultErrorWhenTriangulatingBehindCamera() const {
+    return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_.fx());;
+  }
+
 private:
 
   /** Serialization function */

gtsam/geometry/PinholePose.h

@@ -422,6 +422,10 @@ public:
     return K_->K() * P;
   }
 
+  /// for Nonlinear Triangulation
+  Vector defaultErrorWhenTriangulatingBehindCamera() const {
+    return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_->fx());;
+  }
   /// @}
 
 private:

gtsam/geometry/SphericalCamera.h

@@ -215,6 +215,11 @@ class GTSAM_EXPORT SphericalCamera {
     return Matrix34(pose_.inverse().matrix().block(0, 0, 3, 4));
   }
 
+  /// for Nonlinear Triangulation
+  Vector defaultErrorWhenTriangulatingBehindCamera() const {
+    return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(0.0);
+  }
+
  private:
 
   /** Serialization function */

gtsam/geometry/StereoCamera.h

@@ -170,6 +170,11 @@ public:
       OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 =
           boost::none) const;
 
+  /// for Nonlinear Triangulation
+  Vector defaultErrorWhenTriangulatingBehindCamera() const {
+    return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * K_->fx());;
+  }
+
   /// @}
 
 private:

gtsam/geometry/tests/testTriangulation.cpp

@@ -456,36 +456,17 @@ TEST( triangulation, twoPoses_sphericalCamera) {
 
   double rank_tol = 1e-9;
 
-  // construct projection matrices from poses & calibration
+  // 1. Test linear triangulation via DLT
   std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices =
       getCameraProjectionMatrices<SphericalCamera>(cameras);
   Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
   EXPECT(assert_equal(landmark, point, 1e-7));
 
-  static const boost::shared_ptr<Cal3_S2> canCal = //
+  // 2. Test nonlinear triangulation
-      boost::make_shared<Cal3_S2>(1, 1, 0, 0, 0);
+  point = triangulateNonlinear<SphericalCamera>(cameras, measurements, point);
-  PinholeCamera<Cal3_S2> canCam1(pose1, *canCal);
+  EXPECT(assert_equal(landmark, point, 1e-7));
-  PinholeCamera<Cal3_S2> canCam2(pose2, *canCal);
-  std::cout << "canCam1.project(landmark);" << canCam1.project(landmark) << std::endl;
-  std::cout << "canCam2.project(landmark);" << canCam2.project(landmark) << std::endl;
 
-  CameraSet< PinholeCamera<Cal3_S2> > canCameras;
+  // 3. Test simple DLT, now within triangulatePoint3
-  canCameras.push_back(canCam1);
-  canCameras.push_back(canCam2);
-
-  Point2Vector can_measurements;
-  can_measurements.push_back(canCam1.project(landmark));
-  can_measurements.push_back(canCam2.project(landmark));
-
-  // construct projection matrices from poses & calibration
-  std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> can_projection_matrices =
-      getCameraProjectionMatrices< PinholeCamera<Cal3_S2> >(canCameras);
-  std::cout <<"can_projection_matrices \n" << can_projection_matrices.at(0) <<std::endl;
-  std::cout <<"can_projection_matrices \n" << can_projection_matrices.at(1) <<std::endl;
-
-  Point3 can_point = triangulateDLT(can_projection_matrices, can_measurements, rank_tol);
-  EXPECT(assert_equal(landmark, can_point, 1e-7));
-  // 1. Test simple DLT, perfect in no noise situation
 //  bool optimize = false;
 //  boost::optional<Point3> actual1 = //
 //      triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol, optimize);

gtsam/geometry/triangulation.cpp

@@ -69,16 +69,13 @@ Point3 triangulateDLT(
     const std::vector<Unit3>& unit3measurements, double rank_tol) {
 
   Point2Vector measurements;
-  size_t i=0;
   for (const Unit3& pu : unit3measurements) {  // get canonical pixel projection from Unit3
     Point3 p = pu.point3();
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
     if (p.z() <= 0) // TODO: maybe we should handle this more delicately
       throw(TriangulationCheiralityException());
+#endif
     measurements.push_back(Point2(p.x() / p.z(), p.y() / p.z()));
-    std::cout << "px" << Point2(pu.point3().x() / pu.point3().z(),
-                                   pu.point3().y() / pu.point3().z())<< std::endl;
-    std::cout << "projection_matrices \n "<< projection_matrices.at(i)<< std::endl;
-    i++;
   }
   return triangulateDLT(projection_matrices, measurements, rank_tol);
 }

gtsam/slam/TriangulationFactor.h

@@ -129,7 +129,7 @@ public:
             << std::endl;
       if (throwCheirality_)
         throw e;
-      return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * camera_.calibration().fx());
+      return camera_.defaultErrorWhenTriangulatingBehindCamera();
     }
   }