Merge pull request #1534 from borglab/lost-underconstrained

LOST Triangulation Underconstrained

gtsam/geometry/triangulation.cpp

@@ -85,7 +85,8 @@ Vector4 triangulateHomogeneousDLT(
 
 Point3 triangulateLOST(const std::vector<Pose3>& poses,
                        const Point3Vector& calibratedMeasurements,
-                       const SharedIsotropic& measurementNoise) {
+                       const SharedIsotropic& measurementNoise,
+                       double rank_tol) {
   size_t m = calibratedMeasurements.size();
   assert(m == poses.size());
 
@@ -96,17 +97,38 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
   for (size_t i = 0; i < m; i++) {
     const Pose3& wTi = poses[i];
     // TODO(akshay-krishnan): are there better ways to select j?
-    const int j = (i + 1) % m;
+    int j = (i + 1) % m;
     const Pose3& wTj = poses[j];
 
-    const Point3 d_ij = wTj.translation() - wTi.translation();
+    Point3 d_ij = wTj.translation() - wTi.translation();
+    Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]);
+    Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
+    double num_i = wZi.cross(wZj).norm();
+    double den_i = d_ij.cross(wZj).norm();
 
-    const Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]);
+    // Handle q_i = 0 (or NaN), which arises if the measurement vectors, wZi and 
-    const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
+    // wZj, coincide (or the baseline vector coincides with the jth measurement 
+    // vector).
+    if (num_i == 0 || den_i == 0) {
+      bool success = false;
+      for (size_t k = 2; k < m; k++) {
+        j = (i + k) % m;
+        const Pose3& wTj = poses[j];
+
+        d_ij = wTj.translation() - wTi.translation();
+        wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
+        num_i = wZi.cross(wZj).norm();
+        den_i = d_ij.cross(wZj).norm();
+        if (num_i > 0 && den_i > 0) {
+          success = true;
+          break;
+        }
+      }
+      if (!success) throw(TriangulationUnderconstrainedException());
+    }
 
     // Note: Setting q_i = 1.0 gives same results as DLT.
-    const double q_i = wZi.cross(wZj).norm() /
+    const double q_i = num_i / (measurementNoise->sigma() * den_i);
-                       (measurementNoise->sigma() * d_ij.cross(wZj).norm());
 
     const Matrix23 coefficientMat =
         q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
@@ -115,7 +137,13 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
     A.block<2, 3>(2 * i, 0) << coefficientMat;
     b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation();
   }
-  return A.colPivHouseholderQr().solve(b);
+
+  Eigen::ColPivHouseholderQR<Matrix> A_Qr = A.colPivHouseholderQr();
+  A_Qr.setThreshold(rank_tol);
+
+  if (A_Qr.rank() < 3) throw(TriangulationUnderconstrainedException());
+
+  return A_Qr.solve(b);
 }
 
 Point3 triangulateDLT(

gtsam/geometry/triangulation.h

@@ -110,7 +110,8 @@ GTSAM_EXPORT Point3 triangulateDLT(
  */
 GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
                                     const Point3Vector& calibratedMeasurements,
-                                    const SharedIsotropic& measurementNoise);
+                                    const SharedIsotropic& measurementNoise,
+                                    double rank_tol = 1e-9);
 
 /**
  * Create a factor graph with projection factors from poses and one calibration
@@ -439,7 +440,8 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
     auto calibratedMeasurements =
         calibrateMeasurementsShared<CALIBRATION>(*sharedCal, measurements);
 
-    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
+    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, 
+                            rank_tol);
   } else {
     // construct projection matrices from poses & calibration
     auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
@@ -512,7 +514,8 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
     auto calibratedMeasurements =
         calibrateMeasurements<CAMERA>(cameras, measurements);
 
-    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
+    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, 
+                            rank_tol);
   } else {
     // construct projection matrices from poses & calibration
     auto projection_matrices = projectionMatricesFromCameras(cameras);
@@ -750,4 +753,3 @@ using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
 using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
 using CameraSetSpherical = CameraSet<SphericalCamera>;
 } // \namespace gtsam
-