Example to run LOST and compare to DLT

examples/TriangulationLOSTExample.cpp

@@ -0,0 +1,161 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file TriangulationLOSTExample.cpp
+ * @author Akshay Krishnan
+ * @brief This example runs triangulation several times using 3 different
+ * approaches: LOST, DLT, and DLT with optimization. It reports the covariance
+ * and the runtime for each approach.
+ *
+ * @date 2022-07-10
+ */
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/triangulation.h>
+
+#include <chrono>
+#include <iostream>
+#include <random>
+
+using namespace std;
+using namespace gtsam;
+
+static std::mt19937 rng(42);
+
+void PrintCovarianceStats(const Matrix& mat, const std::string& method) {
+  Matrix centered = mat.rowwise() - mat.colwise().mean();
+  Matrix cov = (centered.adjoint() * centered) / double(mat.rows() - 1);
+  std::cout << method << " covariance: " << std::endl;
+  std::cout << cov << std::endl;
+  std::cout << "Trace sqrt: " << sqrt(cov.trace()) << std::endl << std::endl;
+}
+
+void PrintDuration(const std::chrono::nanoseconds dur, double num_samples,
+                   const std::string& method) {
+  double nanoseconds = dur.count() / num_samples;
+  std::cout << "Time taken by " << method << ": " << nanoseconds * 1e-3
+            << std::endl;
+}
+
+void GetLargeCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras,
+                            std::vector<Pose3>* poses, Point3* point,
+                            Point2Vector* measurements) {
+  const double min_xy = -10, max_xy = 10;
+  const double min_z = -20, max_z = 0;
+  const int num_cameras = 500;
+  cameras->reserve(num_cameras);
+  poses->reserve(num_cameras);
+  measurements->reserve(num_cameras);
+  *point = Point3(0.0, 0.0, 10.0);
+
+  std::uniform_real_distribution<double> rand_xy(min_xy, max_xy);
+  std::uniform_real_distribution<double> rand_z(min_z, max_z);
+  Cal3_S2 identity_K;
+
+  for (int i = 0; i < num_cameras; ++i) {
+    Point3 wti(rand_xy(rng), rand_xy(rng), rand_z(rng));
+    Pose3 wTi(Rot3(), wti);
+
+    poses->push_back(wTi);
+    cameras->emplace_back(wTi, identity_K);
+    measurements->push_back(cameras->back().project(*point));
+  }
+}
+
+void GetSmallCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras,
+                            std::vector<Pose3>* poses, Point3* point,
+                            Point2Vector* measurements) {
+  Pose3 pose_1;
+  Pose3 pose_2(Rot3(), Point3(5., 0., -5.));
+  Cal3_S2 identity_K;
+  PinholeCamera<Cal3_S2> camera_1(pose_1, identity_K);
+  PinholeCamera<Cal3_S2> camera_2(pose_2, identity_K);
+
+  *point = Point3(0, 0, 1);
+  cameras->push_back(camera_1);
+  cameras->push_back(camera_2);
+  *poses = {pose_1, pose_2};
+  *measurements = {camera_1.project(*point), camera_2.project(*point)};
+}
+
+Point2Vector AddNoiseToMeasurements(const Point2Vector& measurements,
+                                    const double measurement_sigma) {
+  std::normal_distribution<double> normal(0.0, measurement_sigma);
+
+  Point2Vector noisy_measurements;
+  noisy_measurements.reserve(measurements.size());
+  for (const auto& p : measurements) {
+    noisy_measurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng));
+  }
+  return noisy_measurements;
+}
+
+/* ************************************************************************* */
+int main(int argc, char* argv[]) {
+  CameraSet<PinholeCamera<Cal3_S2>> cameras;
+  std::vector<Pose3> poses;
+  Point3 gt_point;
+  Point2Vector measurements;
+  GetLargeCamerasDataset(&cameras, &poses, &gt_point, &measurements);
+  // GetSmallCamerasDataset(&cameras, &poses, &gt_point, &measurements);
+  const double measurement_sigma = 1e-2;
+  SharedNoiseModel measurement_noise =
+      noiseModel::Isotropic::Sigma(2, measurement_sigma);
+
+  const long int num_trials = 1000;
+  Matrix dlt_errors = Matrix::Zero(num_trials, 3);
+  Matrix lost_errors = Matrix::Zero(num_trials, 3);
+  Matrix dlt_opt_errors = Matrix::Zero(num_trials, 3);
+
+  double rank_tol = 1e-9;
+  boost::shared_ptr<Cal3_S2> calib = boost::make_shared<Cal3_S2>();
+  std::chrono::nanoseconds dlt_duration;
+  std::chrono::nanoseconds dlt_opt_duration;
+  std::chrono::nanoseconds lost_duration;
+  std::chrono::nanoseconds lost_tls_duration;
+
+  for (int i = 0; i < num_trials; i++) {
+    Point2Vector noisy_measurements =
+        AddNoiseToMeasurements(measurements, measurement_sigma);
+
+    auto lost_start = std::chrono::high_resolution_clock::now();
+    boost::optional<Point3> estimate_lost = triangulatePoint3<Cal3_S2>(
+        cameras, noisy_measurements, rank_tol, false, measurement_noise, true);
+    lost_duration += std::chrono::high_resolution_clock::now() - lost_start;
+
+    auto dlt_start = std::chrono::high_resolution_clock::now();
+    boost::optional<Point3> estimate_dlt = triangulatePoint3<Cal3_S2>(
+        cameras, noisy_measurements, rank_tol, false, measurement_noise, false);
+    dlt_duration += std::chrono::high_resolution_clock::now() - dlt_start;
+
+    auto dlt_opt_start = std::chrono::high_resolution_clock::now();
+    boost::optional<Point3> estimate_dlt_opt = triangulatePoint3<Cal3_S2>(
+        cameras, noisy_measurements, rank_tol, true, measurement_noise, false);
+    dlt_opt_duration +=
+        std::chrono::high_resolution_clock::now() - dlt_opt_start;
+
+    lost_errors.row(i) = *estimate_lost - gt_point;
+    dlt_errors.row(i) = *estimate_dlt - gt_point;
+    dlt_opt_errors.row(i) = *estimate_dlt_opt - gt_point;
+  }
+  PrintCovarianceStats(lost_errors, "LOST");
+  PrintCovarianceStats(dlt_errors, "DLT");
+  PrintCovarianceStats(dlt_opt_errors, "DLT_OPT");
+
+  PrintDuration(lost_duration, num_trials, "LOST");
+  PrintDuration(dlt_duration, num_trials, "DLT");
+  PrintDuration(dlt_opt_duration, num_trials, "DLT_OPT");
+}