unit tests pass

gtsam/geometry/triangulation.cpp

@@ -88,35 +88,6 @@ Vector4 triangulateHomogeneousDLT(
   return v;
 }
 
-Point3 triangulateDLT(const std::vector<Pose3>& poses,
-                      const Point3Vector& homogenousMeasurements,
-                      double rank_tol) {
-  // number of cameras
-  size_t m = poses.size();
-
-  // Allocate DLT matrix
-  Matrix A = Matrix::Zero(m * 2, 4);
-
-  for (size_t i = 0; i < m; i++) {
-    size_t row = i * 2;
-    const Matrix34 projection = poses[i].matrix().block(0, 0, 3, 4);
-    const Point3& p = homogenousMeasurements[i];  // to get access to x,y,z of
-                                                  // the bearing vector
-
-    // build system of equations
-    A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0);
-    A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1);
-  }
-  int rank;
-  double error;
-  Vector v;
-  boost::tie(rank, error, v) = DLT(A, rank_tol);
-
-  if (rank < 3) throw(TriangulationUnderconstrainedException());
-
-  return Point3(v.head<3>() / v[3]);
-}
-
 Point3 triangulateLOST(const std::vector<Pose3>& poses,
                        const Point3Vector& calibratedMeasurements,
                        const SharedIsotropic& measurementNoise) {
@@ -140,7 +111,7 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
     const Point3 w_measurement_j =
         wTj.rotation().rotate(calibratedMeasurements[j]);
 
-    double q_i =
+    const double q_i =
         w_measurement_i.cross(w_measurement_j).norm() /
         (measurementNoise->sigma() * d_ij.cross(w_measurement_j).norm());
     const Matrix23 coefficient_mat =
@@ -174,7 +145,6 @@ Point3 triangulateDLT(
   return Point3(v.head<3>() / v[3]);
 }
 
-///
 /**
  * Optimize for triangulation
  * @param graph nonlinear factors for projection

gtsam/geometry/triangulation.h

@@ -414,20 +414,19 @@ inline Point3Vector calibrateMeasurements(
  * @param measurements A vector of camera measurements
  * @param rank_tol rank tolerance, default 1e-9
  * @param optimize Flag to turn on nonlinear refinement of triangulation
- * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT
+ * @param use_lost_triangulation whether to use the LOST algorithm instead of
+ * DLT
  * @return Returns a Point3
  */
 template <class CALIBRATION>
 Point3 triangulatePoint3(const std::vector<Pose3>& poses,
                          boost::shared_ptr<CALIBRATION> sharedCal,
-    const Point2Vector& measurements, double rank_tol = 1e-9,
+                         const Point2Vector& measurements,
-    bool optimize = false,
+                         double rank_tol = 1e-9, bool optimize = false,
                          const SharedNoiseModel& model = nullptr,
-    const bool use_lost_triangulation = true) {
+                         const bool use_lost_triangulation = false) {
-
   assert(poses.size() == measurements.size());
-  if (poses.size() < 2)
+  if (poses.size() < 2) throw(TriangulationUnderconstrainedException());
-    throw(TriangulationUnderconstrainedException());
 
   // Triangulate linearly
   Point3 point;
@@ -437,7 +436,8 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
     const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
     SharedIsotropic measurementNoise =
         noiseModel::Isotropic::Sigma(2, measurementSigma);
-    // calibrate the measurements to obtain homogenous coordinates in image plane.
+    // calibrate the measurements to obtain homogenous coordinates in image
+    // plane.
     auto calibratedMeasurements =
         calibrateMeasurementsShared<CALIBRATION>(*sharedCal, measurements);
 
@@ -450,7 +450,8 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
     auto undistortedMeasurements =
         undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
 
-    point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
+    point =
+        triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
   }
 
   // Then refine using non-linear optimization
@@ -462,8 +463,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
   // verify that the triangulated point lies in front of all cameras
   for (const Pose3& pose : poses) {
     const Point3& p_local = pose.transformTo(point);
-    if (p_local.z() <= 0)
+    if (p_local.z() <= 0) throw(TriangulationCheiralityException());
-      throw(TriangulationCheiralityException());
   }
 #endif
 
@@ -480,22 +480,20 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
  * @param measurements A vector of camera measurements
  * @param rank_tol rank tolerance, default 1e-9
  * @param optimize Flag to turn on nonlinear refinement of triangulation
- * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT
+ * @param use_lost_triangulation whether to use the LOST algorithm instead of
+ * DLT
  * @return Returns a Point3
  */
 template <class CAMERA>
-Point3 triangulatePoint3(
+Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
-    const CameraSet<CAMERA>& cameras,
+                         const typename CAMERA::MeasurementVector& measurements,
-    const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9,
+                         double rank_tol = 1e-9, bool optimize = false,
-    bool optimize = false,
                          const SharedNoiseModel& model = nullptr,
                          const bool use_lost_triangulation = false) {
-
   size_t m = cameras.size();
   assert(measurements.size() == m);
 
-  if (m < 2)
+  if (m < 2) throw(TriangulationUnderconstrainedException());
-    throw(TriangulationUnderconstrainedException());
 
   // Triangulate linearly
   Point3 point;
@@ -525,7 +523,8 @@ Point3 triangulatePoint3(
     auto undistortedMeasurements =
         undistortMeasurements<CAMERA>(cameras, measurements);
 
-    point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
+    point =
+        triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
   }
 
   // Then refine using non-linear optimization
@@ -537,8 +536,7 @@ Point3 triangulatePoint3(
   // verify that the triangulated point lies in front of all cameras
   for (const CAMERA& camera : cameras) {
     const Point3& p_local = camera.pose().transformTo(point);
-    if (p_local.z() <= 0)
+    if (p_local.z() <= 0) throw(TriangulationCheiralityException());
-      throw(TriangulationCheiralityException());
   }
 #endif