From 20d044fb277fd305377019b2a2aa0c953b19e084 Mon Sep 17 00:00:00 2001
From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com>
Date: Mon, 29 Apr 2024 23:03:38 +0100
Subject: [PATCH] WIP: debug.

---
 .../MultiViewGeometry/MinimalSolvers/P3PSolver.hpp   |  5 +++--
 .../Sara/SfM/BuildingBlocks/CameraPoseEstimator.cpp  | 12 ++++++++----
 .../Sara/SfM/BuildingBlocks/CameraPoseEstimator.hpp  |  2 +-
 3 files changed, 12 insertions(+), 7 deletions(-)

diff --git a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp
index f43b227b3..ef86fab35 100644
--- a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp
+++ b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp
@@ -108,8 +108,6 @@ namespace DO::Sara {
             "The dimension of scene points is incorrect. "
             "They must either 3D (Euclidean) or 4D (homogeneous)!"};
 
-      fmt::print("Pose:\n{}\n", T);
-
       // Project the camera coordinates to the image plane.
       //
       // The result is a list of pixel coordinates.
@@ -136,6 +134,8 @@ namespace DO::Sara {
       const auto ε_squared = (u2 - u1).colwise().squaredNorm().array();
       const auto ε_small = ε_squared < ε_max;
 
+#if 0
+      fmt::print("Pose:\n{}\n", T);
       const auto ε_debug = Eigen::VectorXd{ε_squared.sqrt()};
       const auto col_max = std::min(Eigen::Index{10}, u2.cols());
       for (auto i = 0; i < col_max; ++i)
@@ -146,6 +146,7 @@ namespace DO::Sara {
       }
       fmt::print("ε =\n{}\n", ε_debug.head(col_max).eval());
       fmt::print("ε_small.count() = {}\n", ε_small.count());
+#endif
 
       return ε_small && cheiral;
     }
diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.cpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.cpp
index 29998a6f9..cff79c647 100644
--- a/cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.cpp
+++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.cpp
@@ -25,7 +25,7 @@ auto CameraPoseEstimator::estimate_pose(
 {
   _inlier_predicate.set_camera(camera);
 
-  static constexpr auto debug = true;
+  static constexpr auto debug = false;
   return v2::ransac(point_ray_pairs, _solver, _inlier_predicate,
                     _ransac_iter_max, _ransac_confidence_min, std::nullopt,
                     debug);
@@ -101,16 +101,20 @@ auto CameraPoseEstimator::estimate_pose(
     if (!fv.has_value())
       throw std::runtime_error{"Error: the feature track must be alive!"};
     const auto pixel_coords = pcg.pixel_coords(*fv).cast<double>();
-    if (t < 10)
-      SARA_LOGD(logger, "Backprojecting point {}: {}", t,
-                pixel_coords.transpose().eval());
     // Normalize the rays. This is important for Lambda-Twist P3P method.
     rays.col(t) = camera.backproject(pixel_coords).normalized();
+    if (t < 10)
+      SARA_LOGD(logger, "Backproject point {}:\n{} -> {}", t,
+                pixel_coords.transpose().eval(),
+                rays.col(t).transpose().eval());
   }
 
   // 3. solve the PnP problem with RANSAC.
   const auto [pose, inliers, sample_best] =
       estimate_pose(point_ray_pairs, camera);
+  SARA_LOGD(logger, "[AbsPoseEst] Pose:\n{}", pose);
+  SARA_LOGD(logger, "[AbsPoseEst] inlier count: {}",
+            inliers.flat_array().count());
   const auto pose_estimated_successfully =
       inliers.flat_array().count() >= _ransac_inliers_min;
 
diff --git a/cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.hpp b/cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.hpp
index ff9ad39ee..1bb10f91d 100644
--- a/cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.hpp
+++ b/cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.hpp
@@ -40,7 +40,7 @@ namespace DO::Sara {
     //! @brief Set robust estimation parameters.
     auto
     set_estimation_params(const PixelUnit error_max = 5._px,
-                          const int ransac_iter_max = 1000u,
+                          const int ransac_iter_max = 10000,
                           const double ransac_confidence_min = 0.99) -> void
     {
       _inlier_predicate.ε = error_max.value;