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 85febe075..000000000 --- a/cpp/drafts/Calibration/HomographyDecomposition.cpp +++ /dev/null @@ -1,200 +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(); - std::cout << "P =\n" << P << std::endl; - - 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); - - // 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.)}; - 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 d8d7754d9..37f4f7749 100644 --- a/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp +++ b/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp @@ -11,83 +11,20 @@ //! @file +#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; -} - -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 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 k3 = R * Eigen::Vector3d::UnitZ() * s + t; - 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(); - - 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: @@ -117,7 +54,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 +336,7 @@ class ChessboardCalibrationProblem }; -int __main(int argc, char** argv) +auto sara_graphics_main(int argc, char** argv) -> int { if (argc < 5) { @@ -442,7 +379,8 @@ int __main(int argc, char** argv) // 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(); camera.xi = 1; @@ -481,31 +419,21 @@ 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{}; + const auto pose = estimate_pose_with_p3p(chessboard, camera); + if (pose == std::nullopt || sara::is_nan(*pose)) + continue; -// #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 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; - 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_problem.add(chessboard, R, t); - calibration_problem.add(chessboard, Rs[0], ts[0]); - - inspect(frame_copy, chessboard, camera.K, Rs[0], ts[0]); + 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); selected_frames.emplace_back(video_stream.frame()); chessboards.emplace_back(std::move(chessboard)); @@ -583,7 +511,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 41f1c3989..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 @@ -19,80 +21,12 @@ #include #include #include - -#include -#include -#include +#include namespace sara = DO::Sara; -// This seems to work well... -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; -} - -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) - { - 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)) - 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 ChessboardCalibrationData { public: @@ -152,27 +86,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 +119,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 +203,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 +308,8 @@ 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 + const auto K_initial = + sara::init_calibration_matrix(frame.width(), frame.height()); // Initialize the calibration problem. auto calibration_data = ChessboardCalibrationData{}; @@ -418,28 +340,26 @@ 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{}; + const auto pose = estimate_pose_with_p3p(chessboard, K_initial); + if (pose == std::nullopt || sara::is_nan(*pose)) + continue; -#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, K_initial, Rs, ts, ns); -#else - Rs = {Eigen::Matrix3d::Identity()}; - ts = {Eigen::Vector3d::Zero()}; - ns = {Eigen::Vector3d::Zero()}; -#endif + 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; - 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, R, t); - calibration_data.add(chessboard, 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, K_initial, 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); 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/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 diff --git a/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp b/cpp/examples/Sara/MultiViewGeometry/visual_odometry_example.cpp index 6de80a40f..ed33ff086 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 { }; @@ -119,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; //! @} }; @@ -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() @@ -175,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(); } @@ -324,8 +407,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 +470,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 +537,8 @@ class SingleWindowApp //! @brief Point cloud rendering. MyPointCloudScene _pc_scene; + UserInteractionResponder _responder; + //! @brief Our engine. sara::OdometryPipeline _pipeline; @@ -449,8 +552,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(); @@ -462,7 +565,8 @@ auto main([[maybe_unused]] int const argc, #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"}; + 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()); @@ -478,6 +582,7 @@ auto main([[maybe_unused]] int const argc, const auto video_path = fs::path{argv[1]}; #endif + auto num_frames_to_skip = 0; auto camera = sara::v2::BrownConradyDistortionModel{}; { #if !defined(__APPLE__) @@ -495,21 +600,34 @@ auto main([[maybe_unused]] int const argc, -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/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(); 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( 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; 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) 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/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 new file mode 100644 index 000000000..1d50b8a56 --- /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 + } +} 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 + } }