From 66d916034095d4a8e43aa3e2df1012a75692c2c6 Mon Sep 17 00:00:00 2001 From: David OK Date: Thu, 30 May 2024 15:20:32 +0100 Subject: [PATCH 01/14] WIP: add trackball. --- .../visual_odometry_example.cpp | 109 +++++++++++++++++- 1 file changed, 105 insertions(+), 4 deletions(-) diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp index 6de80a40f..25267a88f 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp @@ -12,7 +12,9 @@ #include #include #include +#include +#include #include #include @@ -31,12 +33,89 @@ namespace fs = std::filesystem; namespace sara = DO::Sara; +namespace k = DO::Kalpana; namespace kgl = DO::Kalpana::GL; using sara::operator""_m; using sara::operator""_deg; +struct UserInteractionResponder +{ + struct Time + { + void update() + { + last_frame = current_frame; + current_frame = static_cast(timer.elapsed_ms()); + delta_time = current_frame - last_frame; + } + + sara::Timer timer; + float delta_time = 0.f; + float last_frame = 0.f; + float current_frame = 0.f; + }; + + Time gtime = Time{}; + + //! @brief View objects. + //! @{ + k::Camera camera = {}; + kgl::TrackBall trackball = {}; + //! @} + + //! @brief View parameters. + //! @{ + bool show_checkerboard = true; + float scale = 1.f; + static constexpr auto scale_factor = 1.05f; + //! @} + + auto normalize_cursor_pos(GLFWwindow* const window, + const Eigen::Vector2d& pos) const -> Eigen::Vector2d + { + auto w = int{}; + auto h = int{}; + glfwGetWindowSize(window, &w, &h); + + const Eigen::Vector2d c = Eigen::Vector2i(w, h).cast() * 0.5; + + Eigen::Vector2d normalized_pos = ((pos - c).array() / c.array()).matrix(); + normalized_pos.y() *= -1; + return normalized_pos; + }; + + + auto mouse_pressed(GLFWwindow* const window, const int button, + const int action) -> void + { + if (button != GLFW_MOUSE_BUTTON_LEFT) + return; + + auto p = Eigen::Vector2d{}; + glfwGetCursorPos(window, &p.x(), &p.y()); + + const Eigen::Vector2f pf = normalize_cursor_pos(window, p).cast(); + if (action == GLFW_PRESS && !trackball.pressed()) + trackball.push(pf); + else if (action == GLFW_RELEASE && trackball.pressed()) + trackball.release(pf); + } + + auto mouse_moved(GLFWwindow* const window, const double x, const double y) + -> void + { + const auto curr_pos = Eigen::Vector2d{x, y}; + const Eigen::Vector2f p = + normalize_cursor_pos(window, curr_pos).cast(); + + if (trackball.pressed()) + trackball.move(p); + } +}; + + struct MyVideoScene : kgl::VideoScene { }; @@ -156,6 +235,8 @@ class SingleWindowApp // Register callbacks. glfwSetWindowSizeCallback(_window, window_size_callback); glfwSetKeyCallback(_window, key_callback); + glfwSetCursorPosCallback(_window, move_trackball); + glfwSetMouseButtonCallback(_window, use_trackball); } ~SingleWindowApp() @@ -324,8 +405,8 @@ class SingleWindowApp return *app_ptr; } - static auto window_size_callback(GLFWwindow* window, const int, - const int) -> void + static auto window_size_callback(GLFWwindow* window, const int, const int) + -> void { auto& self = get_self(window); @@ -387,6 +468,24 @@ class SingleWindowApp } } + + static auto use_trackball(GLFWwindow* window, int button, int action, + int /* mods */) -> void + { + auto& app = get_self(window); + app._responder.mouse_pressed(window, button, action); + } + + static auto move_trackball(GLFWwindow* window, double x, double y) -> void + { + auto& app = get_self(window); + app._responder.mouse_moved(window, x, y); + + app._pc_scene._view.topLeftCorner<3, 3>() = + app._responder.trackball.rotation().toRotationMatrix(); + app._pc_scene._model_view = app._pc_scene._view * app._pc_scene._model; + } + private: static auto init_glfw() -> void { @@ -436,6 +535,8 @@ class SingleWindowApp //! @brief Point cloud rendering. MyPointCloudScene _pc_scene; + UserInteractionResponder _responder; + //! @brief Our engine. sara::OdometryPipeline _pipeline; @@ -449,8 +550,8 @@ class SingleWindowApp bool SingleWindowApp::_glfw_initialized = false; -auto main([[maybe_unused]] int const argc, - [[maybe_unused]] char** const argv) -> int +auto main([[maybe_unused]] int const argc, [[maybe_unused]] char** const argv) + -> int { #if defined(_OPENMP) const auto num_threads = omp_get_max_threads(); From 36da75aad6a87de9233607e17209a61f478f991b Mon Sep 17 00:00:00 2001 From: David OK Date: Fri, 31 May 2024 01:13:40 +0100 Subject: [PATCH 02/14] BUG: we must discard the DoG with implausible refined scale. The refined scale is implausible if it is way too far from the approximate scale at which the DoG has been detected. --- cpp/src/DO/Sara/FeatureDescriptors/SIFT.hpp | 32 +++++-- .../Sara/FeatureDetectors/RefineExtremum.cpp | 92 +++++++++++-------- cpp/src/DO/Sara/FeatureDetectors/SIFT.cpp | 1 + 3 files changed, 76 insertions(+), 49 deletions(-) diff --git a/cpp/src/DO/Sara/FeatureDescriptors/SIFT.hpp b/cpp/src/DO/Sara/FeatureDescriptors/SIFT.hpp index c4aab2940..3e117f5b8 100644 --- a/cpp/src/DO/Sara/FeatureDescriptors/SIFT.hpp +++ b/cpp/src/DO/Sara/FeatureDescriptors/SIFT.hpp @@ -13,6 +13,9 @@ #pragma once +#if defined(DEBUG_SIFT) +# include +#endif #include #include #include @@ -54,25 +57,27 @@ namespace DO { namespace Sara { { } - //! @brief Computes the SIFT descriptor for keypoint @f$ (x,y,\sigma,\theta) @f$. - auto operator()(float x, float y, float sigma, float theta, + //! @brief Computes the SIFT descriptor for keypoint @f$ (x,y,\sigma,\theta) + //! @f$. + auto operator()(float x, float y, float s, float theta, const ImageView& grad_polar_coords, - bool do_normalization = true) const - -> descriptor_type + bool do_normalization = true) const -> descriptor_type { constexpr auto pi = static_cast(M_PI); // The radius of each overlapping patches. const auto& lambda = _bin_scale_unit_length; - const auto l = lambda * sigma; + const auto l = lambda * s; // The radius of the total patch. const auto r = sqrt(2.f) * l * (N + 1) / 2.f; // Linear part of the patch normalization transform. auto T = Matrix2f{}; + // clang-format off T << cos(theta), sin(theta), -sin(theta), cos(theta); + // clang-format on T /= l; // The SIFT descriptor. @@ -82,6 +87,13 @@ namespace DO { namespace Sara { const int rounded_r = int_round(r); const int rounded_x = int_round(x); const int rounded_y = int_round(y); +#if defined(DEBUG_SIFT) + SARA_CHECK(rounded_r); + SARA_CHECK(rounded_x); + SARA_CHECK(rounded_y); + SARA_CHECK(s); + SARA_CHECK(theta); +#endif for (auto v = -rounded_r; v <= rounded_r; ++v) { @@ -130,7 +142,6 @@ namespace DO { namespace Sara { } return h; - } //! @brief Computes the **upright** SIFT descriptor for keypoint @@ -155,8 +166,7 @@ namespace DO { namespace Sara { auto operator()(const std::vector& features, const std::vector& scale_octave_pairs, const ImagePyramid& gradient_polar_coords, - bool parallel = false) const - -> Tensor_ + bool parallel = false) const -> Tensor_ { auto sifts = Tensor_{{int(features.size()), Dim}}; if (parallel) @@ -176,6 +186,9 @@ namespace DO { namespace Sara { { for (size_t i = 0; i < features.size(); ++i) { +#if defined(DEBUG_SIFT) + SARA_CHECK(i); +#endif sifts.matrix().row(i) = this->operator()(features[i], gradient_polar_coords(scale_octave_pairs[i](0), @@ -252,5 +265,4 @@ namespace DO { namespace Sara { //! @} -} /* namespace Sara */ -} /* namespace DO */ +}} // namespace DO::Sara diff --git a/cpp/src/DO/Sara/FeatureDetectors/RefineExtremum.cpp b/cpp/src/DO/Sara/FeatureDetectors/RefineExtremum.cpp index 292b66526..1d435d7be 100644 --- a/cpp/src/DO/Sara/FeatureDetectors/RefineExtremum.cpp +++ b/cpp/src/DO/Sara/FeatureDetectors/RefineExtremum.cpp @@ -229,14 +229,14 @@ namespace DO::Sara { -> vector { // #define PROFILE_ME -#ifdef PROFILE_ME +# ifdef PROFILE_ME auto timer = Timer{}; auto tic_ = [&timer]() { timer.restart(); }; auto toc_ = [&timer](const std::string& what) { const auto elapsed = timer.elapsed_ms(); SARA_DEBUG << "[" << what << "] " << elapsed << "ms\n"; }; -#endif +# endif // ======================================================================== // Classify extrema. @@ -251,22 +251,22 @@ namespace DO::Sara { const auto& me = I(s, o); const auto& next = I(s + 1, o); -#ifdef PROFILE_ME +# ifdef PROFILE_ME tic_(); -#endif +# endif scale_space_dog_extremum_map(previous, me, next, edge_ratio_thres, extremum_thres, map); -#ifdef PROFILE_ME +# ifdef PROFILE_ME toc_("[Halide] DoG extremum map"); -#endif +# endif // ======================================================================== // Refine the location of extrema. // ======================================================================== -#ifdef PROFILE_ME +# ifdef PROFILE_ME tic_(); -#endif +# endif auto location_refined = Image{I(s, o).sizes()}; auto extremum_value = Image{I(s, o).sizes()}; @@ -274,9 +274,9 @@ namespace DO::Sara { const auto& h = I(s, o).height(); const auto wh = w * h; -#ifdef _OPENMP -# pragma omp parallel for -#endif +# ifdef _OPENMP +# pragma omp parallel for +# endif for (int xy = 0; xy < wh; ++xy) { const auto y = xy / w; @@ -299,25 +299,39 @@ namespace DO::Sara { refine_extremum(I, x, y, s, o, type, pos, val, img_padding_sz, refine_iterations); -#ifndef STRICT_LOCAL_EXTREMA - // Reject if contrast too low. - if (std::abs(val) < extremum_thres) + // Approximate scale. + const auto scale_approx = I.scale_relative_to_octave(s); + const auto scale_refined = pos.z(); + // Reject if the refined scale is unreasonably low or high, it has has to + // be not too from the approximate scale at which it has been detected. + static constexpr auto ratio_max = 4.f; // large enough in my opinion + static constexpr auto ratio_min = 1 / ratio_max; + const auto scale_min = ratio_min * scale_approx; + const auto scale_max = ratio_max * scale_approx; + const auto scale_implausible = + !(scale_min < scale_refined && scale_refined < scale_max); + + const auto too_low_contrast = std::abs(val) < extremum_thres; + +# ifndef STRICT_LOCAL_EXTREMA + // Reject if contrast too low or if the scale is implausible + if (too_low_contrast || scale_implausible) { // SARA_DEBUG << "Reject " << x << " " << y << std::endl; map(x, y) = 0; } -#endif +# endif } -#ifdef PROFILE_ME +# ifdef PROFILE_ME toc_("Calculating Location Residual"); -#endif +# endif // ======================================================================== // Fill the list of DoG extrema. // ======================================================================== -#ifdef PROFILE_ME +# ifdef PROFILE_ME tic_(); -#endif +# endif auto extrema = std::vector{}; extrema.reserve(10000); for (int xy = 0; xy < wh; ++xy) @@ -339,27 +353,27 @@ namespace DO::Sara { type == 1 ? OERegion::ExtremumType::Max : OERegion::ExtremumType::Min; extrema.push_back(dog); } -#ifdef PROFILE_ME +# ifdef PROFILE_ME toc_("Populating Extrema"); -#endif +# endif return extrema; } #else auto local_scale_space_extrema(const ImagePyramid& I, int s, int o, - float extremum_thres, - float edge_ratio_thres, int img_padding_sz, - int refine_iterations) -> vector + float extremum_thres, float edge_ratio_thres, + int img_padding_sz, int refine_iterations) + -> vector { // #define PROFILE_ME -#ifdef PROFILE_ME +# ifdef PROFILE_ME auto timer = Timer{}; auto tic_ = [&timer]() { timer.restart(); }; auto toc_ = [&timer](const std::string& what) { const auto elapsed = timer.elapsed_ms(); SARA_DEBUG << "[" << what << "] " << elapsed << "ms\n"; }; -#endif +# endif const auto& w = I(s, o).width(); const auto& h = I(s, o).height(); @@ -429,14 +443,14 @@ namespace DO::Sara { // ======================================================================== // Refine the location of extrema. // ======================================================================== -#ifdef PROFILE_ME +# ifdef PROFILE_ME tic_(); -#endif +# endif auto location_refined = Image{I(s, o).sizes()}; auto extremum_value = Image{I(s, o).sizes()}; -#ifdef _OPENMP -# pragma omp parallel for -#endif +# ifdef _OPENMP +# pragma omp parallel for +# endif for (int xy = 0; xy < wh; ++xy) { const auto y = xy / w; @@ -459,25 +473,25 @@ namespace DO::Sara { refine_extremum(I, x, y, s, o, type, pos, val, img_padding_sz, refine_iterations); -#ifndef STRICT_LOCAL_EXTREMA +# ifndef STRICT_LOCAL_EXTREMA // Reject if contrast too low. if (std::abs(val) < extremum_thres) { // SARA_DEBUG << "Reject " << x << " " << y << std::endl; map(x, y) = 0; } -#endif +# endif } -#ifdef PROFILE_ME +# ifdef PROFILE_ME toc_("Calculating Location Residual"); -#endif +# endif // ======================================================================== // Fill the list of DoG extrema. // ======================================================================== -#ifdef PROFILE_ME +# ifdef PROFILE_ME tic_(); -#endif +# endif auto extrema = std::vector{}; extrema.reserve(10000); for (int xy = 0; xy < wh; ++xy) @@ -499,9 +513,9 @@ namespace DO::Sara { type == 1 ? OERegion::ExtremumType::Max : OERegion::ExtremumType::Min; extrema.push_back(dog); } -#ifdef PROFILE_ME +# ifdef PROFILE_ME toc_("Populating Extrema"); -#endif +# endif return extrema; } diff --git a/cpp/src/DO/Sara/FeatureDetectors/SIFT.cpp b/cpp/src/DO/Sara/FeatureDetectors/SIFT.cpp index 4f0cd12b2..68b451cdd 100644 --- a/cpp/src/DO/Sara/FeatureDetectors/SIFT.cpp +++ b/cpp/src/DO/Sara/FeatureDetectors/SIFT.cpp @@ -88,6 +88,7 @@ namespace DO::Sara { // 4. Rescale the feature position and scale $(x, y, \sigma)$ with the // octave scale. + SARA_LOGD(logger, "[Descriptors] {} keypoints to describe with SIFT", DoGs.size()); for (size_t i = 0; i != DoGs.size(); ++i) { const auto octave_scale_factor = static_cast( From c013fe3516a757aae3912609b0d517574aafe70e Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Sat, 1 Jun 2024 00:31:27 +0100 Subject: [PATCH 03/14] DOC: add note about the planar configuration which the DLT cannot solve. --- .../Sara/MultiViewGeometry/Resectioning/HartleyZisserman.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/cpp/src/DO/Sara/MultiViewGeometry/Resectioning/HartleyZisserman.hpp b/cpp/src/DO/Sara/MultiViewGeometry/Resectioning/HartleyZisserman.hpp index 7482c1eab..9e1923828 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/Resectioning/HartleyZisserman.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/Resectioning/HartleyZisserman.hpp @@ -19,6 +19,9 @@ namespace DO::Sara { + //! N.B.: this won't work if the scene points 'X' are lying on a plane. + //! This is a degenerate configuration the DLT method described by Hartley + //! cannot deal. template auto resectioning_hartley_zisserman(const TensorView_& X, const TensorView_& x) From 903c0a70ad65f9ba8c050968cc049d667c0a0f92 Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Sat, 1 Jun 2024 00:33:02 +0100 Subject: [PATCH 04/14] MAINT: simplify (and accelerate) the point cloud render. --- .../Renderer/ColoredPointCloudRenderer.cpp | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/cpp/src/DO/Kalpana/EasyGL/Renderer/ColoredPointCloudRenderer.cpp b/cpp/src/DO/Kalpana/EasyGL/Renderer/ColoredPointCloudRenderer.cpp index 9f779234c..da3410458 100644 --- a/cpp/src/DO/Kalpana/EasyGL/Renderer/ColoredPointCloudRenderer.cpp +++ b/cpp/src/DO/Kalpana/EasyGL/Renderer/ColoredPointCloudRenderer.cpp @@ -30,11 +30,16 @@ auto ColoredPointCloudRenderer::initialize() -> void void main() { - vec4 cam_coords = view * transform * vec4(in_coords, 1.0); + vec4 cam_coords = view * transform * vec4(in_coords, 1.0); gl_Position = projection * cam_coords; - float z2 = cam_coords.z * cam_coords.z; - float sigma2 = pow(150., 2.); // sigma = 150 meters - gl_PointSize = clamp(5. * point_size * exp(-z2 / sigma2), 2., 15.); + + // float z2 = cam_coords.z * cam_coords.z; + // float sigma2 = pow(150., 2.); // sigma = 150 meters + // gl_PointSize = clamp(5. * point_size * exp(-z2 / sigma2), 2., 15.); + + // Simplify the implementation... + gl_PointSize = point_size; + out_color = in_color; } )shader"; @@ -93,12 +98,10 @@ auto ColoredPointCloudRenderer::destroy() -> void _shader_program.clear(); } -auto ColoredPointCloudRenderer::render(const ColoredPointCloud& point_cloud, - const float point_size, - const Eigen::Matrix4f& transform, - const Eigen::Matrix4f& model_view, - const Eigen::Matrix4f& projection) - -> void +auto ColoredPointCloudRenderer::render( + const ColoredPointCloud& point_cloud, const float point_size, + const Eigen::Matrix4f& transform, const Eigen::Matrix4f& model_view, + const Eigen::Matrix4f& projection) -> void { _shader_program.use(); From 487995ebb7d662403d963e5de065d5e23ba008ec Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Sat, 1 Jun 2024 00:36:26 +0100 Subject: [PATCH 05/14] MAINT: clean up code. --- cpp/drafts/Calibration/HomographyDecomposition.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/cpp/drafts/Calibration/HomographyDecomposition.cpp b/cpp/drafts/Calibration/HomographyDecomposition.cpp index 85febe075..9bc87500d 100644 --- a/cpp/drafts/Calibration/HomographyDecomposition.cpp +++ b/cpp/drafts/Calibration/HomographyDecomposition.cpp @@ -28,7 +28,6 @@ auto decompose_H_RQ_factorization(const Eigen::Matrix3d& H, { const Eigen::Matrix3d invK = K.inverse(); const Eigen::Matrix3d P = (invK * H).normalized(); - std::cout << "P =\n" << P << std::endl; const Eigen::Vector3d n = Eigen::Vector3d::UnitZ(); @@ -49,9 +48,6 @@ auto decompose_H_RQ_factorization(const Eigen::Matrix3d& H, auto t = Eigen::Vector3d{}; t = P.col(2); - // SARA_DEBUG << "R1=\n" << R1 << std::endl; - // SARA_CHECK(R1.determinant()); - // And voila! Rs = {R1}; // ts = {t / std::pow(S(0) * S(1) * S(2), 1 / 3.)}; From 3d63805809d316ec78aa108a76f9d2986f8b42f7 Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Sun, 2 Jun 2024 00:00:21 +0100 Subject: [PATCH 06/14] MAINT: remove draft affine adaptation code. --- cpp/examples/Sara/FeatureDetectors/backup.cpp | 66 ------------------- 1 file changed, 66 deletions(-) delete mode 100644 cpp/examples/Sara/FeatureDetectors/backup.cpp diff --git a/cpp/examples/Sara/FeatureDetectors/backup.cpp b/cpp/examples/Sara/FeatureDetectors/backup.cpp deleted file mode 100644 index 971b83f41..000000000 --- a/cpp/examples/Sara/FeatureDetectors/backup.cpp +++ /dev/null @@ -1,66 +0,0 @@ -/* - Compute the affinity that maps the normalized patch to the local region - around feature $f$. - We denote a point in the normalized patch by $(u,v) \in [0,w]^2$ - The center point is $(w/2, w/2)$ corresponds to the center $(x_f, y_f)$ - of feature $f$. - - We introduce the notion of 'scale unit', i.e., - $1$ scale unit is equivalent $\sigma$ pixels in the image. - */ - /* - Let us set some important constants needed for the computation of the - normalized patch computation. - */ - // Patch "radius" - const int patchRadius = 20; - // Patch side length - const int patchSideLength = 2*patchRadius+1; - // Gaussian smoothing is involved in the computation of gradients orientations - // to compute dominant orientations and the SIFT descriptor. - const float gaussTruncFactor = 3.f; - // A normalized patch is composed of a grid of NxN square patches, i.e. bins, - // centered on the feature - const float binSideLength = 3.f; // side length of a bin in scale unit. - const float numBins = 4.f; - const float scaleRelRadius = sqrt(2.f)*binSideLength*(numBins+1)/2.f; - // Store the keypoints here. - vector keptFeats; - keptFeats.reserve(2*feats.size()); - for (size_t i = 0; i != feats.size(); ++i) - { - if (keepFeatures[i] == 1) - { - // The linear transform computed from the SVD - const Matrix2f& shapeMat = feats[i].shapeMat(); - JacobiSVD svd(shapeMat, ComputeFullU); - Vector2f S(svd.singularValues().cwiseInverse().cwiseSqrt()); - S *= scaleRelRadius/patchRadius; // Scaling - Matrix2f U(svd.matrixU()); // Rotation - Matrix2f L(U*S.asDiagonal()*U.transpose()); // Linear transform. - // The translation vector - Vector2f t(L*Point2f::Ones()*(-patchRadius) + feats[i].center()); - // The affinity that maps the patch to the local region around the feature - Matrix3f T(Matrix3f::Zero()); - T.block<2,2>(0,0) = L; - T.col(2) << t, 1.f; - - // Get the normalized patch. - Image normalizedPatch(patchSideLength,patchSideLength); - int s = scaleOctPairs[i](0); - int o = scaleOctPairs[i](1); - if (!warp(normalizedPatch, gaussPyr(s,o), T, 0.f, true)) - continue; - - - // Rescale the feature position and shape to the original image - // dimensions. - double fact = gaussPyr.octaveScalingFactor(o); - feats[i].shapeMat() *= pow(fact/**scaleRelRadius*/, -2); - feats[i].center() *= fact; - // Store the keypoint. - keptFeats.push_back(feats[i]); - } - } - if (verbose) - toc(); \ No newline at end of file From fb5494399220229971fd1334ac36b3016949c9bc Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Wed, 5 Jun 2024 11:50:16 +0100 Subject: [PATCH 07/14] ENH: improve the initialization of extrinsic parameter with a P3P solver. --- .../tool/calibrate_pinhole_camera.cpp | 128 ++++++++++++++---- .../visual_odometry_example.cpp | 2 +- .../MinimalSolvers/P3PSolver.hpp | 6 - 3 files changed, 101 insertions(+), 35 deletions(-) diff --git a/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp b/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp index 41f1c3989..28bc5f81f 100644 --- a/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp +++ b/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -74,7 +75,7 @@ inline auto inspect(sara::ImageView& image, // { for (auto x = 0; x < chessboard.width(); ++x) { - Eigen::Vector3d P = chessboard.scene_point(x, y).cast(); + auto P = chessboard.scene_point(x, y); P = R * P + t; const Eigen::Vector2f p1 = chessboard.image_point(x, y); @@ -93,6 +94,77 @@ inline auto inspect(sara::ImageView& image, // } +auto estimate_pose_with_p3p(const sara::ChessboardCorners& cb, + const Eigen::Matrix3d& K) + -> std::optional> +{ + auto points = Eigen::Matrix3d{}; + auto rays = Eigen::Matrix3d{}; + + const Eigen::Matrix3d K_inv = K.inverse(); + + SARA_DEBUG << "Filling points and rays for P3P..." << std::endl; + auto xs = std::array{0, 1, 0}; + auto ys = std::array{0, 0, 1}; + for (auto n = 0; n < 3; ++n) + { + const auto& x = xs[n]; + const auto& y = ys[n]; + const Eigen::Vector3d xn = + cb.image_point(x, y).homogeneous().cast(); + if (sara::is_nan(xn)) + continue; + + points.col(n) = cb.scene_point(x, y); + rays.col(n) = (K_inv * xn).normalized(); + } + if (sara::is_nan(points) || sara::is_nan(rays)) + return std::nullopt; + + SARA_DEBUG << "Solving P3P..." << std::endl; + SARA_DEBUG << "Points =\n" << points << std::endl; + SARA_DEBUG << "Rays =\n" << rays << std::endl; + const auto poses = sara::solve_p3p(points, rays); + if (poses.empty()) + return std::nullopt; + + // Find the best poses. + SARA_DEBUG << "Find best pose..." << std::endl; + auto errors = std::vector{}; + for (const auto& pose : poses) + { + auto error = 0; + + auto n = 0; + for (auto y = 0; y < cb.height(); ++y) + { + for (auto x = 0; x < cb.width(); ++x) + { + auto x0 = cb.image_point(x, y); + if (sara::is_nan(x0)) + continue; + + const auto& R = pose.topLeftCorner<3, 3>(); + const auto& t = pose.col(3); + + const Eigen::Vector2f x1 = + (K * (R * cb.scene_point(x, y) + t)).hnormalized().cast(); + error += (x1 - x0).squaredNorm(); + ++n; + } + } + + errors.emplace_back(error / n); + } + + const auto best_pose_index = + std::min_element(errors.begin(), errors.end()) - errors.begin(); + const auto& best_pose = poses[best_pose_index]; + SARA_DEBUG << "Best pose:\n" << best_pose << std::endl; + return best_pose; +} + + class ChessboardCalibrationData { public: @@ -152,27 +224,27 @@ class ChessboardCalibrationData _num_points_at_image.push_back(num_points); } - inline auto obs_2d() const -> const double* + auto obs_2d() const -> const double* { return _observations_2d.data(); } - inline auto obs_3d() const -> const double* + auto obs_3d() const -> const double* { return _observations_3d.data(); } - inline auto mutable_intrinsics() -> double* + auto mutable_intrinsics() -> double* { return _intrinsics.data.data(); } - inline auto mutable_extrinsics(int n) -> double* + auto mutable_extrinsics(int n) -> double* { return _extrinsics.data() + extrinsic_parameter_count * n; } - inline auto rotation(int n) const -> Eigen::AngleAxisd + auto rotation(int n) const -> Eigen::AngleAxisd { auto x = _extrinsics[extrinsic_parameter_count * n + 0]; auto y = _extrinsics[extrinsic_parameter_count * n + 1]; @@ -185,7 +257,7 @@ class ChessboardCalibrationData return {angle, Eigen::Vector3d{x, y, z}}; } - inline auto translation(int n) const -> Eigen::Vector3d + auto translation(int n) const -> Eigen::Vector3d { auto x = _extrinsics[extrinsic_parameter_count * n + 3 + 0]; auto y = _extrinsics[extrinsic_parameter_count * n + 3 + 1]; @@ -269,7 +341,7 @@ class ChessboardCalibrationData camera.v0() = v0; } - inline auto reinitialize_extrinsic_parameters() -> void + auto reinitialize_extrinsic_parameters() -> void { throw std::runtime_error{"Implementation incomplete!"}; @@ -374,20 +446,7 @@ auto sara_graphics_main(int argc, char** argv) -> int auto chessboards = std::vector{}; // Initialize the calibration matrix. -#if 0 const auto K_initial = init_calibration_matrix(frame.width(), frame.height()); -#else /* bootstrap manually */ - auto K_initial = Eigen::Matrix3d{}; - static constexpr auto f = 3228.8689050563653; // 3229.074544798197 - static constexpr auto u0 = 1080.; - static constexpr auto v0 = 1920.; - // clang-format off - K_initial << - f, 0, u0, - 0, f, v0, - 0, 0, 1; - // clang-format on -#endif // Initialize the calibration problem. auto calibration_data = ChessboardCalibrationData{}; @@ -422,15 +481,20 @@ auto sara_graphics_main(int argc, char** argv) -> int auto ts = std::vector{}; auto ns = std::vector{}; -#define USE_QR_FACTORIZATION -#ifdef USE_QR_FACTORIZATION - // This simple approach gives the best results. +// #define USE_RQ_FACTORIZATION +#ifdef USE_RQ_FACTORIZATION const Eigen::Matrix3d H = estimate_H(chessboard).normalized(); decompose_H_RQ_factorization(H, K_initial, Rs, ts, ns); #else - Rs = {Eigen::Matrix3d::Identity()}; - ts = {Eigen::Vector3d::Zero()}; - ns = {Eigen::Vector3d::Zero()}; + const auto pose = estimate_pose_with_p3p(chessboard, K_initial); + if (pose == std::nullopt || sara::is_nan(*pose)) + continue; + Rs.resize(1); + ts.resize(1); + ns.resize(1); + Rs[0] = pose->topLeftCorner<3, 3>(); + ts[0] = pose->col(3); + ns[0] = Eigen::Vector3d::Zero(); #endif SARA_DEBUG << "\nR =\n" << Rs[0] << std::endl; @@ -439,7 +503,15 @@ auto sara_graphics_main(int argc, char** argv) -> int calibration_data.add(chessboard, Rs[0], ts[0]); - inspect(frame_copy, chessboard, K_initial, Rs[0], ts[0]); + // inspect(frame_copy, chessboard, K_initial, Rs[0], ts[0]); + auto camera = sara::v2::PinholeCamera(); + camera.fx() = K_initial(0, 0); + camera.fy() = K_initial(1, 1); + camera.shear() = K_initial(0, 1); + camera.u0() = K_initial(0, 2); + camera.v0() = K_initial(1, 2); + inspect(frame_copy, chessboard, camera, Rs[0], ts[0]); + sara::draw_text(frame_copy, 80, 80, "Chessboard: FOUND!", sara::White8, 60, 0, false, true); sara::display(frame_copy); diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp index 25267a88f..e51e1e66e 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp @@ -563,7 +563,7 @@ auto main([[maybe_unused]] int const argc, [[maybe_unused]] char** const argv) #if defined(USE_HARDCODED_VIDEO_PATH) && defined(__APPLE__) const auto video_path = // fs::path{"/Users/oddkiva/Desktop/datasets/odometry/field.mp4"}; - fs::path{"/Users/oddkiva/Downloads/IMG_7966.MOV"}; + fs::path{"/Users/oddkiva/Desktop/datasets/oddkiva/food/IMG_8023.MOV"}; if (!fs::exists(video_path)) { fmt::print("Video {} does not exist", video_path.string()); diff --git a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp index a2144512b..34754fae3 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp @@ -106,8 +106,6 @@ namespace DO::Sara { "The dimension of scene points is incorrect. " "They must either 3D (Euclidean) or 4D (homogeneous)!"}; -#define PROJECT_TO_IMAGE_PLANE // And we should... -#if defined(PROJECT_TO_IMAGE_PLANE) // Project the camera coordinates to the image plane. // // The result is a list of pixel coordinates. @@ -125,10 +123,6 @@ namespace DO::Sara { auto u2 = Eigen::MatrixXd{2, rays.cols()}; for (auto i = 0; i < u2.cols(); ++i) u2.col(i) = C->project(rays.col(i)); -#else - const Eigen::MatrixXd u1 = X_camera.colwise().hnormalized(); - const Eigen::MatrixXd u2 = rays.colwise().hnormalized(); -#endif // Check the cheirality w.r.t. the candidate pose. const auto cheiral = X_camera.row(2).array() > 0; From 92e54eed3927ee94d704c667d3518ab1d3cf3df3 Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Wed, 5 Jun 2024 11:57:32 +0100 Subject: [PATCH 08/14] MAINT: clean up code. --- .../tool/calibrate_pinhole_camera.cpp | 38 ++++++------------- 1 file changed, 12 insertions(+), 26 deletions(-) diff --git a/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp b/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp index 28bc5f81f..5b1e96816 100644 --- a/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp +++ b/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp @@ -29,7 +29,7 @@ namespace sara = DO::Sara; -// This seems to work well... +// This works quite well in my experience. static inline auto init_calibration_matrix(int w, int h) -> Eigen::Matrix3d { const auto d = static_cast(std::max(w, h)); @@ -82,8 +82,8 @@ inline auto inspect(sara::ImageView& image, // const Eigen::Vector2f p2 = camera.project(P).cast(); if (!sara::is_nan(p1)) - draw_circle(image, p1, 3.f, sara::Cyan8, 3); - draw_circle(image, p2, 3.f, sara::Magenta8, 3); + sara::draw_circle(image, p1, 3.f, sara::Cyan8, 3); + sara::draw_circle(image, p2, 3.f, sara::Magenta8, 3); if (pause) { sara::display(image); @@ -129,7 +129,7 @@ auto estimate_pose_with_p3p(const sara::ChessboardCorners& cb, return std::nullopt; // Find the best poses. - SARA_DEBUG << "Find best pose..." << std::endl; + SARA_DEBUG << "Determining the best pose..." << std::endl; auto errors = std::vector{}; for (const auto& pose : poses) { @@ -161,6 +161,7 @@ auto estimate_pose_with_p3p(const sara::ChessboardCorners& cb, std::min_element(errors.begin(), errors.end()) - errors.begin(); const auto& best_pose = poses[best_pose_index]; SARA_DEBUG << "Best pose:\n" << best_pose << std::endl; + return best_pose; } @@ -477,31 +478,16 @@ auto sara_graphics_main(int argc, char** argv) -> int auto frame_copy = sara::Image{frame}; draw_chessboard(frame_copy, chessboard); - auto Rs = std::vector{}; - auto ts = std::vector{}; - auto ns = std::vector{}; - -// #define USE_RQ_FACTORIZATION -#ifdef USE_RQ_FACTORIZATION - const Eigen::Matrix3d H = estimate_H(chessboard).normalized(); - decompose_H_RQ_factorization(H, K_initial, Rs, ts, ns); -#else const auto pose = estimate_pose_with_p3p(chessboard, K_initial); if (pose == std::nullopt || sara::is_nan(*pose)) continue; - Rs.resize(1); - ts.resize(1); - ns.resize(1); - Rs[0] = pose->topLeftCorner<3, 3>(); - ts[0] = pose->col(3); - ns[0] = Eigen::Vector3d::Zero(); -#endif - - SARA_DEBUG << "\nR =\n" << Rs[0] << std::endl; - SARA_DEBUG << "\nt =\n" << ts[0] << std::endl; - SARA_DEBUG << "\nn =\n" << ns[0] << std::endl; - - calibration_data.add(chessboard, Rs[0], ts[0]); + + const Eigen::Matrix3d R = pose->topLeftCorner<3, 3>(); + const Eigen::Vector3d t = pose->col(3); + SARA_DEBUG << "\nR =\n" << R << std::endl; + SARA_DEBUG << "\nt =\n" << t << std::endl; + + calibration_data.add(chessboard, R, t); // inspect(frame_copy, chessboard, K_initial, Rs[0], ts[0]); auto camera = sara::v2::PinholeCamera(); From 6d407d1d0524c4c2bcaeeadf2c81ac2e9fc1e087 Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Wed, 5 Jun 2024 17:04:56 +0100 Subject: [PATCH 09/14] MAINT: eliminate hard-coded value. --- .../visual_odometry_example.cpp | 33 ++++++++++++++----- .../DO/Sara/SfM/Odometry/OdometryPipeline.cpp | 1 - .../iphone-12-mini-1440p-1x.json | 14 ++++++++ 3 files changed, 39 insertions(+), 9 deletions(-) create mode 100644 data/camera-parameters/iphone-12-mini-1440p-1x.json diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp index e51e1e66e..7b648755d 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp @@ -256,10 +256,12 @@ class SingleWindowApp } auto set_config(const fs::path& video_path, - const sara::v2::BrownConradyDistortionModel& camera) + const sara::v2::BrownConradyDistortionModel& camera, + const int num_frames_to_skip = 4) -> void { _pipeline.set_config(video_path, camera); + _pipeline._video_streamer.set_num_skips(num_frames_to_skip); init_gl_resources(); } @@ -563,7 +565,8 @@ auto main([[maybe_unused]] int const argc, [[maybe_unused]] char** const argv) #if defined(USE_HARDCODED_VIDEO_PATH) && defined(__APPLE__) const auto video_path = // fs::path{"/Users/oddkiva/Desktop/datasets/odometry/field.mp4"}; - fs::path{"/Users/oddkiva/Desktop/datasets/oddkiva/food/IMG_8023.MOV"}; + // fs::path{"/Users/oddkiva/Desktop/datasets/oddkiva/food/IMG_8023.MOV"}; + fs::path{"/Users/oddkiva/Desktop/datasets/oddkiva/cambodia/oudong/IMG_4230.MOV"}; if (!fs::exists(video_path)) { fmt::print("Video {} does not exist", video_path.string()); @@ -579,6 +582,7 @@ auto main([[maybe_unused]] int const argc, [[maybe_unused]] char** const argv) const auto video_path = fs::path{argv[1]}; #endif + auto num_frames_to_skip = 0; auto camera = sara::v2::BrownConradyDistortionModel{}; { #if !defined(__APPLE__) @@ -596,21 +600,34 @@ auto main([[maybe_unused]] int const argc, [[maybe_unused]] char** const argv) -0.0003137658969742134, +0.00021943576376532096; // clang-format on + num_frames_to_skip = 4; #else // iPhone 12 mini 4K - 1x - camera.fx() = 3229.074544798197; - camera.fy() = 3229.074544798197; + // camera.fx() = 3229.074544798197; + // camera.fy() = 3229.074544798197; + // camera.shear() = 0.; + // camera.u0() = 1080.; + // camera.v0() = 1920.; + // camera.k().setZero(); + // camera.p().setZero(); + // num_frames_to_skip = 9; + + // iPhone 12 mini 1440p - 1x + camera.fx() = 1618.2896144891963; + camera.fy() = 1618.2896144891963; camera.shear() = 0.; - camera.u0() = 1080.; - camera.v0() = 1920.; + camera.u0() = 720; + camera.v0() = 960; camera.k().setZero(); camera.p().setZero(); + num_frames_to_skip = 14; #endif } try { - auto app = SingleWindowApp{{800, 600}, "Odometry: " + video_path.string()}; - app.set_config(video_path, camera); + const auto app_title = "Odometry: " + video_path.string(); + auto app = SingleWindowApp{{800, 600}, app_title}; + app.set_config(video_path, camera, num_frames_to_skip); app.run(); } catch (std::exception& e) diff --git a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp b/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp index 6f5021fba..4398f6f3e 100644 --- a/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp +++ b/cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp @@ -32,7 +32,6 @@ auto OdometryPipeline::set_config( { // Build the dependency graph. _video_streamer.open(video_path); - _video_streamer.set_num_skips(4); // The original camera. _camera = camera; // The virtual camera for the undistorted image. diff --git a/data/camera-parameters/iphone-12-mini-1440p-1x.json b/data/camera-parameters/iphone-12-mini-1440p-1x.json new file mode 100644 index 000000000..24dce295b --- /dev/null +++ b/data/camera-parameters/iphone-12-mini-1440p-1x.json @@ -0,0 +1,14 @@ +{ + "model": "iPhone 12 mini", + "mode": "1440p", + "zoom": 1, + "focal_length": { + "x": 1618.2896144891963, + "y": 1618.2896144891963 + }, + "shear": 0, + "principal_point": { + "x": 720, + "y": 960 + } +} From 3bb60aabcc5c7c6dc5ad6eab2d978f24aea72f6c Mon Sep 17 00:00:00 2001 From: Odd Kiva <2375733-oddkiva@users.noreply.gitlab.com> Date: Thu, 6 Jun 2024 20:08:18 +0100 Subject: [PATCH 10/14] MAINT: review calibration code. --- .../calibrate_omnidirectional_cameras.cpp | 121 ++++++++++++++---- .../tool/calibrate_pinhole_camera.cpp | 100 +++++++-------- .../examples/detect_corners.cpp | 6 +- .../visual_odometry_example.cpp | 2 +- 4 files changed, 147 insertions(+), 82 deletions(-) diff --git a/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp b/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp index d8d7754d9..3ff7fc428 100644 --- a/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp +++ b/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -44,6 +45,76 @@ static inline auto init_K(int w, int h) -> Eigen::Matrix3d return K; } +static auto estimate_pose_with_p3p(const sara::ChessboardCorners& cb, + const sara::OmnidirectionalCamera& K) + -> std::optional> +{ + auto points = Eigen::Matrix3d{}; + auto rays = Eigen::Matrix3d{}; + + SARA_DEBUG << "Filling points and rays for P3P..." << std::endl; + auto xs = std::array{0, 1, 0}; + auto ys = std::array{0, 0, 1}; + for (auto n = 0; n < 3; ++n) + { + const auto& x = xs[n]; + const auto& y = ys[n]; + const Eigen::Vector2d xn = cb.image_point(x, y).cast(); + if (sara::is_nan(xn)) + continue; + + points.col(n) = cb.scene_point(x, y); + rays.col(n) = K.backproject(xn).normalized(); + } + if (sara::is_nan(points) || sara::is_nan(rays)) + return std::nullopt; + + SARA_DEBUG << "Solving P3P..." << std::endl; + SARA_DEBUG << "Points =\n" << points << std::endl; + SARA_DEBUG << "Rays =\n" << rays << std::endl; + const auto poses = sara::solve_p3p(points, rays); + if (poses.empty()) + return std::nullopt; + + // Find the best poses. + SARA_DEBUG << "Determining the best pose..." << std::endl; + auto errors = std::vector{}; + for (const auto& pose : poses) + { + auto error = 0; + + auto n = 0; + for (auto y = 0; y < cb.height(); ++y) + { + for (auto x = 0; x < cb.width(); ++x) + { + auto x0 = cb.image_point(x, y); + if (sara::is_nan(x0)) + continue; + + const auto& R = pose.topLeftCorner<3, 3>(); + const auto& t = pose.col(3); + const auto X = (R * cb.scene_point(x, y) + t); + + + const Eigen::Vector2f x1 = K.project(X).cast(); + error += (x1 - x0).squaredNorm(); + ++n; + } + } + + errors.emplace_back(error / n); + } + + const auto best_pose_index = + std::min_element(errors.begin(), errors.end()) - errors.begin(); + const auto& best_pose = poses[best_pose_index]; + SARA_DEBUG << "Best pose:\n" << best_pose << std::endl; + + return best_pose; +} + + inline auto inspect(sara::ImageView& image, // const sara::ChessboardCorners& chessboard, // const sara::OmnidirectionalCamera& camera, // @@ -51,13 +122,15 @@ inline auto inspect(sara::ImageView& image, // const Eigen::Vector3d& t, // bool pause = false) -> void { - const auto o = chessboard.image_point(0, 0); - const auto i = chessboard.image_point(1, 0); - const auto j = chessboard.image_point(0, 1); const auto s = chessboard.square_size().value; - + const Eigen::Vector3d& o3 = t; + const Eigen::Vector3d i3 = R * Eigen::Vector3d::UnitX() * s + t; + const Eigen::Vector3d j3 = R * Eigen::Vector3d::UnitY() * s + t; const Eigen::Vector3d k3 = R * Eigen::Vector3d::UnitZ() * s + t; + const Eigen::Vector2f o = camera.project(o3).cast(); + const Eigen::Vector2f i = camera.project(i3).cast(); + const Eigen::Vector2f j = camera.project(j3).cast(); const Eigen::Vector2f k = camera.project(k3).cast(); static const auto red = sara::Rgb8{167, 0, 0}; @@ -76,7 +149,8 @@ inline auto inspect(sara::ImageView& image, // const Eigen::Vector2f p1 = chessboard.image_point(x, y); const Eigen::Vector2f p2 = camera.project(P).cast(); - draw_circle(image, p1, 3.f, sara::Cyan8, 3); + if (!sara::is_nan(p1)) + sara::draw_circle(image, p1, 3.f, sara::Cyan8, 3); draw_circle(image, p2, 3.f, sara::Magenta8, 3); if (pause) { @@ -117,7 +191,7 @@ class ChessboardCalibrationProblem _intrinsics[0] = K(0, 0); // fy _intrinsics[1] = K(1, 1); - // shear + // shear (NORMALIZED) _intrinsics[2] = K(0, 1) / K(0, 0); // u0 _intrinsics[3] = K(0, 2); @@ -399,7 +473,7 @@ class ChessboardCalibrationProblem }; -int __main(int argc, char** argv) +auto sara_graphics_main(int argc, char** argv) -> int { if (argc < 5) { @@ -481,31 +555,22 @@ int __main(int argc, char** argv) auto frame_copy = sara::Image{frame}; draw_chessboard(frame_copy, chessboard); - auto Rs = std::vector{}; - auto ts = std::vector{}; - auto ns = std::vector{}; - -// #define USE_QR_FACTORIZATION -#ifdef USE_QR_FACTORIZATION - // This simple approach gives the best results. - const Eigen::Matrix3d H = estimate_H(chessboard).normalized(); - decompose_H_RQ_factorization(H, camera.K, Rs, ts, ns); -#else - Rs = {Eigen::Matrix3d::Identity()}; - ts = {Eigen::Vector3d::Zero()}; - ns = {Eigen::Vector3d::Zero()}; -#endif + const auto pose = estimate_pose_with_p3p(chessboard, camera); + if (pose == std::nullopt || sara::is_nan(*pose)) + continue; - SARA_DEBUG << "\nR =\n" << Rs[0] << std::endl; - SARA_DEBUG << "\nt =\n" << ts[0] << std::endl; - SARA_DEBUG << "\nn =\n" << ns[0] << std::endl; + const Eigen::Matrix3d R = pose->topLeftCorner<3, 3>(); + const Eigen::Vector3d t = pose->col(3); + SARA_DEBUG << "\nR =\n" << R << std::endl; + SARA_DEBUG << "\nt =\n" << t << std::endl; - calibration_problem.add(chessboard, Rs[0], ts[0]); + calibration_problem.add(chessboard, R, t); - inspect(frame_copy, chessboard, camera.K, Rs[0], ts[0]); + inspect(frame_copy, chessboard, camera, R, t); sara::draw_text(frame_copy, 80, 80, "Chessboard: FOUND!", sara::White8, 60, 0, false, true); sara::display(frame_copy); + sara::get_key(); selected_frames.emplace_back(video_stream.frame()); chessboards.emplace_back(std::move(chessboard)); @@ -583,7 +648,7 @@ int __main(int argc, char** argv) auto main(int argc, char** argv) -> int { - DO::Sara::GraphicsApplication app(argc, argv); - app.register_user_main(__main); + auto app = sara::GraphicsApplication{argc, argv}; + app.register_user_main(sara_graphics_main); return app.exec(); } diff --git a/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp b/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp index 5b1e96816..352c84d51 100644 --- a/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp +++ b/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp @@ -46,56 +46,8 @@ static inline auto init_calibration_matrix(int w, int h) -> Eigen::Matrix3d return K; } -inline auto inspect(sara::ImageView& image, // - const sara::ChessboardCorners& chessboard, // - const sara::v2::PinholeCamera& camera, // - const Eigen::Matrix3d& R, // - const Eigen::Vector3d& t, // - bool pause = false) -> void -{ - const auto s = chessboard.square_size().value; - - // Draw the axes by projecting them onto the image plane. - const Eigen::Vector3d& o3 = t; - const Eigen::Vector3d i3 = R * Eigen::Vector3d::UnitX() * s + t; - const Eigen::Vector3d j3 = R * Eigen::Vector3d::UnitY() * s + t; - const Eigen::Vector3d k3 = R * Eigen::Vector3d::UnitZ() * s + t; - const Eigen::Vector2f o = camera.project(o3).cast(); - const Eigen::Vector2f i = camera.project(i3).cast(); - const Eigen::Vector2f j = camera.project(j3).cast(); - const Eigen::Vector2f k = camera.project(k3).cast(); - - static const auto red = sara::Rgb8{167, 0, 0}; - static const auto green = sara::Rgb8{89, 216, 26}; - sara::draw_arrow(image, o, i, red, 6); - sara::draw_arrow(image, o, j, green, 6); - sara::draw_arrow(image, o, k, sara::Cyan8, 6); - - for (auto y = 0; y < chessboard.height(); ++y) - { - for (auto x = 0; x < chessboard.width(); ++x) - { - auto P = chessboard.scene_point(x, y); - P = R * P + t; - - const Eigen::Vector2f p1 = chessboard.image_point(x, y); - const Eigen::Vector2f p2 = camera.project(P).cast(); - - if (!sara::is_nan(p1)) - sara::draw_circle(image, p1, 3.f, sara::Cyan8, 3); - sara::draw_circle(image, p2, 3.f, sara::Magenta8, 3); - if (pause) - { - sara::display(image); - sara::get_key(); - } - } - } -} - - -auto estimate_pose_with_p3p(const sara::ChessboardCorners& cb, - const Eigen::Matrix3d& K) +static auto estimate_pose_with_p3p(const sara::ChessboardCorners& cb, + const Eigen::Matrix3d& K) -> std::optional> { auto points = Eigen::Matrix3d{}; @@ -166,6 +118,54 @@ auto estimate_pose_with_p3p(const sara::ChessboardCorners& cb, } +inline auto inspect(sara::ImageView& image, // + const sara::ChessboardCorners& chessboard, // + const sara::v2::PinholeCamera& camera, // + const Eigen::Matrix3d& R, // + const Eigen::Vector3d& t, // + bool pause = false) -> void +{ + const auto s = chessboard.square_size().value; + + // Draw the axes by projecting them onto the image plane. + const Eigen::Vector3d& o3 = t; + const Eigen::Vector3d i3 = R * Eigen::Vector3d::UnitX() * s + t; + const Eigen::Vector3d j3 = R * Eigen::Vector3d::UnitY() * s + t; + const Eigen::Vector3d k3 = R * Eigen::Vector3d::UnitZ() * s + t; + const Eigen::Vector2f o = camera.project(o3).cast(); + const Eigen::Vector2f i = camera.project(i3).cast(); + const Eigen::Vector2f j = camera.project(j3).cast(); + const Eigen::Vector2f k = camera.project(k3).cast(); + + static const auto red = sara::Rgb8{167, 0, 0}; + static const auto green = sara::Rgb8{89, 216, 26}; + sara::draw_arrow(image, o, i, red, 6); + sara::draw_arrow(image, o, j, green, 6); + sara::draw_arrow(image, o, k, sara::Cyan8, 6); + + for (auto y = 0; y < chessboard.height(); ++y) + { + for (auto x = 0; x < chessboard.width(); ++x) + { + auto P = chessboard.scene_point(x, y); + P = R * P + t; + + const Eigen::Vector2f p1 = chessboard.image_point(x, y); + const Eigen::Vector2f p2 = camera.project(P).cast(); + + if (!sara::is_nan(p1)) + sara::draw_circle(image, p1, 3.f, sara::Cyan8, 3); + sara::draw_circle(image, p2, 3.f, sara::Magenta8, 3); + if (pause) + { + sara::display(image); + sara::get_key(); + } + } + } +} + + class ChessboardCalibrationData { public: diff --git a/cpp/drafts/ChessboardDetection/examples/detect_corners.cpp b/cpp/drafts/ChessboardDetection/examples/detect_corners.cpp index 26144c380..9a0cd3f53 100644 --- a/cpp/drafts/ChessboardDetection/examples/detect_corners.cpp +++ b/cpp/drafts/ChessboardDetection/examples/detect_corners.cpp @@ -57,7 +57,7 @@ auto draw_corner(sara::ImageView& display, } -auto __main(int argc, char** argv) -> int +auto sara_graphics_main(int argc, char** argv) -> int { try { @@ -178,7 +178,7 @@ auto __main(int argc, char** argv) -> int auto main(int argc, char** argv) -> int { - DO::Sara::GraphicsApplication app(argc, argv); - app.register_user_main(__main); + auto app = sara::GraphicsApplication(argc, argv); + app.register_user_main(sara_graphics_main); return app.exec(); } diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp index 7b648755d..ed33ff086 100644 --- a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp +++ b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp @@ -198,7 +198,7 @@ struct MyPointCloudScene : kgl::PointCloudScene Eigen::Matrix4f _view = Eigen::Matrix4f::Identity(); Eigen::Matrix4f _model = Eigen::Matrix4f::Identity(); double _delta = (5._m).value; - double _angle_delta = (10._deg).value; + double _angle_delta = (20._deg).value; //! @} }; From 1ac765b30804d7243a2fd625d30b6495c231fee4 Mon Sep 17 00:00:00 2001 From: David OK Date: Thu, 6 Jun 2024 22:57:14 +0100 Subject: [PATCH 11/14] MAINT: fix compile error. --- cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp b/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp index 352c84d51..4726d77ee 100644 --- a/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp +++ b/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp @@ -496,8 +496,8 @@ auto sara_graphics_main(int argc, char** argv) -> int camera.shear() = K_initial(0, 1); camera.u0() = K_initial(0, 2); camera.v0() = K_initial(1, 2); - inspect(frame_copy, chessboard, camera, Rs[0], ts[0]); + inspect(frame_copy, chessboard, camera, R, t); sara::draw_text(frame_copy, 80, 80, "Chessboard: FOUND!", sara::White8, 60, 0, false, true); sara::display(frame_copy); From eb4c7732de068d5383886ecb145051b8283ef69f Mon Sep 17 00:00:00 2001 From: David OK Date: Thu, 6 Jun 2024 23:35:37 +0100 Subject: [PATCH 12/14] ENH: improve the calibration of omnidirectional cameras. --- .../Calibration/tool/calibrate_omnidirectional_cameras.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp b/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp index 3ff7fc428..a176bff1c 100644 --- a/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp +++ b/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp @@ -517,6 +517,7 @@ auto sara_graphics_main(int argc, char** argv) -> int // Initialize the calibration matrix. auto camera = sara::OmnidirectionalCamera{}; camera.K = init_K(frame.width(), frame.height()); + camera.K_inverse = camera.K.inverse(); camera.radial_distortion_coefficients.setZero(); camera.tangential_distortion_coefficients.setZero(); camera.xi = 1; @@ -567,10 +568,9 @@ auto sara_graphics_main(int argc, char** argv) -> int calibration_problem.add(chessboard, R, t); inspect(frame_copy, chessboard, camera, R, t); + sara::display(frame_copy); sara::draw_text(frame_copy, 80, 80, "Chessboard: FOUND!", sara::White8, 60, 0, false, true); - sara::display(frame_copy); - sara::get_key(); selected_frames.emplace_back(video_stream.frame()); chessboards.emplace_back(std::move(chessboard)); From cc01581d10e18520be8b8d14acfa45eacfa88724 Mon Sep 17 00:00:00 2001 From: David OK Date: Fri, 7 Jun 2024 00:08:33 +0100 Subject: [PATCH 13/14] ENH: improve the initialization method of the extrinsic parameters. My homography decomposition methods were implemented in a dodgy manner and without any real understanding of the maths. I simply replaced them with the robust P3P solver that we have at our disposal (Lambda-Twist). --- cpp/drafts/Calibration/CMakeLists.txt | 7 +- .../Calibration/HomographyDecomposition.cpp | 196 --------------- .../Calibration/HomographyDecomposition.hpp | 28 --- .../Calibration/HomographyEstimation.cpp | 109 --------- .../Calibration/HomographyEstimation.hpp | 24 -- cpp/drafts/Calibration/Utilities.hpp | 229 ++++++++++++++++++ .../calibrate_omnidirectional_cameras.cpp | 145 +---------- .../tool/calibrate_pinhole_camera.cpp | 146 +---------- 8 files changed, 240 insertions(+), 644 deletions(-) delete mode 100644 cpp/drafts/Calibration/HomographyDecomposition.cpp delete mode 100644 cpp/drafts/Calibration/HomographyDecomposition.hpp delete mode 100644 cpp/drafts/Calibration/HomographyEstimation.cpp delete mode 100644 cpp/drafts/Calibration/HomographyEstimation.hpp create mode 100644 cpp/drafts/Calibration/Utilities.hpp diff --git a/cpp/drafts/Calibration/CMakeLists.txt b/cpp/drafts/Calibration/CMakeLists.txt index 339eca636..30fcbf246 100644 --- a/cpp/drafts/Calibration/CMakeLists.txt +++ b/cpp/drafts/Calibration/CMakeLists.txt @@ -1,8 +1,7 @@ add_library( - DO_Sara_Calibration STATIC - Chessboard.hpp Chessboard.cpp # - HomographyDecomposition.cpp HomographyDecomposition.hpp # - HomographyEstimation.cpp HomographyEstimation.hpp) + DO_Sara_Calibration # + STATIC Chessboard.hpp Chessboard.cpp # + Utilities.hpp) target_link_libraries(DO_Sara_Calibration PUBLIC DO::Sara::Core DO::Sara::Graphics) set_property(TARGET DO_Sara_Calibration PROPERTY FOLDER "Libraries/Sara") diff --git a/cpp/drafts/Calibration/HomographyDecomposition.cpp b/cpp/drafts/Calibration/HomographyDecomposition.cpp deleted file mode 100644 index 9bc87500d..000000000 --- a/cpp/drafts/Calibration/HomographyDecomposition.cpp +++ /dev/null @@ -1,196 +0,0 @@ -// ========================================================================== // -// This file is part of Sara, a basic set of libraries in C++ for computer -// vision. -// -// Copyright (C) 2022-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, -// you can obtain one at http://mozilla.org/MPL/2.0/. -// ========================================================================== // - -//! @file - -#include "HomographyDecomposition.hpp" - -#include -#include - - -namespace sara = DO::Sara; - - -auto decompose_H_RQ_factorization(const Eigen::Matrix3d& H, - const Eigen::Matrix3d& K, - std::vector& Rs, - std::vector& ts, - std::vector& ns) -> void -{ - const Eigen::Matrix3d invK = K.inverse(); - const Eigen::Matrix3d P = (invK * H).normalized(); - - const Eigen::Vector3d n = Eigen::Vector3d::UnitZ(); - - auto R = Eigen::Matrix3d{}; - R.col(0) = P.col(0); - R.col(1) = P.col(1); - R.col(2) = R.col(0).cross(R.col(1)); - - // My trick is to use the RQ factorization described in Multiple View - // Geometry. - auto K1 = Eigen::Matrix3d{}; - auto R1 = Eigen::Matrix3d{}; - sara::rq_factorization_3x3(R, K1, R1); - - const auto svd = K1.jacobiSvd(); - const Eigen::Vector3d S = svd.singularValues(); - - auto t = Eigen::Vector3d{}; - t = P.col(2); - - // And voila! - Rs = {R1}; - // ts = {t / std::pow(S(0) * S(1) * S(2), 1 / 3.)}; - ts = {t / S(0)}; - ns = {n}; -} - -auto decompose_H_faugeras(const Eigen::Matrix3d& H, const Eigen::Matrix3d& K, - std::vector& Rs, - std::vector& ts, - std::vector& ns) -> void -{ - const Eigen::Matrix3d invK = K.inverse(); - const Eigen::Matrix3d P = invK * H; - - const auto svd = P.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV); - - Eigen::Vector3d S = svd.singularValues(); - - const Eigen::Matrix3d U = svd.matrixU(); - const Eigen::Matrix3d V = svd.matrixV(); - - Rs.clear(); - ts.clear(); - ns.clear(); - - const auto& d1 = S(0); - const auto& d2 = S(1); - const auto& d3 = S(2); - const auto detU_times_detV = U.determinant() * V.determinant(); - - const auto R_prime_when_d_prime_positive = - [d1, d2, d3](auto eps1, auto eps3) -> Eigen::Matrix3d { - const auto q1 = (sara::square(d1) - sara::square(d2)) * - (sara::square(d2) - sara::square(d3)); - const auto q2_inv = 1 / ((d1 + d3) * d2); - const auto q3 = sara::square(d2) + d1 * d3; - - const auto s = eps1 * eps3 * std::sqrt(q1) * q2_inv; - const auto c = q3 * q2_inv; - - auto R = Eigen::Matrix3d{}; - // clang-format off - R << c, 0, -s, - 0, 1, 0, - s, 0, c; - // clang-format on - - return R; - }; - - const auto t_prime_and_n_prime_when_d_prime_positive = - [d1, d2, d3, detU_times_detV]( - auto eps1, auto eps3) -> std::pair { - const auto q1 = sara::square(d1) - sara::square(d2); - const auto q2_inv = 1 / (sara::square(d1) - sara::square(d3)); - const auto q3 = sara::square(d2) - sara::square(d3); - - const auto x1 = eps1 * std::sqrt(q1 * q2_inv); - const auto x3 = eps3 * std::sqrt(q3 * q2_inv); - - const auto factor = d1 - d3; - const auto d_prime = -d2; - const auto d = detU_times_detV * d_prime; - - const auto t = Eigen::Vector3d{factor * x1 / d, 0, -factor * x3 / d}; - const auto n = Eigen::Vector3d{x1, 0, x3}; - - return std::make_pair(t, n); - }; - - const auto R_prime_when_d_prime_negative = - [d1, d2, d3](auto eps1, auto eps3) -> Eigen::Matrix3d { - // R' is a symmetry i.e. a rotation of angle π. - const auto q1 = (sara::square(d1) - sara::square(d2)) * - (sara::square(d2) - sara::square(d3)); - const auto q2_inv = 1 / ((d1 - d3) * d2); - const auto q3 = d1 * d3 - sara::square(d2); - - const auto s = eps1 * eps3 * std::sqrt(q1) * q2_inv; - const auto c = q3 * q2_inv; - - auto R = Eigen::Matrix3d{}; - // clang-format off - R << c, 0, s, - 0, 1, 0, - s, 0, -c; - // clang-format on - - return R; - }; - - const auto t_prime_and_n_prime_when_d_prime_negative = - [d1, d2, d3, detU_times_detV]( - auto eps1, auto eps3) -> std::pair { - const auto q1 = sara::square(d1) - sara::square(d2); - const auto q2_inv = 1 / (sara::square(d1) - sara::square(d3)); - const auto q3 = sara::square(d2) - sara::square(d3); - - const auto x1 = eps1 * q1 * q2_inv; - const auto x3 = eps3 * q3 * q2_inv; - - const auto factor = d1 + d3; - const auto d_prime = -d2; - const auto d = detU_times_detV * d_prime; - - const auto t = Eigen::Vector3d{factor * x1 / d, 0, factor * x3 / d}; - const auto n = Eigen::Vector3d{x1, 0, x3}; - - return std::make_pair(t, n); - }; - - const auto s = U.determinant() * V.determinant(); - - // That case should be rare in practice. - for (const auto& eps1 : {-1, 1}) - { - for (const auto& eps3 : {-1, 1}) - { - const Eigen::Matrix3d R = - s * U * R_prime_when_d_prime_positive(eps1, eps3) * V.transpose(); - Rs.push_back(R); - - const auto [t, n] = t_prime_and_n_prime_when_d_prime_positive(eps1, eps3); - ts.push_back(U * t); - ns.push_back(V * n); - } - } - - // This should be the case most of the time in practice. - for (const auto& eps1 : {-1, 1}) - { - for (const auto& eps3 : {-1, 1}) - { - SARA_CHECK(eps1); - SARA_CHECK(eps3); - const Eigen::Matrix3d R = - s * U * R_prime_when_d_prime_negative(eps1, eps3) * V.transpose(); - Rs.push_back(R); - - const auto [t, n] = t_prime_and_n_prime_when_d_prime_negative(eps1, eps3); - ts.push_back(U * t); - ns.push_back(V * n); - } - } -} diff --git a/cpp/drafts/Calibration/HomographyDecomposition.hpp b/cpp/drafts/Calibration/HomographyDecomposition.hpp deleted file mode 100644 index 134b16bbe..000000000 --- a/cpp/drafts/Calibration/HomographyDecomposition.hpp +++ /dev/null @@ -1,28 +0,0 @@ -// ========================================================================== // -// This file is part of Sara, a basic set of libraries in C++ for computer -// vision. -// -// Copyright (C) 2022-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, -// you can obtain one at http://mozilla.org/MPL/2.0/. -// ========================================================================== // - -//! @file - -#pragma once - -#include - - -auto decompose_H_RQ_factorization(const Eigen::Matrix3d& H, - const Eigen::Matrix3d& K, - std::vector& Rs, - std::vector& ts, - std::vector& ns) -> void; - -auto decompose_H_faugeras(const Eigen::Matrix3d& H, const Eigen::Matrix3d& K, - std::vector& Rs, - std::vector& ts, - std::vector& ns) -> void; diff --git a/cpp/drafts/Calibration/HomographyEstimation.cpp b/cpp/drafts/Calibration/HomographyEstimation.cpp deleted file mode 100644 index 832981773..000000000 --- a/cpp/drafts/Calibration/HomographyEstimation.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// ========================================================================== // -// This file is part of Sara, a basic set of libraries in C++ for computer -// vision. -// -// Copyright (C) 2022-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, -// you can obtain one at http://mozilla.org/MPL/2.0/. -// ========================================================================== // - -//! @file - -#include - - -// Use the direct linear transform method to estimate the homography. -auto estimate_H(const Eigen::MatrixXd& p1, const Eigen::MatrixXd& p2) - -> Eigen::Matrix3d -{ - const auto N = p1.cols(); - - // Form the data matrix used to determine H. - auto A = Eigen::MatrixXd{N * 2, 9}; - for (auto i = 0; i < N; ++i) - { - // The image point - const auto xi = p1.col(i); - const auto ui = xi(0); - const auto vi = xi(1); - - // The 3D coordinate on the chessboard plane. - static const auto zero = Eigen::RowVector3d::Zero(); - const auto yiT = p2.col(i).transpose(); - - A.row(2 * i + 0) << -yiT, zero, ui * yiT; - A.row(2 * i + 1) << zero, -yiT, vi * yiT; - } - - // SVD. - const auto svd = A.jacobiSvd(Eigen::ComputeFullV); - const auto V = svd.matrixV(); - const auto H_flat = V.col(8); - - // clang-format off - auto H = Eigen::Matrix3d{}; - H << H_flat.head(3).transpose(), - H_flat.segment(3, 3).transpose(), - H_flat.tail(3).transpose(); - // clang-format on - - return H; -} - -auto estimate_H(const DO::Sara::ChessboardCorners& chessboard) - -> Eigen::Matrix3d -{ - const auto w = chessboard.width(); - const auto h = chessboard.height(); - auto N = 0; - for (auto y = 0; y < h; ++y) - for (auto x = 0; x < w; ++x) - if (!DO::Sara::is_nan(chessboard.image_point(x, y))) - ++N; - - auto A = Eigen::MatrixXd{N * 2, 9}; - auto p1 = Eigen::MatrixXd{3, N}; - auto p2 = Eigen::MatrixXd{3, N}; - - // Collect the 2D-3D image point coordinates. - { - auto n = 0; - for (auto y = 0; y < h; ++y) - { - for (auto x = 0; x < w; ++x) - { - if (DO::Sara::is_nan(chessboard.image_point(x, y))) - continue; - - p1.col(n) = chessboard.image_point(x, y).homogeneous().cast(); - p2.col(n) = chessboard.scene_point(x, y).head(2).homogeneous(); - ++n; - } - } - } - - // Rescale the image point coordinates. - // - // For now, keep it simple by just dividing by 1000. Lazy but it works. - // - // clang-format off - const auto T = (Eigen::Matrix3d{} << - 1e-3, 0, 0, - 0, 1e-3, 0, - 0, 0, 1).finished(); - // clang-format on - const Eigen::Matrix3d invT = T.inverse(); - - // Now apply the normalizing transform on the image point coordinates. - p1 = T * p1; - - // Use the direct linear transform method to estimate the homography. - auto H = estimate_H(p1, p2); - - // Denormalize the homography. - H = invT * H; - - return H; -} diff --git a/cpp/drafts/Calibration/HomographyEstimation.hpp b/cpp/drafts/Calibration/HomographyEstimation.hpp deleted file mode 100644 index e754c8c49..000000000 --- a/cpp/drafts/Calibration/HomographyEstimation.hpp +++ /dev/null @@ -1,24 +0,0 @@ -// ========================================================================== // -// This file is part of Sara, a basic set of libraries in C++ for computer -// vision. -// -// Copyright (C) 2022-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, -// you can obtain one at http://mozilla.org/MPL/2.0/. -// ========================================================================== // - -//! @file - -#pragma once - -#include - - -//! @brief Estimate the homography using the Direct Linear Transform method. -auto estimate_H(const Eigen::MatrixXd& p1, const Eigen::MatrixXd& p2) - -> Eigen::Matrix3d; - -auto estimate_H(const DO::Sara::ChessboardCorners& chessboard) - -> Eigen::Matrix3d; diff --git a/cpp/drafts/Calibration/Utilities.hpp b/cpp/drafts/Calibration/Utilities.hpp new file mode 100644 index 000000000..113562118 --- /dev/null +++ b/cpp/drafts/Calibration/Utilities.hpp @@ -0,0 +1,229 @@ +// ========================================================================== // +// This file is part of Sara, a basic set of libraries in C++ for computer +// vision. +// +// 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, +// you can obtain one at http://mozilla.org/MPL/2.0/. +// ========================================================================== // + +#pragma once + +#include + +#include +#include +#include +#include + + +namespace DO::Sara { + + inline auto init_calibration_matrix(const int w, const int h) + -> Eigen::Matrix3d + { + const auto d = static_cast(std::max(w, h)); + const auto f = 0.5 * d; + + // clang-format off + const auto K = (Eigen::Matrix3d{} << + f, 0, w * 0.5, + 0, f, h * 0.5, + 0, 0, 1 + ).finished(); + // clang-format on + + return K; + } + + inline auto estimate_pose_with_p3p(const ChessboardCorners& cb, + const OmnidirectionalCamera& K) + -> std::optional> + { + auto points = Eigen::Matrix3d{}; + auto rays = Eigen::Matrix3d{}; + + SARA_DEBUG << "Filling points and rays for P3P..." << std::endl; + auto xs = std::array{0, 1, 0}; + auto ys = std::array{0, 0, 1}; + for (auto n = 0; n < 3; ++n) + { + const auto& x = xs[n]; + const auto& y = ys[n]; + const Eigen::Vector2d xn = cb.image_point(x, y).cast(); + if (is_nan(xn)) + continue; + + points.col(n) = cb.scene_point(x, y); + rays.col(n) = K.backproject(xn).normalized(); + } + if (is_nan(points) || is_nan(rays)) + return std::nullopt; + + SARA_DEBUG << "Solving P3P..." << std::endl; + SARA_DEBUG << "Points =\n" << points << std::endl; + SARA_DEBUG << "Rays =\n" << rays << std::endl; + const auto poses = solve_p3p(points, rays); + if (poses.empty()) + return std::nullopt; + + // Find the best poses. + SARA_DEBUG << "Determining the best pose..." << std::endl; + auto errors = std::vector{}; + for (const auto& pose : poses) + { + auto error = 0; + + auto n = 0; + for (auto y = 0; y < cb.height(); ++y) + { + for (auto x = 0; x < cb.width(); ++x) + { + auto x0 = cb.image_point(x, y); + if (is_nan(x0)) + continue; + + const auto& R = pose.topLeftCorner<3, 3>(); + const auto& t = pose.col(3); + const auto X = (R * cb.scene_point(x, y) + t); + + + const Eigen::Vector2f x1 = K.project(X).cast(); + error += (x1 - x0).squaredNorm(); + ++n; + } + } + + errors.emplace_back(error / n); + } + + const auto best_pose_index = + std::min_element(errors.begin(), errors.end()) - errors.begin(); + const auto& best_pose = poses[best_pose_index]; + SARA_DEBUG << "Best pose:\n" << best_pose << std::endl; + + return best_pose; + } + + inline auto estimate_pose_with_p3p(const ChessboardCorners& cb, + const Eigen::Matrix3d& K) + -> std::optional> + { + auto points = Eigen::Matrix3d{}; + auto rays = Eigen::Matrix3d{}; + + const Eigen::Matrix3d K_inv = K.inverse(); + + SARA_DEBUG << "Filling points and rays for P3P..." << std::endl; + auto xs = std::array{0, 1, 0}; + auto ys = std::array{0, 0, 1}; + for (auto n = 0; n < 3; ++n) + { + const auto& x = xs[n]; + const auto& y = ys[n]; + const Eigen::Vector3d xn = + cb.image_point(x, y).homogeneous().cast(); + if (is_nan(xn)) + continue; + + points.col(n) = cb.scene_point(x, y); + rays.col(n) = (K_inv * xn).normalized(); + } + if (is_nan(points) || is_nan(rays)) + return std::nullopt; + + SARA_DEBUG << "Solving P3P..." << std::endl; + SARA_DEBUG << "Points =\n" << points << std::endl; + SARA_DEBUG << "Rays =\n" << rays << std::endl; + const auto poses = solve_p3p(points, rays); + if (poses.empty()) + return std::nullopt; + + // Find the best poses. + SARA_DEBUG << "Determining the best pose..." << std::endl; + auto errors = std::vector{}; + for (const auto& pose : poses) + { + auto error = 0; + + auto n = 0; + for (auto y = 0; y < cb.height(); ++y) + { + for (auto x = 0; x < cb.width(); ++x) + { + auto x0 = cb.image_point(x, y); + if (is_nan(x0)) + continue; + + const auto& R = pose.topLeftCorner<3, 3>(); + const auto& t = pose.col(3); + + const Eigen::Vector2f x1 = + (K * (R * cb.scene_point(x, y) + t)).hnormalized().cast(); + error += (x1 - x0).squaredNorm(); + ++n; + } + } + + errors.emplace_back(error / n); + } + + const auto best_pose_index = + std::min_element(errors.begin(), errors.end()) - errors.begin(); + const auto& best_pose = poses[best_pose_index]; + SARA_DEBUG << "Best pose:\n" << best_pose << std::endl; + + return best_pose; + } + + + template + inline auto inspect(ImageView& image, // + const ChessboardCorners& chessboard, // + const CameraModel& camera, // + const Eigen::Matrix3d& R, // + const Eigen::Vector3d& t, // + bool pause = false) -> void + { + const auto s = chessboard.square_size().value; + + const Eigen::Vector3d& o3 = t; + const Eigen::Vector3d i3 = R * Eigen::Vector3d::UnitX() * s + t; + const Eigen::Vector3d j3 = R * Eigen::Vector3d::UnitY() * s + t; + const Eigen::Vector3d k3 = R * Eigen::Vector3d::UnitZ() * s + t; + const Eigen::Vector2f o = camera.project(o3).template cast(); + const Eigen::Vector2f i = camera.project(i3).template cast(); + const Eigen::Vector2f j = camera.project(j3).template cast(); + const Eigen::Vector2f k = camera.project(k3).template cast(); + + static const auto red = Rgb8{167, 0, 0}; + static const auto green = Rgb8{89, 216, 26}; + draw_arrow(image, o, i, red, 6); + draw_arrow(image, o, j, green, 6); + draw_arrow(image, o, k, Blue8, 6); + + for (auto y = 0; y < chessboard.height(); ++y) + { + for (auto x = 0; x < chessboard.width(); ++x) + { + Eigen::Vector3d P = chessboard.scene_point(x, y).cast(); + P = R * P + t; + + const Eigen::Vector2f p1 = chessboard.image_point(x, y); + const Eigen::Vector2f p2 = camera.project(P).template cast(); + + if (!is_nan(p1)) + draw_circle(image, p1, 3.f, Cyan8, 3); + draw_circle(image, p2, 3.f, Magenta8, 3); + if (pause) + { + display(image); + get_key(); + } + } + } + } + +} // namespace DO::Sara diff --git a/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp b/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp index a176bff1c..37f4f7749 100644 --- a/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp +++ b/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp @@ -11,157 +11,20 @@ //! @file +#include +#include + #include #include #include #include #include -#include -#include - -#include -#include -#include namespace sara = DO::Sara; -// This seems to work well... -static inline auto init_K(int w, int h) -> Eigen::Matrix3d -{ - const auto d = static_cast(std::max(w, h)); - const auto f = 0.5 * d; - - // clang-format off - const auto K = (Eigen::Matrix3d{} << - f, 0, w * 0.5, - 0, f, h * 0.5, - 0, 0, 1 - ).finished(); - // clang-format on - - return K; -} - -static auto estimate_pose_with_p3p(const sara::ChessboardCorners& cb, - const sara::OmnidirectionalCamera& K) - -> std::optional> -{ - auto points = Eigen::Matrix3d{}; - auto rays = Eigen::Matrix3d{}; - - SARA_DEBUG << "Filling points and rays for P3P..." << std::endl; - auto xs = std::array{0, 1, 0}; - auto ys = std::array{0, 0, 1}; - for (auto n = 0; n < 3; ++n) - { - const auto& x = xs[n]; - const auto& y = ys[n]; - const Eigen::Vector2d xn = cb.image_point(x, y).cast(); - if (sara::is_nan(xn)) - continue; - - points.col(n) = cb.scene_point(x, y); - rays.col(n) = K.backproject(xn).normalized(); - } - if (sara::is_nan(points) || sara::is_nan(rays)) - return std::nullopt; - - SARA_DEBUG << "Solving P3P..." << std::endl; - SARA_DEBUG << "Points =\n" << points << std::endl; - SARA_DEBUG << "Rays =\n" << rays << std::endl; - const auto poses = sara::solve_p3p(points, rays); - if (poses.empty()) - return std::nullopt; - - // Find the best poses. - SARA_DEBUG << "Determining the best pose..." << std::endl; - auto errors = std::vector{}; - for (const auto& pose : poses) - { - auto error = 0; - - auto n = 0; - for (auto y = 0; y < cb.height(); ++y) - { - for (auto x = 0; x < cb.width(); ++x) - { - auto x0 = cb.image_point(x, y); - if (sara::is_nan(x0)) - continue; - - const auto& R = pose.topLeftCorner<3, 3>(); - const auto& t = pose.col(3); - const auto X = (R * cb.scene_point(x, y) + t); - - - const Eigen::Vector2f x1 = K.project(X).cast(); - error += (x1 - x0).squaredNorm(); - ++n; - } - } - - errors.emplace_back(error / n); - } - - const auto best_pose_index = - std::min_element(errors.begin(), errors.end()) - errors.begin(); - const auto& best_pose = poses[best_pose_index]; - SARA_DEBUG << "Best pose:\n" << best_pose << std::endl; - - return best_pose; -} - - -inline auto inspect(sara::ImageView& image, // - const sara::ChessboardCorners& chessboard, // - const sara::OmnidirectionalCamera& camera, // - const Eigen::Matrix3d& R, // - const Eigen::Vector3d& t, // - bool pause = false) -> void -{ - const auto s = chessboard.square_size().value; - - const Eigen::Vector3d& o3 = t; - const Eigen::Vector3d i3 = R * Eigen::Vector3d::UnitX() * s + t; - const Eigen::Vector3d j3 = R * Eigen::Vector3d::UnitY() * s + t; - const Eigen::Vector3d k3 = R * Eigen::Vector3d::UnitZ() * s + t; - const Eigen::Vector2f o = camera.project(o3).cast(); - const Eigen::Vector2f i = camera.project(i3).cast(); - const Eigen::Vector2f j = camera.project(j3).cast(); - const Eigen::Vector2f k = camera.project(k3).cast(); - - static const auto red = sara::Rgb8{167, 0, 0}; - static const auto green = sara::Rgb8{89, 216, 26}; - sara::draw_arrow(image, o, i, red, 6); - sara::draw_arrow(image, o, j, green, 6); - sara::draw_arrow(image, o, k, sara::Blue8, 6); - - for (auto y = 0; y < chessboard.height(); ++y) - { - for (auto x = 0; x < chessboard.width(); ++x) - { - Eigen::Vector3d P = chessboard.scene_point(x, y).cast(); - P = R * P + t; - - const Eigen::Vector2f p1 = chessboard.image_point(x, y); - const Eigen::Vector2f p2 = camera.project(P).cast(); - - if (!sara::is_nan(p1)) - sara::draw_circle(image, p1, 3.f, sara::Cyan8, 3); - draw_circle(image, p2, 3.f, sara::Magenta8, 3); - if (pause) - { - sara::display(image); - sara::get_key(); - } - } - } -} - - class ChessboardCalibrationProblem { public: @@ -516,7 +379,7 @@ auto sara_graphics_main(int argc, char** argv) -> int // Initialize the calibration matrix. auto camera = sara::OmnidirectionalCamera{}; - camera.K = init_K(frame.width(), frame.height()); + camera.K = sara::init_calibration_matrix(frame.width(), frame.height()); camera.K_inverse = camera.K.inverse(); camera.radial_distortion_coefficients.setZero(); camera.tangential_distortion_coefficients.setZero(); diff --git a/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp b/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp index 4726d77ee..9b6bcceae 100644 --- a/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp +++ b/cpp/drafts/Calibration/tool/calibrate_pinhole_camera.cpp @@ -11,6 +11,8 @@ //! @file +#include + #include #include #include @@ -21,151 +23,10 @@ #include #include -#include -#include -#include - namespace sara = DO::Sara; -// This works quite well in my experience. -static inline auto init_calibration_matrix(int w, int h) -> Eigen::Matrix3d -{ - const auto d = static_cast(std::max(w, h)); - const auto f = 0.5 * d; - - // clang-format off - const auto K = (Eigen::Matrix3d{} << - f, 0, w * 0.5, - 0, f, h * 0.5, - 0, 0, 1 - ).finished(); - // clang-format on - - return K; -} - -static auto estimate_pose_with_p3p(const sara::ChessboardCorners& cb, - const Eigen::Matrix3d& K) - -> std::optional> -{ - auto points = Eigen::Matrix3d{}; - auto rays = Eigen::Matrix3d{}; - - const Eigen::Matrix3d K_inv = K.inverse(); - - SARA_DEBUG << "Filling points and rays for P3P..." << std::endl; - auto xs = std::array{0, 1, 0}; - auto ys = std::array{0, 0, 1}; - for (auto n = 0; n < 3; ++n) - { - const auto& x = xs[n]; - const auto& y = ys[n]; - const Eigen::Vector3d xn = - cb.image_point(x, y).homogeneous().cast(); - if (sara::is_nan(xn)) - continue; - - points.col(n) = cb.scene_point(x, y); - rays.col(n) = (K_inv * xn).normalized(); - } - if (sara::is_nan(points) || sara::is_nan(rays)) - return std::nullopt; - - SARA_DEBUG << "Solving P3P..." << std::endl; - SARA_DEBUG << "Points =\n" << points << std::endl; - SARA_DEBUG << "Rays =\n" << rays << std::endl; - const auto poses = sara::solve_p3p(points, rays); - if (poses.empty()) - return std::nullopt; - - // Find the best poses. - SARA_DEBUG << "Determining the best pose..." << std::endl; - auto errors = std::vector{}; - for (const auto& pose : poses) - { - auto error = 0; - - auto n = 0; - for (auto y = 0; y < cb.height(); ++y) - { - for (auto x = 0; x < cb.width(); ++x) - { - auto x0 = cb.image_point(x, y); - if (sara::is_nan(x0)) - continue; - - const auto& R = pose.topLeftCorner<3, 3>(); - const auto& t = pose.col(3); - - const Eigen::Vector2f x1 = - (K * (R * cb.scene_point(x, y) + t)).hnormalized().cast(); - error += (x1 - x0).squaredNorm(); - ++n; - } - } - - errors.emplace_back(error / n); - } - - const auto best_pose_index = - std::min_element(errors.begin(), errors.end()) - errors.begin(); - const auto& best_pose = poses[best_pose_index]; - SARA_DEBUG << "Best pose:\n" << best_pose << std::endl; - - return best_pose; -} - - -inline auto inspect(sara::ImageView& image, // - const sara::ChessboardCorners& chessboard, // - const sara::v2::PinholeCamera& camera, // - const Eigen::Matrix3d& R, // - const Eigen::Vector3d& t, // - bool pause = false) -> void -{ - const auto s = chessboard.square_size().value; - - // Draw the axes by projecting them onto the image plane. - const Eigen::Vector3d& o3 = t; - const Eigen::Vector3d i3 = R * Eigen::Vector3d::UnitX() * s + t; - const Eigen::Vector3d j3 = R * Eigen::Vector3d::UnitY() * s + t; - const Eigen::Vector3d k3 = R * Eigen::Vector3d::UnitZ() * s + t; - const Eigen::Vector2f o = camera.project(o3).cast(); - const Eigen::Vector2f i = camera.project(i3).cast(); - const Eigen::Vector2f j = camera.project(j3).cast(); - const Eigen::Vector2f k = camera.project(k3).cast(); - - static const auto red = sara::Rgb8{167, 0, 0}; - static const auto green = sara::Rgb8{89, 216, 26}; - sara::draw_arrow(image, o, i, red, 6); - sara::draw_arrow(image, o, j, green, 6); - sara::draw_arrow(image, o, k, sara::Cyan8, 6); - - for (auto y = 0; y < chessboard.height(); ++y) - { - for (auto x = 0; x < chessboard.width(); ++x) - { - auto P = chessboard.scene_point(x, y); - P = R * P + t; - - const Eigen::Vector2f p1 = chessboard.image_point(x, y); - const Eigen::Vector2f p2 = camera.project(P).cast(); - - if (!sara::is_nan(p1)) - sara::draw_circle(image, p1, 3.f, sara::Cyan8, 3); - sara::draw_circle(image, p2, 3.f, sara::Magenta8, 3); - if (pause) - { - sara::display(image); - sara::get_key(); - } - } - } -} - - class ChessboardCalibrationData { public: @@ -447,7 +308,8 @@ auto sara_graphics_main(int argc, char** argv) -> int auto chessboards = std::vector{}; // Initialize the calibration matrix. - const auto K_initial = init_calibration_matrix(frame.width(), frame.height()); + const auto K_initial = + sara::init_calibration_matrix(frame.width(), frame.height()); // Initialize the calibration problem. auto calibration_data = ChessboardCalibrationData{}; From 5443c72d33160d81b49c91aae006dc0c0bb86994 Mon Sep 17 00:00:00 2001 From: David OK Date: Fri, 7 Jun 2024 00:27:48 +0100 Subject: [PATCH 14/14] ENH: add calibration parameters for consumer fisheye cameras. - GOPRO 7 Hero Black, Superview mode - Insta ONE RS 360 --- .../gopro-7-hero-black-superview.json | 16 +++++++++++++ data/camera-parameters/insta-oners-360.json | 15 ++++++++++++ .../iphone-12-mini-1440p-1x.json | 24 +++++++++---------- .../iphone-12-mini-4K-1x.json | 24 +++++++++---------- .../iphone-12-mini-HD-1x.json | 24 +++++++++---------- 5 files changed, 67 insertions(+), 36 deletions(-) create mode 100644 data/camera-parameters/gopro-7-hero-black-superview.json create mode 100644 data/camera-parameters/insta-oners-360.json diff --git a/data/camera-parameters/gopro-7-hero-black-superview.json b/data/camera-parameters/gopro-7-hero-black-superview.json new file mode 100644 index 000000000..f62e46758 --- /dev/null +++ b/data/camera-parameters/gopro-7-hero-black-superview.json @@ -0,0 +1,16 @@ +{ + "model": "GOPRO 7 Hero Black", + "mode": "Superview", + "focal_length": { + "x": 1665.17, + "y": 1604.16 + }, + "shear": 1.43155, + "principal_point": { + "x": 1368.04, + "y": 771.516 + }, + "k": [0.0984142, -0.975655, 0], + "p": [-0.00120209, 0.00196812], + "xi": 0.765742 +} diff --git a/data/camera-parameters/insta-oners-360.json b/data/camera-parameters/insta-oners-360.json new file mode 100644 index 000000000..bb059ca38 --- /dev/null +++ b/data/camera-parameters/insta-oners-360.json @@ -0,0 +1,15 @@ +{ + "model": "Insta OneRS 360", + "focal_length": { + "x": 1241.79, + "y": 1242.7 + }, + "shear": 0.356642, + "principal_point": { + "x": 540.279, + "y": 959.875 + }, + "k": [0.307218, -0.0697542, 0], + "p": [-0.00115393, 0.000505966], + "xi": 1.36077 +} diff --git a/data/camera-parameters/iphone-12-mini-1440p-1x.json b/data/camera-parameters/iphone-12-mini-1440p-1x.json index 24dce295b..1d50b8a56 100644 --- a/data/camera-parameters/iphone-12-mini-1440p-1x.json +++ b/data/camera-parameters/iphone-12-mini-1440p-1x.json @@ -1,14 +1,14 @@ { - "model": "iPhone 12 mini", - "mode": "1440p", - "zoom": 1, - "focal_length": { - "x": 1618.2896144891963, - "y": 1618.2896144891963 - }, - "shear": 0, - "principal_point": { - "x": 720, - "y": 960 - } + "model": "iPhone 12 mini", + "mode": "1440p", + "zoom": 1, + "focal_length": { + "x": 1618.2896144891963, + "y": 1618.2896144891963 + }, + "shear": 0, + "principal_point": { + "x": 720, + "y": 960 + } } diff --git a/data/camera-parameters/iphone-12-mini-4K-1x.json b/data/camera-parameters/iphone-12-mini-4K-1x.json index e03f12ebc..8417f4d27 100644 --- a/data/camera-parameters/iphone-12-mini-4K-1x.json +++ b/data/camera-parameters/iphone-12-mini-4K-1x.json @@ -1,14 +1,14 @@ { - "model": "iPhone 12 mini", - "mode": "4K", - "zoom": 1, - "focal_length": { - "x": 3229.074544798197, - "y": 3229.074544798197 - }, - "shear": 0, - "principal_point": { - "x": 1080, - "y": 1920 - } + "model": "iPhone 12 mini", + "mode": "4K", + "zoom": 1, + "focal_length": { + "x": 3229.074544798197, + "y": 3229.074544798197 + }, + "shear": 0, + "principal_point": { + "x": 1080, + "y": 1920 + } } diff --git a/data/camera-parameters/iphone-12-mini-HD-1x.json b/data/camera-parameters/iphone-12-mini-HD-1x.json index b838139c2..b54f01259 100644 --- a/data/camera-parameters/iphone-12-mini-HD-1x.json +++ b/data/camera-parameters/iphone-12-mini-HD-1x.json @@ -1,14 +1,14 @@ { - "model": "iPhone 12 mini", - "mode": "HD", - "zoom": 1, - "focal_length": { - "x": 1649.2068306381805, - "y": 1649.2068306381805 - }, - "shear": 0, - "principal_point": { - "x": 540, - "y": 960 - } + "model": "iPhone 12 mini", + "mode": "HD", + "zoom": 1, + "focal_length": { + "x": 1649.2068306381805, + "y": 1649.2068306381805 + }, + "shear": 0, + "principal_point": { + "x": 540, + "y": 960 + } }