experimenting with LS and TLS

gtsam/geometry/triangulation.cpp

@@ -65,6 +65,50 @@ Vector3 triangulateLOSTHomogeneous(
   size_t m = calibrated_measurements.size();
   assert(m == poses.size());
 
+  // Construct the system matrices.
+  Matrix A = Matrix::Zero(m * 2, 4);
+
+  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;
+    const Pose3& wTj = poses[j];
+
+    const Point3 d_ij = wTj.translation() - wTi.translation();
+
+    const Point3 w_measurement_i =
+        wTi.rotation().rotate(calibrated_measurements[i]);
+    const Point3 w_measurement_j =
+        wTj.rotation().rotate(calibrated_measurements[j]);
+
+    const double q_i = w_measurement_i.cross(w_measurement_j).norm() /
+                       (measurement_sigma * d_ij.cross(w_measurement_j).norm());
+    const Matrix23 coefficient_mat =
+        q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) *
+        wTi.rotation().matrix().transpose();
+
+    A.block<2, 4>(2 * i, 0) << coefficient_mat, -coefficient_mat * wTi.translation();
+  }
+
+  const double rank_tol = 1e-6;
+  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]);
+}
+
+Vector3 triangulateLOSTHomogeneousLS(
+    const std::vector<Pose3>& poses,
+    const std::vector<Point3>& calibrated_measurements, 
+    const double measurement_sigma) {
+  size_t m = calibrated_measurements.size();
+  assert(m == poses.size());
+
   // Construct the system matrices.
   Matrix A = Matrix::Zero(m * 2, 3);
   Matrix b = Matrix::Zero(m * 2, 1);
@@ -75,26 +119,20 @@ Vector3 triangulateLOSTHomogeneous(
     const int j = (i + 1) % m;
     const Pose3& wTj = poses[j];
 
-    Point3 d_ij = wTj.translation() - wTi.translation();
+    const Point3 d_ij = wTj.translation() - wTi.translation();
 
-    Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]);
-    Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]);
+    const Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]);
+    const Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]);
 
-    double numerator = w_measurement_i.cross(w_measurement_j).norm();
-    double denominator = d_ij.cross(w_measurement_j).norm();
-
-    double q_i = numerator / (measurement_sigma * denominator);
-    Matrix23 coefficient_mat =
+    const double q_i = w_measurement_i.cross(w_measurement_j).norm() /
+                       (measurement_sigma * d_ij.cross(w_measurement_j).norm());
+    const Matrix23 coefficient_mat =
         q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) *
         wTi.rotation().matrix().transpose();
 
-    A.row(2 * i) = coefficient_mat.row(0);
-    A.row(2 * i + 1) = coefficient_mat.row(1);
-    Point2 p = coefficient_mat * wTi.translation();
-    b(2 * i) = p.x();
-    b(2 * i + 1) = p.y();
+    A.block<2, 3>(2*i, 0) = coefficient_mat;
+    b.block<2, 1>(2*i, 0) = coefficient_mat * wTi.translation();
   }
-  // Solve Ax = b, using QR decomposition
   return A.colPivHouseholderQr().solve(b);
 }
 

gtsam/geometry/triangulation.h

@@ -75,6 +75,11 @@ triangulateLOSTHomogeneous(const std::vector<Pose3>& poses,
                            const std::vector<Point3>& calibrated_measurements,
                            const double measurement_sigma);
 
+GTSAM_EXPORT Vector3
+triangulateLOSTHomogeneousLS(const std::vector<Pose3>& poses,
+                           const std::vector<Point3>& calibrated_measurements,
+                           const double measurement_sigma);
+
 /**
  * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors
  * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman)
@@ -397,9 +402,9 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
 
 template <class CALIBRATION>
 Point3 triangulateLOSTPoint3(
-    const std::vector<PinholeCamera<CALIBRATION>>& cameras,
-    const std::vector<Point2>& measurements,
-    const double measurement_sigma = 1e-2) {
+    const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
+    const Point2Vector& measurements,
+    const double measurement_sigma = 1e-2, bool use_dlt = false) {
   const size_t num_cameras = cameras.size();
   assert(measurements.size() == num_cameras);
 
@@ -417,7 +422,11 @@ Point3 triangulateLOSTPoint3(
   poses.reserve(cameras.size());
   for (const auto& camera : cameras) poses.push_back(camera.pose());
 
-  Point3 point = triangulateLOSTHomogeneous(poses, calibrated_measurements, measurement_sigma);
+  Point3 point = use_dlt
+                     ? triangulateLOSTHomogeneous(
+                           poses, calibrated_measurements, measurement_sigma)
+                     : triangulateLOSTHomogeneousLS(
+                           poses, calibrated_measurements, measurement_sigma);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   // verify that the triangulated point lies in front of all cameras