WIP: debug.

oddkiva · Apr 29, 2024 · 20d044f · 20d044f
1 parent f581096
commit 20d044f
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 7 deletions.
diff --git 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
@@ -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
@@ -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;