ENH: add robust estimation API for the PnP problem.

cpp/src/DO/Sara/Core/PhysicalQuantities.hpp

@@ -252,8 +252,6 @@ namespace DO::Sara {
     {
     }
 
-    auto operator=(const self_type&) -> self_type& = default;
-
     inline constexpr operator scalar_type() const
     {
       return value;

cpp/src/DO/Sara/MultiViewGeometry/Geometry/Normalizer.hpp

@@ -25,6 +25,8 @@ namespace DO::Sara {
   //! @ingroup GeometryDataNormalizer
 
   //! @{
+
+  //! @brief Normalizer for the two-view homography estimation.
   template <>
   struct Normalizer<Homography>
   {
@@ -70,7 +72,7 @@ namespace DO::Sara {
     Eigen::Matrix3d T2_inv;
   };
 
-
+  //! @brief Normalizer for the two-view fundamental matrix estimation.
   template <>
   struct Normalizer<FundamentalMatrix>
   {
@@ -150,6 +152,7 @@ namespace DO::Sara {
     Eigen::Matrix3d K2_inv;
   };
 
+  //! @brief Normalizer for the two-view relative pose estimation.
   template <>
   struct Normalizer<TwoViewGeometry>
   {
@@ -189,6 +192,31 @@ namespace DO::Sara {
     Eigen::Matrix3d K2_inv;
   };
 
+  //! @brief Normalizer for the PnP estimation.
+  template <>
+  struct Normalizer<Eigen::Matrix<double, 3, 4>>
+  {
+    using PoseMatrix = Eigen::Matrix<double, 3, 4>;
+
+    Normalizer() = default;
+
+    auto normalize(const TensorView_<double, 2>& scene_points,
+                   const TensorView_<double, 2>& backprojected_rays) const
+    {
+      return std::make_tuple(scene_points, backprojected_rays);
+    }
+
+    auto normalize(const PointCorrespondenceList<double>& M) const
+        -> PointCorrespondenceList<double>
+    {
+      return M;
+    }
+
+    auto denormalize(const PoseMatrix&) const -> void
+    {
+    }
+  };
+
   //! @}
 
 } /* namespace DO::Sara */

cpp/src/DO/Sara/MultiViewGeometry/Geometry/PinholeCamera.hpp

@@ -11,8 +11,6 @@
 
 #pragma once
 
-#include <DO/Sara/Defines.hpp>
-
 #include <DO/Sara/Core/EigenExtension.hpp>
 #include <DO/Sara/MultiViewGeometry/Geometry/EssentialMatrix.hpp>
 

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

@@ -2,7 +2,7 @@
 // This file is part of Sara, a basic set of libraries in C++ for computer
 // vision.
 //
-// Copyright (C) 2024 David Ok <[email protected]>
+// Copyright (C) 2024-present David Ok <[email protected]>
 //
 // This Source Code Form is subject to the terms of the Mozilla Public
 // License v. 2.0. If a copy of the MPL was not distributed with this file,
@@ -11,7 +11,7 @@
 
 #pragma once
 
-#include "DO/Sara/Core/Math/UsualFunctions.hpp"
+#include <DO/Sara/Core/Math/UsualFunctions.hpp>
 #include <DO/Sara/MultiViewGeometry/PnP/LambdaTwist.hpp>
 #include <DO/Sara/MultiViewGeometry/PointRayCorrespondenceList.hpp>
 
@@ -24,15 +24,28 @@ namespace DO::Sara {
     static constexpr auto num_points = 3;
     static constexpr auto num_models = 4;
 
-    using data_point_type = TensorView_<T, 2>;
+    using tensor_view_type = TensorView_<T, 2>;
+    using data_point_type = std::array<TensorView_<T, 2>, 2>;
     using model_type = Eigen::Matrix<T, 3, 4>;
 
-    inline auto operator()(const data_point_type& x) const
+    inline auto operator()(const tensor_view_type& scene_points,
+                           const tensor_view_type& rays) const
         -> std::vector<model_type>
     {
-      const Eigen::Matrix3<T> scene_points = x.matrix().leftCols(3);
-      const Eigen::Matrix3<T> backprojected_rays = x.matrix().rightCols(3);
-      return solve_p3p(scene_points, backprojected_rays);
+      const auto sp_mat_ = scene_points.colmajor_view().matrix();
+
+      Eigen::Matrix3<T> sp_mat = sp_mat_.topRows(3);
+      if (sp_mat_.cols() == 4)
+        sp_mat.array().rowwise() /= sp_mat_.array().row(3);
+
+      const Eigen::Matrix3<T> ray_mat = rays.colmajor_view().matrix();
+      return solve_p3p(sp_mat, ray_mat);
+    }
+
+    inline auto operator()(const data_point_type& X) -> std::vector<model_type>
+    {
+      const auto& [scene_points, backprojected_rays] = X;
+      return this->operator()(scene_points, backprojected_rays);
     }
   };
 
@@ -47,7 +60,7 @@ namespace DO::Sara {
     const CameraModel* camera = nullptr;
     //! @brief The pose matrix.
     PoseMatrix T;
-    //! @brief Image reprojection error in pixel.
+    //! @brief Image reprojection error in pixels.
     double ε;
 
     inline CheiralPnPConsistency() = default;
@@ -71,14 +84,26 @@ namespace DO::Sara {
         throw std::runtime_error{
             "Error: you must initialize the intrinsic camera parameters!"};
 
+      if (scene_points.cols() != rays.cols())
+        throw std::runtime_error{
+            "Error: the number of scene points and rays must be equal!"};
+
       const auto& X_world = scene_points;
-      const Eigen::MatrixXd X_camera = T * X_world;
+      auto X_camera = Eigen::MatrixXd{};
+      if (X_world.rows() == 3)
+        X_camera = T * X_world.colwise().homogeneous();
+      else if (X_world.rows() == 4)
+        X_camera = T * X_world;
+      else
+        throw std::runtime_error{
+            "The dimension of scene points is incorrect. They must either 3D "
+            "(Euclidean) or 4D (homogeneous)!"};
 
       auto u1 = Eigen::MatrixXd{2, scene_points.cols()};
       for (auto i = 0; i < u1.cols(); ++i)
         u1.col(i) = camera->project(X_camera.col(i));
 
-      auto u2 = Eigen::MatrixXd{rays.rows(), rays.cols()};
+      auto u2 = Eigen::MatrixXd{2, rays.cols()};
       for (auto i = 0; i < u2.cols(); ++i)
         u2.col(i) = camera->project(rays.col(i));
 
@@ -94,7 +119,7 @@ namespace DO::Sara {
 
     //! @brief Check the inlier predicate on a list of correspondences.
     template <typename T>
-    inline auto operator()(const PointRayCorrespondenceSubsetList<T>& m) const
+    inline auto operator()(const PointRayCorrespondenceList<T>& m) const
         -> Eigen::Array<bool, 1, Eigen::Dynamic>
     {
       const auto& scene_points = m.x.colmajor_view().matrix();
@@ -103,4 +128,7 @@ namespace DO::Sara {
     }
   };
 
+
+  //! @}
+
 }  // namespace DO::Sara

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

@@ -186,9 +186,9 @@ namespace DO::Sara {
                                  double confidence = 0.99) -> std::uint64_t
   {
     // Check the range of values...
-    if (!(0 <= inlier_ratio && inlier_ratio < 1))
+    if (!(0 <= inlier_ratio && inlier_ratio <= 1))
       throw std::runtime_error{
-          "Error: the inlier ratio must be in the open interval [0, 1["};
+          "Error: the inlier ratio must be in the open interval [0, 1]"};
     if (!(0 <= confidence && confidence < 1))
       throw std::runtime_error{
           "Error: the confidence value must be in the open interval [0, 1["};

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

@@ -13,6 +13,8 @@
 
 #include <DO/Sara/RANSAC/RANSAC.hpp>
 
+#include <DO/Sara/Core/DebugUtilities.hpp>
+
 
 namespace DO::Sara::v2 {
 
@@ -100,9 +102,9 @@ namespace DO::Sara::v2 {
           inliers_best.flat_array() = inliers;
           subset_best = minimal_index_subsets[n];
 
-          //
-          inlier_ratio_current =
-              num_inliers / static_cast<double>(data_points.size());
+          inlier_ratio_current = std::clamp(
+              num_inliers / static_cast<double>(data_points.size()),  //
+              0., 1.);
           update_num_iterations();
 
           if (verbose)

cpp/test/Sara/MultiViewGeometry/SyntheticDataUtilities.hpp

@@ -34,7 +34,8 @@ inline auto make_cube_vertices()
   return cube;
 }
 
-inline auto make_planar_chessboard_corners(int rows, int cols, double square_size)
+inline auto make_planar_chessboard_corners(int rows, int cols,
+                                           double square_size)
 {
   auto corners = Eigen::MatrixXd{4, rows * cols};
   for (auto y = 0; y < rows; ++y)
@@ -47,10 +48,8 @@ inline auto make_planar_chessboard_corners(int rows, int cols, double square_siz
 inline auto make_relative_motion(double x = 0.1, double y = 0.3, double z = 0.2)
     -> DO::Sara::Motion
 {
-  using namespace DO::Sara;
-
   // Euler composite rotation.
-  const Eigen::Matrix3d R = rotation(z, y, x);
+  const Eigen::Matrix3d R = DO::Sara::rotation(z, y, x);
   // - The axes of the world coordinate system has turned by the following
   //   rotational quantity.
   // - The columns of R are the vector coordinates of the world axes w.r.t.

cpp/test/Sara/RANSAC/CMakeLists.txt

@@ -5,6 +5,6 @@ foreach (file ${test_SOURCE_FILES})
   sara_add_test(
     NAME ${filename}
     SOURCES ${file}
-    DEPENDENCIES DO::Sara::MultiViewGeometry DO::Sara::RANSAC
+    DEPENDENCIES DO::Sara::MultiViewGeometry DO::Sara::RANSAC fmt::fmt
     FOLDER RANSAC)
 endforeach ()