From 51c865a45faabd0e95308073ed504b82e536c1dc Mon Sep 17 00:00:00 2001 From: David OK Date: Wed, 24 Apr 2024 17:18:10 +0100 Subject: [PATCH] ENH: add robust estimation API for the PnP problem. --- cpp/src/DO/Sara/Core/PhysicalQuantities.hpp | 2 - .../MultiViewGeometry/Geometry/Normalizer.hpp | 30 ++++++++++- .../Geometry/PinholeCamera.hpp | 2 - .../MinimalSolvers/P3PSolver.hpp | 50 +++++++++++++++---- cpp/src/DO/Sara/RANSAC/RANSAC.hpp | 4 +- cpp/src/DO/Sara/RANSAC/RANSACv2.hpp | 8 +-- .../SyntheticDataUtilities.hpp | 7 ++- cpp/test/Sara/RANSAC/CMakeLists.txt | 2 +- 8 files changed, 79 insertions(+), 26 deletions(-) diff --git a/cpp/src/DO/Sara/Core/PhysicalQuantities.hpp b/cpp/src/DO/Sara/Core/PhysicalQuantities.hpp index 39f2d4d43..bb2be3837 100644 --- a/cpp/src/DO/Sara/Core/PhysicalQuantities.hpp +++ b/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; diff --git a/cpp/src/DO/Sara/MultiViewGeometry/Geometry/Normalizer.hpp b/cpp/src/DO/Sara/MultiViewGeometry/Geometry/Normalizer.hpp index 3e44cafff..92043f845 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/Geometry/Normalizer.hpp +++ b/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 { @@ -70,7 +72,7 @@ namespace DO::Sara { Eigen::Matrix3d T2_inv; }; - + //! @brief Normalizer for the two-view fundamental matrix estimation. template <> struct Normalizer { @@ -150,6 +152,7 @@ namespace DO::Sara { Eigen::Matrix3d K2_inv; }; + //! @brief Normalizer for the two-view relative pose estimation. template <> struct Normalizer { @@ -189,6 +192,31 @@ namespace DO::Sara { Eigen::Matrix3d K2_inv; }; + //! @brief Normalizer for the PnP estimation. + template <> + struct Normalizer> + { + using PoseMatrix = Eigen::Matrix; + + Normalizer() = default; + + auto normalize(const TensorView_& scene_points, + const TensorView_& backprojected_rays) const + { + return std::make_tuple(scene_points, backprojected_rays); + } + + auto normalize(const PointCorrespondenceList& M) const + -> PointCorrespondenceList + { + return M; + } + + auto denormalize(const PoseMatrix&) const -> void + { + } + }; + //! @} } /* namespace DO::Sara */ diff --git a/cpp/src/DO/Sara/MultiViewGeometry/Geometry/PinholeCamera.hpp b/cpp/src/DO/Sara/MultiViewGeometry/Geometry/PinholeCamera.hpp index 848013e01..7ecdbfe16 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/Geometry/PinholeCamera.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/Geometry/PinholeCamera.hpp @@ -11,8 +11,6 @@ #pragma once -#include - #include #include diff --git a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp index 5c3fadf76..07d4c4a2b 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp +++ b/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 +// Copyright (C) 2024-present David Ok // // 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 #include #include @@ -24,15 +24,28 @@ namespace DO::Sara { static constexpr auto num_points = 3; static constexpr auto num_models = 4; - using data_point_type = TensorView_; + using tensor_view_type = TensorView_; + using data_point_type = std::array, 2>; using model_type = Eigen::Matrix; - 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 { - const Eigen::Matrix3 scene_points = x.matrix().leftCols(3); - const Eigen::Matrix3 backprojected_rays = x.matrix().rightCols(3); - return solve_p3p(scene_points, backprojected_rays); + const auto sp_mat_ = scene_points.colmajor_view().matrix(); + + Eigen::Matrix3 sp_mat = sp_mat_.topRows(3); + if (sp_mat_.cols() == 4) + sp_mat.array().rowwise() /= sp_mat_.array().row(3); + + const Eigen::Matrix3 ray_mat = rays.colmajor_view().matrix(); + return solve_p3p(sp_mat, ray_mat); + } + + inline auto operator()(const data_point_type& X) -> std::vector + { + 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 - inline auto operator()(const PointRayCorrespondenceSubsetList& m) const + inline auto operator()(const PointRayCorrespondenceList& m) const -> Eigen::Array { const auto& scene_points = m.x.colmajor_view().matrix(); @@ -103,4 +128,7 @@ namespace DO::Sara { } }; + + //! @} + } // namespace DO::Sara diff --git a/cpp/src/DO/Sara/RANSAC/RANSAC.hpp b/cpp/src/DO/Sara/RANSAC/RANSAC.hpp index 124af43a3..fe6250bd8 100644 --- a/cpp/src/DO/Sara/RANSAC/RANSAC.hpp +++ b/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["}; diff --git a/cpp/src/DO/Sara/RANSAC/RANSACv2.hpp b/cpp/src/DO/Sara/RANSAC/RANSACv2.hpp index cb4fd1a16..6a7c377ee 100644 --- a/cpp/src/DO/Sara/RANSAC/RANSACv2.hpp +++ b/cpp/src/DO/Sara/RANSAC/RANSACv2.hpp @@ -13,6 +13,8 @@ #include +#include + 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(data_points.size()); + inlier_ratio_current = std::clamp( + num_inliers / static_cast(data_points.size()), // + 0., 1.); update_num_iterations(); if (verbose) diff --git a/cpp/test/Sara/MultiViewGeometry/SyntheticDataUtilities.hpp b/cpp/test/Sara/MultiViewGeometry/SyntheticDataUtilities.hpp index 7798a696c..47103ea36 100644 --- a/cpp/test/Sara/MultiViewGeometry/SyntheticDataUtilities.hpp +++ b/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. diff --git a/cpp/test/Sara/RANSAC/CMakeLists.txt b/cpp/test/Sara/RANSAC/CMakeLists.txt index 8dee67c92..3a76f3b67 100644 --- a/cpp/test/Sara/RANSAC/CMakeLists.txt +++ b/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 ()