WIP: debug...

cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp

@@ -15,6 +15,8 @@
 #include <DO/Sara/MultiViewGeometry/PnP/LambdaTwist.hpp>
 #include <DO/Sara/MultiViewGeometry/PointRayCorrespondenceList.hpp>
 
+#include <fmt/format.h>
+
 
 namespace DO::Sara {
 
@@ -28,9 +30,9 @@ namespace DO::Sara {
     using data_point_type = std::array<TensorView_<T, 2>, 2>;
     using model_type = Eigen::Matrix<T, 3, 4>;
 
-    inline auto operator()(const tensor_view_type& scene_points,
-                           const tensor_view_type& rays) const
-        -> std::vector<model_type>
+    inline auto
+    operator()(const tensor_view_type& scene_points,
+               const tensor_view_type& rays) const -> std::vector<model_type>
     {
       const auto sp_mat_ = scene_points.colmajor_view().matrix();
 
@@ -106,6 +108,8 @@ 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.
@@ -129,7 +133,19 @@ namespace DO::Sara {
 
       // Check the **squared** image reprojection errors.
       const auto ε_max = square(ε);
-      const auto ε_small = (u2 - u1).colwise().squaredNorm().array() < ε_max;
+      const auto ε_squared = (u2 - u1).colwise().squaredNorm().array();
+      const auto ε_small = ε_squared < ε_max;
+
+      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)
+      {
+        fmt::print("u1[{}]: {}   u2[{}]: {}\n",      //
+                   i, u1.col(i).transpose().eval(),  //
+                   i, u2.col(i).transpose().eval());
+      }
+      fmt::print("ε =\n{}\n", ε_debug.head(col_max).eval());
+      fmt::print("ε_small.count() = {}\n", ε_small.count());
 
       return ε_small && cheiral;
     }

cpp/src/DO/Sara/RANSAC/RANSACv2.hpp

@@ -87,6 +87,9 @@ namespace DO::Sara::v2 {
                         data_normalizer->denormalize(model);
                       });
 
+      if (verbose)
+        SARA_CHECK(candidate_models.size());
+
       // Count the inliers.
       for (const auto& model : candidate_models)
       {

cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.cpp

@@ -25,8 +25,10 @@ auto CameraPoseEstimator::estimate_pose(
 {
   _inlier_predicate.set_camera(camera);
 
+  static constexpr auto debug = true;
   return v2::ransac(point_ray_pairs, _solver, _inlier_predicate,
-                    _ransac_iter_max, _ransac_confidence_min);
+                    _ransac_iter_max, _ransac_confidence_min, std::nullopt,
+                    debug);
 }
 
 auto CameraPoseEstimator::estimate_pose(
@@ -39,7 +41,7 @@ auto CameraPoseEstimator::estimate_pose(
 
   const auto num_ftracks = static_cast<Eigen::Index>(valid_ftracks.size());
 
-  SARA_LOGD(logger, "Cleaning feature tracks using NMS...");
+  SARA_LOGD(logger, "Applying NMS to the {} feature tracks...", num_ftracks);
 
   auto ftracks_filtered = std::vector<FeatureTrack>{};
   ftracks_filtered.resize(num_ftracks);
@@ -53,8 +55,8 @@ auto CameraPoseEstimator::estimate_pose(
           throw std::runtime_error{"Error: the filtered track must contain the "
                                    "target camera vertex!"};
         if (ftrack_filtered.size() <= 2)
-          throw std::runtime_error{
-              "Error: a filtered feature track can't possibly be of size 2!"};
+          throw std::runtime_error{"Error: a filtered feature track can't "
+                                   "possibly have cardinality 2!"};
         return ftrack_filtered;
       });
 
@@ -80,11 +82,16 @@ auto CameraPoseEstimator::estimate_pose(
     // If there are more than one scene point index, we fetch the barycentric
     // coordinates anyway.
     scene_coords.col(t) = pcg.barycenter(scene_point_indices).coords();
+    if (t < 10)
+      std::cout << t << " -> " << scene_coords.col(t).transpose() << std::endl;
   }
 
   // 2. Collect the backprojected rays from the current camera view for each
   //    feature track.
-  SARA_LOGD(logger, "Calculating backprojected rays for each feature track...");
+  SARA_LOGD(logger,
+            "Calculating backprojected rays from camera pose [{}] for each "
+            "feature track...",
+            pv);
   auto rays = point_ray_pairs.y.colmajor_view().matrix();
   for (auto t = 0; t < num_ftracks; ++t)
   {
@@ -94,7 +101,11 @@ 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>();
-    rays.col(t) = camera.backproject(pixel_coords);
+    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();
   }
 
   // 3. solve the PnP problem with RANSAC.

cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.hpp

@@ -39,7 +39,7 @@ namespace DO::Sara {
 
     //! @brief Set robust estimation parameters.
     auto
-    set_estimation_params(const PixelUnit error_max = 0.5_px,
+    set_estimation_params(const PixelUnit error_max = 5._px,
                           const int ransac_iter_max = 1000u,
                           const double ransac_confidence_min = 0.99) -> void
     {

cpp/src/DO/Sara/SfM/OdometryV2/OdometryPipeline.cpp

@@ -222,7 +222,10 @@ auto v2::OdometryPipeline::grow_geometry() -> bool
                                           _pose_curr, _camera_corrected,
                                           *_point_cloud_generator);
     if (!abs_pose_est_successful)
+    {
+      SARA_LOGI(logger, "Failed to estimate the absolute pose!");
       return false;
+    }
 
     // 7. Update the current absolute pose, which was initialized dummily.
     abs_pose_curr = QuaternionBasedPose<double>{