diff --git a/HomLib/CMakeLists.txt b/HomLib/CMakeLists.txt index 4e086f6..1d818e8 100644 --- a/HomLib/CMakeLists.txt +++ b/HomLib/CMakeLists.txt @@ -1,10 +1,12 @@ set(SRCS + helpers/affine2sift.cpp helpers/generate_problem_instance.cpp helpers/gj.cpp helpers/normalize2dpts.cpp helpers/radial.cpp helpers/roots.cpp robust/refinement.cpp + solvers/barath_visapp_2016/get_barath_visapp_2016_affine.cpp solvers/fitzgibbon_cvpr_2001/get_fitzgibbon_cvpr_2001.cpp solvers/fitzgibbon_cvpr_2001/get_fitzgibbon_cvpr_2001_single.cpp solvers/kukelova_cvpr_2015/get_kukelova_cvpr_2015.cpp @@ -13,6 +15,8 @@ set(SRCS solvers/nakano_icpr_2025/get_nakano_icpr_2025.cpp solvers/valtonenornhag_icpr_2020/get_valtonenornhag_icpr_2020_fHf.cpp solvers/valtonenornhag_icpr_2020/solver_valtonenornhag_icpr_2020_fHf.cpp + solvers/valtonenornhag_icpr_2026/get_valtonenornhag_icpr_2026_affine.cpp + solvers/valtonenornhag_icpr_2026/get_valtonenornhag_icpr_2026_ori.cpp solvers/valtonenornhag_wacv_2021/get_valtonenornhag_wacv_2021_fHf.cpp solvers/valtonenornhag_wacv_2021/get_valtonenornhag_wacv_2021_frHfr.cpp solvers/valtonenornhag_wacv_2021/solver_valtonenornhag_wacv_2021_fHf.cpp diff --git a/HomLib/helpers/affine2sift.cpp b/HomLib/helpers/affine2sift.cpp new file mode 100644 index 0000000..9d6861b --- /dev/null +++ b/HomLib/helpers/affine2sift.cpp @@ -0,0 +1,154 @@ +#include +#include +#include + + + + + +using namespace Eigen; + + + +void affine2sift_solver(Eigen::MatrixXd const& A, double q1, std::vector* r1, std::vector* r2) +{ + // Compute coefficients +double const* A_data = A.data(); +double const A11 = A_data[0]; +double const A21 = A_data[1]; +double const A12 = A_data[2]; +double const A22 = A_data[3]; + VectorXd coeffs(14); + double _t2_ = A12*2.0; + double _t3_ = A22*2.0; + double _t4_ = q1*2.0; + double _t5_ = -A11; + double _t6_ = -A21; + double _t7_ = -q1; + double _t8_ = -_t4_; + coeffs[0] = q1+_t5_; + coeffs[1] = _t5_+_t7_; + coeffs[2] = _t2_; + coeffs[3] = _t2_; + coeffs[4] = A11+q1; + coeffs[5] = A11+_t7_; + coeffs[6] = _t6_; + coeffs[7] = _t8_; + coeffs[8] = _t6_; + coeffs[9] = _t3_; + coeffs[10] = _t3_; + coeffs[11] = A21; + coeffs[12] = _t8_; + coeffs[13] = A21; + + + // Setup elimination template + static const int coeffs0_ind[] = { 0,6,0,6,7,0,2,6,9,1,7,8,1,8,1,3,7,8,10,0,2,6,7,9,2,4,9,11 }; + static const int coeffs1_ind[] = { 5,13,3,5,10,13,1,3,8,10,3,5,10,12,13,2,4,9,11,12,5,12,13,4,11,12,4,11 }; + + +static const int C0_ind[] = {3,7,10,14,15,17,19,21,23,27,30,31,34,38,41,43,44,45,47,48,50,52,53,54,57,59,61,63}; + +static const int C1_ind[] = {0,4,8,10,12,14,16,18,20,22,25,27,29,30,31,32,34,36,38,39,41,44,45,48,52,53,57,61}; + +MatrixXd C0 = MatrixXd::Zero(8,8); +MatrixXd C1 = MatrixXd::Zero(8,8); +for (int i = 0; i < 28; i++) { + C0(C0_ind[i]) = coeffs(coeffs0_ind[i]); +} + +for (int i = 0; i < 28; i++) { + C1(C1_ind[i]) = coeffs(coeffs1_ind[i]); +} + +/* +Eigen::Matrix b = Eigen::Matrix::Zero(); +b(4,0) = -1; +b(5,1) = -1; +b(6,2) = -1; +b(7,3) = -1; +Eigen::Matrix alpha = C0.transpose().fullPivLu().solve(b); +Eigen::Matrix RR; +RR << alpha.transpose()*C1, Eigen::Matrix::Identity(); +//AM_ind = [6,7,1,2,3,8,9,4]; +//AM = RR(AM_ind,:); +Eigen::Matrix AM; +AM << RR.col(5), RR.col(6), RR.col(0), RR.col(1), RR.col(2), RR.col(7), RR.col(8), RR.col(3); +Eigen::EigenSolver< Eigen::Matrix > AMsolver(AM); +Eigen::MatrixXcd V = AMsolver.eigenvectors(); +V = V.array() * (Eigen::Matrix::Ones()*V.col(0)).array(); +Eigen::VectorXcd r1c = AMsolver.eigenvalues(); +Eigen::VectorXcd r2c = V.row(5); +*/ + +//[V,D] = eig(AM); +//V = V ./ (ones(size(V,1),1)*V(1,:)); +//sols(1,:) = diag(D).'; +//sols(2,:) = V(6,:); + +MatrixXd C12 = C0.fullPivLu().solve(C1); + + + + // Setup action matrix + Matrix RR; + RR << -C12.bottomRows(4), Matrix::Identity(8, 8); + + static const int AM_ind[] = { 5,6,0,1,2,7,8,3 }; + Matrix AM; + for (int i = 0; i < 8; i++) { + AM.row(i) = RR.row(AM_ind[i]); + } + + MatrixXcd sols(2, 8); + sols.setZero(); + + // Solve eigenvalue problem + EigenSolver > es(AM); + ArrayXcd D = es.eigenvalues(); + ArrayXXcd V = es.eigenvectors(); + + V = (V / V.row(0).array().replicate(8, 1)).eval(); + + + sols.row(0) = D.transpose().array(); + sols.row(1) = V.row(5).array(); + + + + + Eigen::VectorXcd r1c = sols.row(0); + Eigen::VectorXcd r2c = sols.row(1); + int nsols = r1c.size(); + for (int isol = 0; isol < nsols; ++isol) { + if ( r1c(isol).imag() == 0 && r2c(isol).imag() == 0 ) + { + r1->push_back(r1c(isol).real()); + r2->push_back(r2c(isol).real()); + } + } +} + +// Action = +// Quotient ring basis (V) = r1^2, r1*r2, r1*r2^2, r2, r2^2, r2^3 +// Available monomials (RR*V) = r1^2*r2, r1^2*r2^2, r1*r2^3, 1, r1, r1^2, r1*r2, r1*r2^2, r2, r2^2, r2^3 + +void affine2sift(const Eigen::Matrix2d &A, double &s1, double &c1, double &s2, double &c2, double &q ) +{ + q = sqrt(A.determinant()); + std::vector r1solns, r2solns; + affine2sift_solver(A, q, &r1solns, &r2solns); + + double r1 = r1solns[0]; + double r2 = r2solns[0]; + c1 = (1-r1*r1)/(1+r1*r1); + s1 = (2*r1)/(1+r1*r1); + c2 = (1-r2*r2)/(1+r2*r2); + s2 = (2*r2)/(1+r2*r2); + + // check residuals + //double res1 = c1*s2*A(0,0) + s1*s2*A(0,1) - c1*c2*A(1,0) - c2*s1*A(1,1); + //double res2 = A(0,1)*A(1,0)-A(0,0)*A(1,1)+q*q; + //double res3 = A(0,0)*c1 + A(0,1)*s1 - c2*q; + //double res4 = A(1,0)*c1 + A(1,1)*s1 - s2*q; +} diff --git a/HomLib/helpers/affine2sift.hpp b/HomLib/helpers/affine2sift.hpp new file mode 100644 index 0000000..1d95e0b --- /dev/null +++ b/HomLib/helpers/affine2sift.hpp @@ -0,0 +1,18 @@ +#ifndef SRC_HELPERS_AFFINE2SIFT_HPP_ +#define SRC_HELPERS_AFFINE2SIFT_HPP_ + +#include +#include +#include + +using namespace Eigen; + +// Converts an affine correspondence to a scale-and-orientation correspondence. +// There are eight possible solutions, but this arbitrarily returns the first one. +void affine2sift(const Eigen::Matrix2d &A, // input affine transformation matrix + double &s_ref, double &c_ref, // sine and cosine of feature orientation in reference image + double &s_query, double &c_query, // sine and cosine of feature orientation in query image + double &q // ratio of feature scales (scale in query image / scale in reference image) +); + +#endif // SRC_HELPERS_AFFINE2SIFT_HPP_ \ No newline at end of file diff --git a/HomLib/helpers/generate_problem_instance.cpp b/HomLib/helpers/generate_problem_instance.cpp index fe7e9ed..7ca4400 100644 --- a/HomLib/helpers/generate_problem_instance.cpp +++ b/HomLib/helpers/generate_problem_instance.cpp @@ -21,15 +21,40 @@ #include #include +#include #include #include "problem_instance.hpp" #include "radial.hpp" #include "generate_problem_instance.hpp" +#include "affine2sift.hpp" namespace HomLib { static const double kPI = 3.14159265358979323846; + + static Eigen::Matrix2d affineFromHomography( const Eigen::Matrix3d &H, const Eigen::Vector2d &x, const Eigen::Vector2d &y , double k) + { + // x are the distorted coeffs + double h1 = H(0,0), h2 = H(0,1), h3 = H(0,2), + h4 = H(1,0), h5 = H(1,1), h6 = H(1,2), + h7 = H(2,0), h8 = H(2,1), h9 = H(2,2); + double u1 = x(0), v1 = x(1), + u2 = y(0), v2 = y(1); + double dist_fact = k*(u1*u1 + v1*v1) + 1; + double s = h7*u1 + h8*v1 + h9*(dist_fact); + // Note that + // u2 = (h1*u1 + h2*v1 + h3*(dist_fact))/(s) + // v2 = (h4*u1 + h5*v1 + h6*(dist_fact))/(s) + Eigen::Matrix2d A; + A << (h1 + 2*h3*k*u1)/(s) - ((h7 + 2*h9*k*u1)*(h1*u1 + h2*v1 + h3*(dist_fact)))/(s*s), + (h2 + 2*h3*k*v1)/(s) - ((h8 + 2*h9*k*v1)*(h1*u1 + h2*v1 + h3*(dist_fact)))/(s*s), + (h4 + 2*h6*k*u1)/(s) - ((h7 + 2*h9*k*u1)*(h4*u1 + h5*v1 + h6*(dist_fact)))/(s*s), + (h5 + 2*h6*k*v1)/(s) - ((h8 + 2*h9*k*v1)*(h4*u1 + h5*v1 + h6*(dist_fact)))/(s*s); + + + return A; + } HomLib::ProblemInstance generate_problem_instance(const ProblemConfig &config) { @@ -42,7 +67,7 @@ namespace HomLib { random_engine.seed(std::chrono::system_clock::now().time_since_epoch().count()); std::uniform_real_distribution depth_gen(config.min_depth_, config.max_depth_); std::uniform_real_distribution coord_gen(-fov_scale, fov_scale); - // std::uniform_real_distribution focal_gen(config.min_focal_, config.max_focal_); + std::uniform_real_distribution focal_gen(config.min_focal_, config.max_focal_); std::normal_distribution direction_gen(0.0, 1.0); std::uniform_real_distribution dist_gen(config.min_dist_, config.max_dist_); @@ -54,13 +79,17 @@ namespace HomLib { t.normalize(); Eigen::Matrix3d R = Eigen::Quaternion::UnitRandom().toRotationMatrix(); - // double focal_gt = focal_gen(random_engine); + double focal_gt = focal_gen(random_engine); // Point to point correspondences instance.x1.clear(); instance.x2.clear(); + instance.A.clear(); + instance.ori.clear(); instance.x1.reserve(config.number_points); instance.x2.reserve(config.number_points); + instance.A.reserve(config.number_points); + instance.ori.reserve(config.number_points); // Generate plane Eigen::Vector3d n; @@ -74,6 +103,30 @@ namespace HomLib { // ground truth homography instance.posedata.homography = alpha * R + t * n.transpose(); + + // Distort + switch (config.distortion) { + case HomLib::DistortionCase::NO_DISTORTION: + instance.posedata.distortion_parameter = 0.0; + instance.posedata.distortion_parameter2 = 0.0; + break; + case HomLib::DistortionCase::ONE_SIDED_LEFT: + instance.posedata.distortion_parameter = 0.0; + instance.posedata.distortion_parameter2 = dist_gen(random_engine); + break; + case HomLib::DistortionCase::ONE_SIDED_RIGHT: + instance.posedata.distortion_parameter = dist_gen(random_engine); + instance.posedata.distortion_parameter2 = 0.0; + break; + case HomLib::DistortionCase::TWO_SIDED_EQUAL: + instance.posedata.distortion_parameter = dist_gen(random_engine); + instance.posedata.distortion_parameter2 = instance.posedata.distortion_parameter; + break; + case HomLib::DistortionCase::TWO_SIDED: + instance.posedata.distortion_parameter = dist_gen(random_engine); + instance.posedata.distortion_parameter2 = dist_gen(random_engine); + break; + } bool failed_instance = false; for (int j = 0; j < config.number_points; ++j) { @@ -103,11 +156,51 @@ namespace HomLib { // try to generate another point continue; } + Eigen::Vector2d x1h = x1.hnormalized(); + + // Distort + Eigen::Vector2d x1hd, x2hd; + x1hd = HomLib::radialdistort(x1h, instance.posedata.distortion_parameter); + x2hd = HomLib::radialdistort(x2h, instance.posedata.distortion_parameter2); + // calculate affine from homography + // This assumes DistortionCase.NO_DISTORTION or DistortionCase.ONE_SIDED_RIGHT + Eigen::Matrix2d A = affineFromHomography(instance.posedata.homography, x1hd, x2hd, instance.posedata.distortion_parameter); + + // check if determinant is positive + if ( A.determinant() < 0 ) continue; // - Eigen::Vector2d x1h = x1.hnormalized(); - instance.x1.push_back(x1h); - instance.x2.push_back(x2h); + double s_1, c_1, s_2, c_2, q; + affine2sift(A, s_1, c_1, s_2, c_2, q); + /* ONLY WORKS FOR RIGHT-SIDED AND NO_DISTORTION + double h_1 = instance.posedata.homography(0,0), + h_2 = instance.posedata.homography(0,1), + h_3 = instance.posedata.homography(0,2), + h_4 = instance.posedata.homography(1,0), + h_5 = instance.posedata.homography(1,1), + h_6 = instance.posedata.homography(1,2), + h_7 = instance.posedata.homography(2,0), + h_8 = instance.posedata.homography(2,1), + h_9 = instance.posedata.homography(2,2); + double u_1 = x1hd[0], + v_1 = x1hd[1], + u_2 = x2hd[0], + v_2 = x2hd[1]; + lambda = instance.posedata.distortion_parameter; + + double res = -h_1*s_2*c_1 - h_2*s_1*s_2 + h_4*c_1*c_2 + h_5*s_1*c_2 + h_7*u_2*s_2*c_1 - h_7*v_2*c_1*c_2 + h_8*u_2*s_1*s_2 + -h_8*v_2*s_1*c_2 -2*h_3*u_1*s_2*c_1*lambda - 2*h_3*v_1*s_1*s_2*lambda + 2*h_6*u_1*c_1*c_2*lambda + 2*h_6*v_1*s_1*c_2*lambda + + 2*h_9*u_1*u_2*s_2*c_1*lambda - 2*h_9*u_1*v_2*c_1*c_2*lambda + 2*h_9*v_1*u_2*s_1*s_2*lambda - 2*h_9*v_1*v_2*s_1*c_2*lambda; + */ + + Eigen::Vector2d ori; + ori[0] = std::atan2(s_1, c_1); + ori[1] = std::atan2(s_2, c_2); + + instance.x1.push_back(x1hd); + instance.x2.push_back(x2hd); + instance.A.push_back(A); + instance.ori.push_back(ori); point_okay = true; break; @@ -120,28 +213,6 @@ namespace HomLib { if (failed_instance) { continue; } - - // Distort - if (config.no_distortion) { - instance.posedata.distortion_parameter = 0.0; - instance.posedata.distortion_parameter2 = 0.0; - } else { - instance.posedata.distortion_parameter2 = dist_gen(random_engine); - if (config.one_sided) { - instance.posedata.distortion_parameter = 0.0; - } else { - if (config.equal) { - instance.posedata.distortion_parameter = instance.posedata.distortion_parameter2; - } else { - instance.posedata.distortion_parameter = dist_gen(random_engine); - } - } - - if (!config.one_sided) { - HomLib::radialdistort(instance.x1, &instance.x1, instance.posedata.distortion_parameter); - } - HomLib::radialdistort(instance.x2, &instance.x2, instance.posedata.distortion_parameter2); - } // Focal length //instance.x1 *= focal_gt; diff --git a/HomLib/helpers/problem_instance.hpp b/HomLib/helpers/problem_instance.hpp index 729d240..b4a72f5 100644 --- a/HomLib/helpers/problem_instance.hpp +++ b/HomLib/helpers/problem_instance.hpp @@ -27,10 +27,21 @@ #include "posedata.hpp" namespace HomLib { + +enum DistortionCase { + NO_DISTORTION, + ONE_SIDED_LEFT, + ONE_SIDED_RIGHT, + TWO_SIDED_EQUAL, + TWO_SIDED +}; + struct ProblemInstance { HomLib::PoseData posedata; std::vector x1; std::vector x2; + std::vector A; + std::vector ori; double hom_error(const Eigen::Matrix3d &H_est) const { @@ -44,29 +55,21 @@ struct ProblemInstance { } double dist_error(double k1_est, double k2_est) const { - // Computes the algebraic mean (makes more sense to me..) - // return std::sqrt(std::abs(posedata.distortion_parameter-k1_est) * std::abs(posedata.distortion_parameter2-k2_est)); - return 0.5 * (std::abs(posedata.distortion_parameter-k1_est) + std::abs(posedata.distortion_parameter2-k2_est)); - } - double dist_error(double k_est) const - { - // In case of one-sided k1 = 0.0, so it is always safe to use k2. - return std::abs(posedata.distortion_parameter2-k_est); + return 0.5*(std::abs(posedata.distortion_parameter-k1_est) + std::abs(posedata.distortion_parameter2-k2_est)); } }; struct ProblemConfig { - bool one_sided; - bool equal; + DistortionCase distortion; double point_noise; int number_points; double camera_fov_ = 70.0; double min_depth_ = 0.1; double max_depth_ = 10.0; - double min_focal_ = 1000.0; + double min_focal_ = 100.0; double max_focal_ = 1000.0; - double min_dist_ = -0.3; - double max_dist_ = -0.3; + double min_dist_ = -0.2; + double max_dist_ = -0.01; bool no_distortion = false; }; } diff --git a/HomLib/includes/HomLib/get_barath_visapp_2016.hpp b/HomLib/includes/HomLib/get_barath_visapp_2016.hpp new file mode 100644 index 0000000..3592e4d --- /dev/null +++ b/HomLib/includes/HomLib/get_barath_visapp_2016.hpp @@ -0,0 +1,68 @@ +// Copyright (c) 2020 Marcus Valtonen Örnhag +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef INCLUDES_HOMLIB_GET_BARATH_VISAPP_2016_HPP_ +#define INCLUDES_HOMLIB_GET_BARATH_VISAPP_2016_HPP_ + +#include +#include +#include "posedata.hpp" +#include "radial.hpp" +#include "affine_pose_estimator.h" +#include "refinement.hpp" + +namespace HomLib { +namespace BarathVISAPP2016 { +std::vector get_affine( + const std::vector &x, + const std::vector &y, + const std::vector &A +); + +class AffineSolver : public AffinePoseEstimator { + public: + AffineSolver() = default; + int solve(const std::vector &x, const std::vector &y, const std::vector &A, std::vector *poses) const { + std::vector output = HomLib::BarathVISAPP2016::get_affine(x, y, A); + for (size_t i = 0; i < output.size(); i++) { + poses->push_back(output[i]); + } + return output.size(); + } + int minimal_sample_size() const { + return 2; + } + inline Eigen::Vector2d undistort(const HomLib::PoseData pose, const Eigen::Vector2d &xd) const { + Eigen::Vector2d xu = xd; // No dist + return xu; + } + inline Eigen::Vector2d distort(const HomLib::PoseData pose, const Eigen::Vector2d &yu) const { + Eigen::Vector2d yd = yu; // No dist + return yd; + } + inline void refine(HomLib::PoseData &pose, const std::vector &x, const std::vector &y) const { + HomLib::refinement_no_dist(x, y, pose); + } + }; + +} +} + +#endif // INCLUDES_HOMLIB_GET_BARATH_VISAPP_2016_HPP_ diff --git a/HomLib/includes/HomLib/get_nakano_icpr_2025.hpp b/HomLib/includes/HomLib/get_nakano_icpr_2025.hpp index b7aa382..11c47fc 100644 --- a/HomLib/includes/HomLib/get_nakano_icpr_2025.hpp +++ b/HomLib/includes/HomLib/get_nakano_icpr_2025.hpp @@ -25,9 +25,11 @@ #include #include "posedata.hpp" #include "radial.hpp" +#include "affine_pose_estimator.h" +#include "orientation_pose_estimator.h" #include "pose_estimator.h" #include "refinement.hpp" -#include + namespace HomLib { namespace NakanoICPR2025 { std::vector get( @@ -35,6 +37,22 @@ std::vector get( const std::vector &y, bool extra_check ); +std::vector get_affine( + const std::vector &x, + const std::vector &y, + const std::vector &A, + bool extra_check +); +std::vector get_ori( + const std::vector &x, + const std::vector &y, + const std::vector &ori +); +std::vector get_affine_no_dist( + const std::vector &x, + const std::vector &y, + const std::vector &A +); class SolverSingleSided : public PoseEstimator { public: SolverSingleSided() = default; @@ -48,12 +66,143 @@ class SolverSingleSided : public PoseEstimator { int minimal_sample_size() const { return 5; } + inline Eigen::Vector2d undistort(const HomLib::PoseData pose, const Eigen::Vector2d &xd) const { + Eigen::Vector2d xu = HomLib::radialundistort(xd, 0.0); // One-sided + return xu; + } + inline Eigen::Vector2d distort(const HomLib::PoseData pose, const Eigen::Vector2d &yu) const { + Eigen::Vector2d yd = HomLib::radialdistort(yu, pose.distortion_parameter2); + return yd; + } inline void refine(HomLib::PoseData &pose, const std::vector &x, const std::vector &y) const { HomLib::refinement_onesided(x, y, pose); } private: bool extra_check = false; }; +class SolverSingleSidedRight : public PoseEstimator { + public: + SolverSingleSidedRight() = default; + int solve(const std::vector &x, const std::vector &y, std::vector *poses) const { + std::vector output = HomLib::NakanoICPR2025::get(y, x, extra_check); // Hack here + for (size_t i = 0; i < output.size(); i++) { + output[i].homography = output[i].homography.inverse(); //Hack here + output[i].distortion_parameter = output[i].distortion_parameter2; //Hack here + output[i].distortion_parameter2 = 0.0; //Hack here + // std::cout << "H[" << i << "] = " << output[i].homography / output[i].homography(2,2) << std::endl; + // std::cout << "k1[" << i << "] = " << output[i].distortion_parameter << std::endl; + // std::cout << "k2[" << i << "] = " << output[i].distortion_parameter2 << std::endl; + poses->push_back(output[i]); + } + return output.size(); + } + int minimal_sample_size() const { + return 5; + } + inline Eigen::Vector2d undistort(const HomLib::PoseData pose, const Eigen::Vector2d &xd) const { + Eigen::Vector2d xu = HomLib::radialundistort(xd, pose.distortion_parameter); // One-sided + return xu; + } + inline Eigen::Vector2d distort(const HomLib::PoseData pose, const Eigen::Vector2d &yu) const { + Eigen::Vector2d yd = yu; + return yd; + } + inline void refine(HomLib::PoseData &pose, const std::vector &x, const std::vector &y) const { + HomLib::refinement_onesided_right(x, y, pose); + } + private: + bool extra_check = false; + }; + +class AffineSolverSingleSided : public AffinePoseEstimator { + public: + AffineSolverSingleSided() = default; + int solve(const std::vector &x, const std::vector &y, const std::vector &A, std::vector *poses) const { + std::vector output = HomLib::NakanoICPR2025::get_affine(x, y, A, extra_check); + for (size_t i = 0; i < output.size(); i++) { + // std::cout << "H[" << i << "] = " << output[i].homography / output[i].homography(2,2) << std::endl; + // std::cout << "k1[" << i << "] = " << output[i].distortion_parameter << std::endl; + // std::cout << "k2[" << i << "] = " << output[i].distortion_parameter2 << std::endl; + poses->push_back(output[i]); + } + return output.size(); + } + int minimal_sample_size() const { + return 2; + } + inline Eigen::Vector2d undistort(const HomLib::PoseData pose, const Eigen::Vector2d &xd) const { + Eigen::Vector2d xu = HomLib::radialundistort(xd, pose.distortion_parameter); // One-sided + return xu; + } + inline Eigen::Vector2d distort(const HomLib::PoseData pose, const Eigen::Vector2d &yu) const { + Eigen::Vector2d yd = yu; + return yd; + } + inline void refine(HomLib::PoseData &pose, const std::vector &x, const std::vector &y) const { + HomLib::refinement_onesided_right(x, y, pose); + } + private: + bool extra_check = false; + }; + +class OrientationSolverSingleSided : public OrientationPoseEstimator { + public: + OrientationSolverSingleSided() = default; + int solve(const std::vector &x, const std::vector &y, const std::vector &ori, std::vector *poses) const { + std::vector output = HomLib::NakanoICPR2025::get_ori(x, y, ori); + for (size_t i = 0; i < output.size(); i++) { + // std::cout << "H[" << i << "] = " << output[i].homography / output[i].homography(2,2) << std::endl; + // std::cout << "k1[" << i << "] = " << output[i].distortion_parameter << std::endl; + // std::cout << "k2[" << i << "] = " << output[i].distortion_parameter2 << std::endl; + poses->push_back(output[i]); + } + return output.size(); + } + int minimal_sample_size() const { + return 4; // Degenerates ? + } + inline Eigen::Vector2d undistort(const HomLib::PoseData pose, const Eigen::Vector2d &xd) const { + Eigen::Vector2d xu = HomLib::radialundistort(xd, pose.distortion_parameter); // One-sided + return xu; + } + inline Eigen::Vector2d distort(const HomLib::PoseData pose, const Eigen::Vector2d &yu) const { + Eigen::Vector2d yd = yu; + return yd; + } + inline void refine(HomLib::PoseData &pose, const std::vector &x, const std::vector &y) const { + HomLib::refinement_onesided_right(x, y, pose); + } + }; + +class AffineSolverNoDist : public AffinePoseEstimator { + public: + AffineSolverNoDist() = default; + int solve(const std::vector &x, const std::vector &y, const std::vector &A, std::vector *poses) const { + std::vector output = HomLib::NakanoICPR2025::get_affine_no_dist(x, y, A); + for (size_t i = 0; i < output.size(); i++) { + // std::cout << "H[" << i << "] = " << output[i].homography / output[i].homography(2,2) << std::endl; + // std::cout << "k1[" << i << "] = " << output[i].distortion_parameter << std::endl; + // std::cout << "k2[" << i << "] = " << output[i].distortion_parameter2 << std::endl; + poses->push_back(output[i]); + } + return output.size(); + } + int minimal_sample_size() const { + return 2; + } + inline Eigen::Vector2d undistort(const HomLib::PoseData pose, const Eigen::Vector2d &xd) const { + Eigen::Vector2d xu = xd; // No dist + return xu; + } + inline Eigen::Vector2d distort(const HomLib::PoseData pose, const Eigen::Vector2d &yu) const { + Eigen::Vector2d yd = yu; // No dist + return yd; + } + inline void refine(HomLib::PoseData &pose, const std::vector &x, const std::vector &y) const { + HomLib::refinement_no_dist(x, y, pose); + } + }; + } } diff --git a/HomLib/includes/HomLib/get_valtonenornhag_icpr_2026.hpp b/HomLib/includes/HomLib/get_valtonenornhag_icpr_2026.hpp new file mode 100644 index 0000000..4535b40 --- /dev/null +++ b/HomLib/includes/HomLib/get_valtonenornhag_icpr_2026.hpp @@ -0,0 +1,102 @@ +// Copyright (c) 2020 Marcus Valtonen Örnhag +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef INCLUDES_HOMLIB_GET_VALTONENORNHAG_ICPR_2026_HPP_ +#define INCLUDES_HOMLIB_GET_VALTONENORNHAG_ICPR_2026_HPP_ + +#include +#include +#include "posedata.hpp" +#include "radial.hpp" +#include "affine_pose_estimator.h" +#include "orientation_pose_estimator.h" +#include "refinement.hpp" + +namespace HomLib { +namespace ValtonenOrnhagICPR2026 { +std::vector get_affine( + const std::vector &x, + const std::vector &y, + const std::vector &A, + bool extra_check +); +std::vector get_ori( + const std::vector &x, + const std::vector &y, + const std::vector &ori +); + +class AffineSolverSingleSided : public AffinePoseEstimator { + public: + AffineSolverSingleSided() = default; + int solve(const std::vector &x, const std::vector &y, const std::vector &A, std::vector *poses) const { + std::vector output = HomLib::ValtonenOrnhagICPR2026::get_affine(x, y, A, extra_check); + for (size_t i = 0; i < output.size(); i++) { + poses->push_back(output[i]); + } + return output.size(); + } + int minimal_sample_size() const { + return 2; + } + inline Eigen::Vector2d undistort(const HomLib::PoseData pose, const Eigen::Vector2d &xd) const { + Eigen::Vector2d xu = HomLib::radialundistort(xd, pose.distortion_parameter); // One-sided + return xu; + } + inline Eigen::Vector2d distort(const HomLib::PoseData pose, const Eigen::Vector2d &yu) const { + Eigen::Vector2d yd = yu; + return yd; + } + inline void refine(HomLib::PoseData &pose, const std::vector &x, const std::vector &y) const { + HomLib::refinement_onesided_right(x, y, pose); + } + private: + bool extra_check = false; + }; + +class OrientationSolverSingleSided : public OrientationPoseEstimator { + public: + OrientationSolverSingleSided() = default; + int solve(const std::vector &x, const std::vector &y, const std::vector &ori, std::vector *poses) const { + std::vector output = HomLib::ValtonenOrnhagICPR2026::get_ori(x, y, ori); + for (size_t i = 0; i < output.size(); i++) { + poses->push_back(output[i]); + } + return output.size(); + } + int minimal_sample_size() const { + return 4; // Degenerates ? + } + inline Eigen::Vector2d undistort(const HomLib::PoseData pose, const Eigen::Vector2d &xd) const { + Eigen::Vector2d xu = HomLib::radialundistort(xd, pose.distortion_parameter); // One-sided + return xu; + } + inline Eigen::Vector2d distort(const HomLib::PoseData pose, const Eigen::Vector2d &yu) const { + Eigen::Vector2d yd = yu; + return yd; + } + inline void refine(HomLib::PoseData &pose, const std::vector &x, const std::vector &y) const { + HomLib::refinement_onesided_right(x, y, pose); + } + }; +} +} + +#endif // INCLUDES_HOMLIB_GET_VALTONENORNHAG_ICPR_2026_HPP_ diff --git a/HomLib/includes/HomLib/refinement.hpp b/HomLib/includes/HomLib/refinement.hpp index 3b69959..6d96810 100644 --- a/HomLib/includes/HomLib/refinement.hpp +++ b/HomLib/includes/HomLib/refinement.hpp @@ -25,11 +25,21 @@ #include "posedata.hpp" namespace HomLib { +void refinement_no_dist( + const std::vector &x1, + const std::vector &x2, + HomLib::PoseData &p +); void refinement_onesided( const std::vector &x1, const std::vector &x2, HomLib::PoseData &p ); +void refinement_onesided_right( + const std::vector &x1, + const std::vector &x2, + HomLib::PoseData &p +); void refinement_twosided_equal( const std::vector &x1, const std::vector &x2, diff --git a/HomLib/robust/affine_pose_estimator.h b/HomLib/robust/affine_pose_estimator.h new file mode 100644 index 0000000..e82d596 --- /dev/null +++ b/HomLib/robust/affine_pose_estimator.h @@ -0,0 +1,74 @@ +#pragma once + +#include +#include +#include +#include "posedata.hpp" + +namespace HomLib { + + // We use CRTP here for the solvers. + template + class AffinePoseEstimator { + public: + int estimate(const std::vector &x, const std::vector &y, const std::vector &A, std::vector *poses) const; + + inline int minimal_sample_size() const { + return static_cast(this)->minimal_sample_size(); + } + + // Options + bool normalize_image_coord = true; + + protected: + AffinePoseEstimator() = default; + }; +}; + + +template +int HomLib::AffinePoseEstimator::estimate(const std::vector &x_, const std::vector &y_, const std::vector &A_, std::vector *poses) const +{ + std::vector x = x_; + std::vector y = y_; + std::vector A = A_; + + // Rescale image plane + double f0 = 0.0; + if (normalize_image_coord) { + // TODO: Consider full Hartley normalization, i.e. also translate. + for (size_t i = 0; i < x.size(); i++) { + f0 += x[i].norm(); + } + f0 /= x.size(); + f0 /= std::sqrt(2.0); + for (size_t i = 0; i < x.size(); i++) { + x[i] /= f0; + y[i] /= f0; + } + } + + // Call solver implementation + poses->clear(); + int n_sols = static_cast(this)->solve(x, y, A, poses); + + // Revert image coordinate scaling + if (normalize_image_coord) { + double f02 = f0 * f0; + for (size_t i = 0; i < poses->size(); ++i) { + (*poses)[i].homography(0,0) *= f0; + (*poses)[i].homography(0,1) *= f0; + (*poses)[i].homography(0,2) *= f02; + (*poses)[i].homography(1,0) *= f0; + (*poses)[i].homography(1,1) *= f0; + (*poses)[i].homography(1,2) *= f02; + (*poses)[i].homography(2,2) *= f0; + (*poses)[i].distortion_parameter /= f02; + (*poses)[i].distortion_parameter2 /= f02; + } + } + + return n_sols; +} + + diff --git a/HomLib/robust/affine_ransac_estimator.h b/HomLib/robust/affine_ransac_estimator.h new file mode 100644 index 0000000..f57850a --- /dev/null +++ b/HomLib/robust/affine_ransac_estimator.h @@ -0,0 +1,130 @@ +#pragma once + +#include +#include +#include "posedata.hpp" +#include +namespace HomLib { + +template +class AffineRansacEstimator { +public: + AffineRansacEstimator(const std::vector &x_, const std::vector &y_, const std::vector &A_, Solver est) { + x = x_; + y = y_; + A = A_; + solver = est; + } + + inline int min_sample_size() const { + return solver.minimal_sample_size(); + } + inline int non_minimal_sample_size() const { + return solver.minimal_sample_size() * 4; + } + inline int num_data() const { + return x.size(); + } + + int MinimalSolver(const std::vector& sample, + std::vector* poses) const { + + std::vector xx; + std::vector yy; + std::vector AA; + + for (size_t i = 0; i < sample.size(); i++) { + xx.push_back(x[sample[i]]); + yy.push_back(y[sample[i]]); + AA.push_back(A[sample[i]]); + } + solver.estimate(xx, yy, AA, poses); + + //std::cout << "H =\n" << (*poses)[0].homography << std::endl; + + return poses->size(); + } + + // Returns 0 if no model could be estimated and 1 otherwise. + int NonMinimalSolver(const std::vector& sample, HomLib::PoseData* pose) const { + if (!use_non_minimal) + return 0; + + std::vector xx; + std::vector yy; + std::vector AA; + + for (size_t i = 0; i < sample.size(); i++) { + xx.push_back(x[sample[i]]); + yy.push_back(y[sample[i]]); + AA.push_back(A[sample[i]]); + } + + // Call non-minimal solver (same as the minimal... but more points) + std::vector poses; + solver.estimate(xx, yy, AA, &poses); + + //std::cout << "H =\n" << poses[0].homography << std::endl; + + // for all pose candidates compute score + double best_score = std::numeric_limits::max(); + int best_idx = -1; + + for (size_t i = 0; i < poses.size(); ++i) { + double score = 0; + for (size_t j = 0; j < sample.size(); ++j) + score += EvaluateModelOnPoint(poses[i], sample[j]); + if (score < best_score) { + best_score = score; + best_idx = i; + } + } + + if (best_idx != -1) { + *pose = poses[best_idx]; + return 1; + } else { + return 0; + } + } + + // Evaluates the line on the i-th data point. + double EvaluateModelOnPoint(const HomLib::PoseData& pose, int i) const { + // Rectify + Eigen::Vector2d z = solver.undistort(pose, x[i]); + + // Compute reprojection error + Eigen::Vector3d Z = pose.homography * z.homogeneous(); + z = Z.hnormalized(); + z = solver.distort(pose, z); + + // std::cout << "evaluate " << i << ": " << (y[i] - z).squaredNorm() << std::endl; + + return (y[i] - z).squaredNorm(); + } + + // Linear least squares solver. Calls NonMinimalSolver. + inline void LeastSquares(const std::vector& sample, HomLib::PoseData* p) const { + if (!use_local_opt) + return; + std::vector xx; + std::vector yy; + + for (size_t i = 0; i < sample.size(); i++) { + xx.push_back(x[sample[i]]); + yy.push_back(y[sample[i]]); + } + solver.refine(*p, xx, yy); + } + + + bool use_non_minimal = true; + bool use_local_opt = true; +private: + Solver solver; + std::vector x; + std::vector y; + std::vector A; +}; + +} diff --git a/HomLib/robust/orientation_pose_estimator.h b/HomLib/robust/orientation_pose_estimator.h new file mode 100644 index 0000000..d9ba0b2 --- /dev/null +++ b/HomLib/robust/orientation_pose_estimator.h @@ -0,0 +1,74 @@ +#pragma once + +#include +#include +#include +#include "posedata.hpp" + +namespace HomLib { + + // We use CRTP here for the solvers. + template + class OrientationPoseEstimator { + public: + int estimate(const std::vector &x, const std::vector &y, const std::vector &ori, std::vector *poses) const; + + inline int minimal_sample_size() const { + return static_cast(this)->minimal_sample_size(); + } + + // Options + bool normalize_image_coord = true; + + protected: + OrientationPoseEstimator() = default; + }; +}; + + +template +int HomLib::OrientationPoseEstimator::estimate(const std::vector &x_, const std::vector &y_, const std::vector &ori_, std::vector *poses) const +{ + std::vector x = x_; + std::vector y = y_; + std::vector ori = ori_; + + // Rescale image plane + double f0 = 0.0; + if (normalize_image_coord) { + // TODO: Consider full Hartley normalization, i.e. also translate. + for (size_t i = 0; i < x.size(); i++) { + f0 += x[i].norm(); + } + f0 /= x.size(); + f0 /= std::sqrt(2.0); + for (size_t i = 0; i < x.size(); i++) { + x[i] /= f0; + y[i] /= f0; + } + } + + // Call solver implementation + poses->clear(); + int n_sols = static_cast(this)->solve(x, y, ori, poses); + + // Revert image coordinate scaling + if (normalize_image_coord) { + double f02 = f0 * f0; + for (size_t i = 0; i < poses->size(); ++i) { + (*poses)[i].homography(0,0) *= f0; + (*poses)[i].homography(0,1) *= f0; + (*poses)[i].homography(0,2) *= f02; + (*poses)[i].homography(1,0) *= f0; + (*poses)[i].homography(1,1) *= f0; + (*poses)[i].homography(1,2) *= f02; + (*poses)[i].homography(2,2) *= f0; + (*poses)[i].distortion_parameter /= f02; + (*poses)[i].distortion_parameter2 /= f02; + } + } + + return n_sols; +} + + diff --git a/HomLib/robust/orientation_ransac_estimator.h b/HomLib/robust/orientation_ransac_estimator.h new file mode 100644 index 0000000..e3be568 --- /dev/null +++ b/HomLib/robust/orientation_ransac_estimator.h @@ -0,0 +1,130 @@ +#pragma once + +#include +#include +#include "posedata.hpp" +#include +namespace HomLib { + +template +class OrientationRansacEstimator { +public: + OrientationRansacEstimator(const std::vector &x_, const std::vector &y_, const std::vector &ori_, Solver est) { + x = x_; + y = y_; + ori = ori_; + solver = est; + } + + inline int min_sample_size() const { + return solver.minimal_sample_size(); + } + inline int non_minimal_sample_size() const { + return solver.minimal_sample_size() * 4; + } + inline int num_data() const { + return x.size(); + } + + int MinimalSolver(const std::vector& sample, + std::vector* poses) const { + + std::vector xx; + std::vector yy; + std::vector oo; + + for (size_t i = 0; i < sample.size(); i++) { + xx.push_back(x[sample[i]]); + yy.push_back(y[sample[i]]); + oo.push_back(ori[sample[i]]); + } + solver.estimate(xx, yy, oo, poses); + + //std::cout << "H =\n" << (*poses)[0].homography << std::endl; + + return poses->size(); + } + + // Returns 0 if no model could be estimated and 1 otherwise. + int NonMinimalSolver(const std::vector& sample, HomLib::PoseData* pose) const { + if (!use_non_minimal) + return 0; + + std::vector xx; + std::vector yy; + std::vector oo; + + for (size_t i = 0; i < sample.size(); i++) { + xx.push_back(x[sample[i]]); + yy.push_back(y[sample[i]]); + oo.push_back(ori[sample[i]]); + } + + // Call non-minimal solver (same as the minimal... but more points) + std::vector poses; + solver.estimate(xx, yy, oo, &poses); + + //std::cout << "H =\n" << poses[0].homography << std::endl; + + // for all pose candidates compute score + double best_score = std::numeric_limits::max(); + int best_idx = -1; + + for (size_t i = 0; i < poses.size(); ++i) { + double score = 0; + for (size_t j = 0; j < sample.size(); ++j) + score += EvaluateModelOnPoint(poses[i], sample[j]); + if (score < best_score) { + best_score = score; + best_idx = i; + } + } + + if (best_idx != -1) { + *pose = poses[best_idx]; + return 1; + } else { + return 0; + } + } + + // Evaluates the line on the i-th data point. + double EvaluateModelOnPoint(const HomLib::PoseData& pose, int i) const { + // Rectify + Eigen::Vector2d z = solver.undistort(pose, x[i]); + + // Compute reprojection error + Eigen::Vector3d Z = pose.homography * z.homogeneous(); + z = Z.hnormalized(); + z = solver.distort(pose, z); + + // std::cout << "evaluate " << i << ": " << (y[i] - z).squaredNorm() << std::endl; + + return (y[i] - z).squaredNorm(); + } + + // Linear least squares solver. Calls NonMinimalSolver. + inline void LeastSquares(const std::vector& sample, HomLib::PoseData* p) const { + if (!use_local_opt) + return; + std::vector xx; + std::vector yy; + + for (size_t i = 0; i < sample.size(); i++) { + xx.push_back(x[sample[i]]); + yy.push_back(y[sample[i]]); + } + solver.refine(*p, xx, yy); + } + + + bool use_non_minimal = true; + bool use_local_opt = true; +private: + Solver solver; + std::vector x; + std::vector y; + std::vector ori; +}; + +} diff --git a/HomLib/robust/refinement.cpp b/HomLib/robust/refinement.cpp index baffdef..58d9787 100644 --- a/HomLib/robust/refinement.cpp +++ b/HomLib/robust/refinement.cpp @@ -9,6 +9,106 @@ static const int MAX_ITER = 10; namespace HomLib { +// Non-linear refinement of transfer error |x2 - pi(H*x1)|^2, parameterized by fixing H(2,2) = 1 +void refinement_no_dist( + const std::vector &x1, + const std::vector &x2, + HomLib::PoseData &p +) { + int n_pts = x1.size(); + int n_res = 2 * n_pts; + int n_params = 8; + double lm_damp = INITIAL_LM_DAMP; + + // Order for jacobian is: h11, h21, h31, h12, h22, h32, h13, h23 + Eigen::Matrix J(n_res, n_params); + J.setZero(); + Eigen::Matrix res(n_res, 1); + Eigen::Matrix dx(n_params, 1); + res.setZero(); + + Eigen::Matrix Hess; + Eigen::Matrix g; + + //std::cout << "allocation done\n"; + + for (int iter = 0; iter < MAX_ITER; iter++) { + Eigen::Matrix3d H = p.homography; + H /= H(2,2); + + const double H0_0 = H(0, 0), H0_1 = H(0, 1), H0_2 = H(0, 2); + const double H1_0 = H(1, 0), H1_1 = H(1, 1), H1_2 = H(1, 2); + const double H2_0 = H(2, 0), H2_1 = H(2, 1), H2_2 = H(2, 2); + + //std::cout << "entering first row of iter"<>(H_new.data()) += dx.head(8); + p.homography = H_new; + + if (dx.array().abs().maxCoeff() < SMALL_NUMBER) + break; + lm_damp = std::max(1e-8, lm_damp / 10.0); + } +} // Non-linear refinement of transfer error |x2(k) - pi(H*x1)|^2, parameterized by fixing H(2,2) = 1 void refinement_onesided( const std::vector &x1, @@ -35,7 +135,7 @@ void refinement_onesided( for (int iter = 0; iter < MAX_ITER; iter++) { Eigen::Matrix3d H = p.homography; H /= H(2,2); - const double k = p.distortion_parameter; + const double k = p.distortion_parameter2; const double H0_0 = H(0, 0), H0_1 = H(0, 1), H0_2 = H(0, 2); const double H1_0 = H(1, 0), H1_1 = H(1, 1), H1_2 = H(1, 2); @@ -98,6 +198,114 @@ void refinement_onesided( g = -J.transpose()*res; + if (g.cwiseAbs().maxCoeff() < TOL_CONVERGENCE) + break; + + //std::cout << "iter=" << iter << " res=" << res.squaredNorm() << ", g="<< g.squaredNorm() << "\n"; + //std::cout << res << "\n"; + + dx = Hess.ldlt().solve(g); + + Eigen::Matrix3d H_new = H; + Eigen::Map>(H_new.data()) += dx.head(8); + p.homography = H_new; + p.distortion_parameter2 += dx(8); + + if (dx.array().abs().maxCoeff() < SMALL_NUMBER) + break; + lm_damp = std::max(1e-8, lm_damp / 10.0); + } +} + + +// Non-linear refinement of transfer error |x2 - pi(H*x1(k))|^2, parameterized by fixing H(2,2) = 1 +void refinement_onesided_right( + const std::vector &x1, + const std::vector &x2, + HomLib::PoseData &p +) { + int n_pts = x1.size(); + int n_res = 2 * n_pts; + int n_params = 9; + double lm_damp = INITIAL_LM_DAMP; + + // Order for jacobian is: h11, h21, h31, h12, h22, h32, h13, h23, k + Eigen::Matrix J(n_res, n_params); + J.setZero(); + Eigen::Matrix res(n_res, 1); + Eigen::Matrix dx(n_params, 1); + res.setZero(); + + Eigen::Matrix Hess; + Eigen::Matrix g; + + //std::cout << "allocation done\n"; + + for (int iter = 0; iter < MAX_ITER; iter++) { + Eigen::Matrix3d H = p.homography; + H /= H(2,2); + const double k = p.distortion_parameter; + + const double H0_0 = H(0, 0), H0_1 = H(0, 1), H0_2 = H(0, 2); + const double H1_0 = H(1, 0), H1_1 = H(1, 1), H1_2 = H(1, 2); + const double H2_0 = H(2, 0), H2_1 = H(2, 1), H2_2 = H(2, 2); + + //std::cout << "entering first row of iter"<>(H_new.data()) += dx.head(8); p.homography = H_new; p.distortion_parameter += dx(8); + p.distortion_parameter2 = p.distortion_parameter; if (dx.array().abs().maxCoeff() < SMALL_NUMBER) break; @@ -235,7 +444,7 @@ void refinement_twosided( int n_params = 10; double lm_damp = INITIAL_LM_DAMP; - // Order for jacobian is: h11, h21, h31, h12, h22, h32, h13, h23, k + // Order for jacobian is: h11, h21, h31, h12, h22, h32, h13, h23, k1, k2 Eigen::Matrix J(n_res, n_params); J.setZero(); Eigen::Matrix res(n_res, 1); diff --git a/HomLib/solvers/barath_visapp_2016/get_barath_visapp_2016_affine.cpp b/HomLib/solvers/barath_visapp_2016/get_barath_visapp_2016_affine.cpp new file mode 100644 index 0000000..934bbce --- /dev/null +++ b/HomLib/solvers/barath_visapp_2016/get_barath_visapp_2016_affine.cpp @@ -0,0 +1,157 @@ +// Copyright (c) 2020 Marcus Valtonen Örnhag +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + + +#include +#include +#include +#include +#include +#include "posedata.hpp" + +#include "radial.hpp" +#include "get_barath_visapp_2016.hpp" + +namespace HomLib { +namespace BarathVISAPP2016 { + std::vector get_affine( + const std::vector &x, + const std::vector &y, + const std::vector &Aff + ) { + + double weight = 1.0; + Eigen::Matrix coefficients(6 * x.size(), 9); + size_t rowIdx = 0; + for (size_t i = 0; i < x.size(); i++) + { + const double + x1 = x[i](0), + y1 = x[i](1), + x2 = y[i](0), + y2 = y[i](1), + a11 = Aff[i](0,0), + a12 = Aff[i](0,1), + a21 = Aff[i](1,0), + a22 = Aff[i](1,1); + + const double + kMinusWeightTimesX1 = -weight * x1, + kMinusWeightTimesY1 = -weight * y1, + kWeightTimesX2 = weight * x2, + kWeightTimesY2 = weight * y2; + + coefficients(rowIdx, 0) = kMinusWeightTimesX1; + coefficients(rowIdx, 1) = kMinusWeightTimesY1; + coefficients(rowIdx, 2) = -weight; + coefficients(rowIdx, 3) = 0; + coefficients(rowIdx, 4) = 0; + coefficients(rowIdx, 5) = 0; + coefficients(rowIdx, 6) = kWeightTimesX2 * x1; + coefficients(rowIdx, 7) = kWeightTimesX2 * y1; + coefficients(rowIdx, 8) = kWeightTimesX2; + ++rowIdx; + + coefficients(rowIdx, 0) = 0; + coefficients(rowIdx, 1) = 0; + coefficients(rowIdx, 2) = 0; + coefficients(rowIdx, 3) = kMinusWeightTimesX1; + coefficients(rowIdx, 4) = kMinusWeightTimesY1; + coefficients(rowIdx, 5) = -weight; + coefficients(rowIdx, 6) = kWeightTimesY2 * x1; + coefficients(rowIdx, 7) = kWeightTimesY2 * y1; + coefficients(rowIdx, 8) = kWeightTimesY2; + ++rowIdx; + + // If the minimal case is considered, we + // do not need all constraints to estimate + // the homography. + //if (i == 1) { + // break; + //} + + // NOTE(MARCUS): Degenerates without.... don't know why + + coefficients(rowIdx, 0) = -1; + coefficients(rowIdx, 1) = 0; + coefficients(rowIdx, 2) = 0; + coefficients(rowIdx, 3) = 0; + coefficients(rowIdx, 4) = 0; + coefficients(rowIdx, 5) = 0; + coefficients(rowIdx, 6) = x2 + a11 * x1; + coefficients(rowIdx, 7) = a11 * y1; + coefficients(rowIdx, 8) = a11; + ++rowIdx; + + coefficients(rowIdx, 0) = 0; + coefficients(rowIdx, 1) = -1; + coefficients(rowIdx, 2) = 0; + coefficients(rowIdx, 3) = 0; + coefficients(rowIdx, 4) = 0; + coefficients(rowIdx, 5) = 0; + coefficients(rowIdx, 6) = a12 * x1; + coefficients(rowIdx, 7) = x2 + a12 * y1; + coefficients(rowIdx, 8) = a12; + ++rowIdx; + + coefficients(rowIdx, 0) = 0; + coefficients(rowIdx, 1) = 0; + coefficients(rowIdx, 2) = 0; + coefficients(rowIdx, 3) = -1; + coefficients(rowIdx, 4) = 0; + coefficients(rowIdx, 5) = 0; + coefficients(rowIdx, 6) = y2 + a21 * x1; + coefficients(rowIdx, 7) = a21 * y1; + coefficients(rowIdx, 8) = a21; + ++rowIdx; + + coefficients(rowIdx, 0) = 0; + coefficients(rowIdx, 1) = 0; + coefficients(rowIdx, 2) = 0; + coefficients(rowIdx, 3) = 0; + coefficients(rowIdx, 4) = -1; + coefficients(rowIdx, 5) = 0; + coefficients(rowIdx, 6) = a22 * x1; + coefficients(rowIdx, 7) = y2 + a22 * y1; + coefficients(rowIdx, 8) = a22; + ++rowIdx; + } + + Eigen::JacobiSVD svd(coefficients,Eigen::ComputeThinV); + + const Eigen::Matrix &h = svd.matrixV().rightCols<1>(); + + Eigen::Matrix3d H; + H << h(0), h(1), h(2), + h(3), h(4), h(5), + h(6), h(7), h(8); + H /= H(2,2); + + std::vector output; + HomLib::PoseData pd; + pd.homography = H; + pd.distortion_parameter = 0.0; + pd.distortion_parameter2 = 0.0; + output.push_back(pd); + + return output; + } +} // namespace BarathVISAPP2016 +} // namespace HomLib diff --git a/HomLib/solvers/valtonenornhag_icpr_2026/get_valtonenornhag_icpr_2026_affine.cpp b/HomLib/solvers/valtonenornhag_icpr_2026/get_valtonenornhag_icpr_2026_affine.cpp new file mode 100644 index 0000000..0688333 --- /dev/null +++ b/HomLib/solvers/valtonenornhag_icpr_2026/get_valtonenornhag_icpr_2026_affine.cpp @@ -0,0 +1,234 @@ +// Copyright (c) 2020 Marcus Valtonen Örnhag +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + + +#include +#include +#include +#include +#include +#include "posedata.hpp" + +#include "radial.hpp" +#include "get_valtonenornhag_icpr_2026.hpp" + +namespace HomLib { +namespace ValtonenOrnhagICPR2026 { + std::vector get_affine( + const std::vector &x, + const std::vector &y, + const std::vector &Aff, + bool extra_check + ) { + + double weight = 1.0; + + // Create M matrix + Eigen::Matrix M(6 * x.size(), 12); + + size_t rowIdx = 0; + for (size_t i = 0; i < x.size(); i++) { + const double + x1 = x[i](0), + y1 = x[i](1), + x2 = y[i](0), + y2 = y[i](1), + a11 = Aff[i](0,0), + a12 = Aff[i](0,1), + a21 = Aff[i](1,0), + a22 = Aff[i](1,1); + + const double r12 = x1 * x1 + y1 * y1; + + const double + kMinusWeightTimesR12 = -weight * r12, + kMinusWeightTimesX1 = -weight * x1, + kMinusWeightTimesY1 = -weight * y1, + kWeightTimesX2 = weight * x2, + kWeightTimesY2 = weight * y2; + + M(rowIdx, 0) = kMinusWeightTimesR12; + M(rowIdx, 1) = kMinusWeightTimesX1; + M(rowIdx, 2) = kMinusWeightTimesY1; + M(rowIdx, 3) = -weight; + M(rowIdx, 4) = 0; + M(rowIdx, 5) = 0; + M(rowIdx, 6) = 0; + M(rowIdx, 7) = 0; + M(rowIdx, 8) = kWeightTimesX2 * r12; + M(rowIdx, 9) = kWeightTimesX2 * x1; + M(rowIdx, 10) = kWeightTimesX2 * y1; + M(rowIdx, 11) = kWeightTimesX2; + ++rowIdx; + + M(rowIdx, 0) = 0; + M(rowIdx, 1) = 0; + M(rowIdx, 2) = 0; + M(rowIdx, 3) = 0; + M(rowIdx, 4) = kMinusWeightTimesR12; + M(rowIdx, 5) = kMinusWeightTimesX1; + M(rowIdx, 6) = kMinusWeightTimesY1; + M(rowIdx, 7) = -weight; + M(rowIdx, 8) = kWeightTimesY2 * r12; + M(rowIdx, 9) = kWeightTimesY2 * x1; + M(rowIdx, 10) = kWeightTimesY2 * y1; + M(rowIdx, 11) = kWeightTimesY2; + ++rowIdx; + + // If the minimal case is considered, we + // do not need all constraints to estimate + // the homography. + //if (i == 1) { + // break; + //} + + // NOTE(MARCUS): Degenerates without.... don't know why + + M(rowIdx, 0) = -2 * x1; + M(rowIdx, 1) = -1; + M(rowIdx, 2) = 0; + M(rowIdx, 3) = 0; + M(rowIdx, 4) = 0; + M(rowIdx, 5) = 0; + M(rowIdx, 6) = 0; + M(rowIdx, 7) = 0; + M(rowIdx, 8) = 2 * x1 * x2 + a11 * r12; + M(rowIdx, 9) = x2 + a11 * x1; + M(rowIdx, 10) = a11 * y1; + M(rowIdx, 11) = a11; + ++rowIdx; + + M(rowIdx, 0) = -2 * y1; + M(rowIdx, 1) = 0; + M(rowIdx, 2) = -1; + M(rowIdx, 3) = 0; + M(rowIdx, 4) = 0; + M(rowIdx, 5) = 0; + M(rowIdx, 6) = 0; + M(rowIdx, 7) = 0; + M(rowIdx, 8) = 2 * y1 * x2 + a12 * r12; + M(rowIdx, 9) = a12 * x1; + M(rowIdx, 10) = x2 + a12 * y1; + M(rowIdx, 11) = a12; + ++rowIdx; + + M(rowIdx, 0) = 0; + M(rowIdx, 1) = 0; + M(rowIdx, 2) = 0; + M(rowIdx, 3) = 0; + M(rowIdx, 4) = -2 * x1; + M(rowIdx, 5) = -1; + M(rowIdx, 6) = 0; + M(rowIdx, 7) = 0; + M(rowIdx, 8) = 2 * x1 * y2 + a21 * r12; + M(rowIdx, 9) = y2 + a21 * x1; + M(rowIdx, 10) = a21 * y1; + M(rowIdx, 11) = a21; + ++rowIdx; + + M(rowIdx, 0) = 0; + M(rowIdx, 1) = 0; + M(rowIdx, 2) = 0; + M(rowIdx, 3) = 0; + M(rowIdx, 4) = -2 * y1; + M(rowIdx, 5) = 0; + M(rowIdx, 6) = -1; + M(rowIdx, 7) = 0; + M(rowIdx, 8) = 2 * y1 * y2 + a22 * r12; + M(rowIdx, 9) = a22 * x1; + M(rowIdx, 10) = y2 + a22 * y1; + M(rowIdx, 11) = a22; + ++rowIdx; + } + + // Compute nullspace using QR + Eigen::Matrix Q = M.transpose().householderQr().householderQ(); + + std::vector output; + if (M.rows() < 12) { + Eigen::Matrix N = Q.rightCols(3); + + // Create generalized eigenvalue problem + Eigen::Matrix3d A; + A << N.row(0), N.row(4), N.row(8); + Eigen::Matrix3d B; + B << N.row(3), N.row(7), N.row(11); + + Eigen::GeneralizedEigenSolver ges; + ges.compute(A, B, true); + Eigen::Vector3cd ks = ges.eigenvalues(); + Eigen::Matrix3d X = ges.eigenvectors().real(); + + // Keep only real solutions (up to 3) + for (int i = 0; i < 3; i++) { + if (std::abs(ks(i).imag()) < 1e-14) { + double k = ks(i).real(); + Eigen::Vector3d alpha = X.col(i); + Eigen::Matrix g = alpha[0] * N.col(0) + alpha[1] * N.col(1) + alpha[2] * N.col(2); + Eigen::Matrix G = Eigen::Map>(g.data()).transpose(); + Eigen::Matrix3d H = G.bottomRightCorner(3, 3); + + // Package output + HomLib::PoseData pd; + pd.homography = H.inverse(); + pd.distortion_parameter = k; + output.push_back(pd); + } + } + + // Compute reprojection error using the unused constraint + if (extra_check) { + double min_res = std::numeric_limits::max(); + int best_id = -1; + for (size_t i = 0; i < output.size(); i++) { + // Measure reprojection error in undistorted space + Eigen::Vector2d y4u_est1 = HomLib::radialundistort(y[4], output[i].distortion_parameter); + Eigen::Vector3d tmp = output[i].homography * x[4].homogeneous(); + Eigen::Vector2d y4u_est2 = tmp.hnormalized(); + double res = (y4u_est1 - y4u_est2).squaredNorm(); + if (res < min_res) { + min_res = res; + best_id = i; + } + } + std::vector output2; + if (best_id >= 0) { + output2.push_back(output[best_id]); + } + return output2; + } + } else { + // Non-minimal + Eigen::Matrix g = Q.rightCols(1); + Eigen::Matrix G = Eigen::Map>(g.data()).transpose(); + Eigen::Matrix3d H = G.bottomRightCorner(3, 3); + // Package output + HomLib::PoseData pd; + pd.homography = H; // Note: not inverse now + pd.distortion_parameter = (double) (G.col(0).transpose() * G.col(3)) / G.col(3).squaredNorm(); + pd.distortion_parameter2 = 0.0; + pd.focal_length = 0.0; + output.push_back(pd); + } + return output; + } + +} // namespace ValtonenOrnhagICPR2026 +} // namespace HomLib diff --git a/HomLib/solvers/valtonenornhag_icpr_2026/get_valtonenornhag_icpr_2026_ori.cpp b/HomLib/solvers/valtonenornhag_icpr_2026/get_valtonenornhag_icpr_2026_ori.cpp new file mode 100644 index 0000000..00fb6f2 --- /dev/null +++ b/HomLib/solvers/valtonenornhag_icpr_2026/get_valtonenornhag_icpr_2026_ori.cpp @@ -0,0 +1,161 @@ +// Copyright (c) 2020 Marcus Valtonen Örnhag +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + + +#include +#include +#include +#include +#include +#include +#include "posedata.hpp" + +#include "radial.hpp" +#include "get_valtonenornhag_icpr_2026.hpp" + +namespace HomLib { +namespace ValtonenOrnhagICPR2026 { + std::vector get_ori( + const std::vector &x, + const std::vector &y, + const std::vector &ori + ) { + + double weight = 1.0; + + // Create M matrix + Eigen::Matrix M(3 * x.size(), 12); + + size_t rowIdx = 0; + for (size_t i = 0; i < x.size(); i++) { + double + x1 = x[i](0), + y1 = x[i](1), + x2 = y[i](0), + y2 = y[i](1), + s1 = std::sin(ori[i](0)), + s2 = std::sin(ori[i](1)), + c1 = std::cos(ori[i](0)), + c2 = std::cos(ori[i](1)); + + double r12 = x1 * x1 + y1 * y1; + + double + kMinusWeightTimesR12 = -weight * r12, + kMinusWeightTimesX1 = -weight * x1, + kMinusWeightTimesY1 = -weight * y1, + kWeightTimesX2 = weight * x2, + kWeightTimesY2 = weight * y2; + + M(rowIdx, 0) = kMinusWeightTimesR12; + M(rowIdx, 1) = kMinusWeightTimesX1; + M(rowIdx, 2) = kMinusWeightTimesY1; + M(rowIdx, 3) = -weight; + M(rowIdx, 4) = 0; + M(rowIdx, 5) = 0; + M(rowIdx, 6) = 0; + M(rowIdx, 7) = 0; + M(rowIdx, 8) = kWeightTimesX2 * r12; + M(rowIdx, 9) = kWeightTimesX2 * x1; + M(rowIdx, 10) = kWeightTimesX2 * y1; + M(rowIdx, 11) = kWeightTimesX2; + ++rowIdx; + + M(rowIdx, 0) = 0; + M(rowIdx, 1) = 0; + M(rowIdx, 2) = 0; + M(rowIdx, 3) = 0; + M(rowIdx, 4) = kMinusWeightTimesR12; + M(rowIdx, 5) = kMinusWeightTimesX1; + M(rowIdx, 6) = kMinusWeightTimesY1; + M(rowIdx, 7) = -weight; + M(rowIdx, 8) = kWeightTimesY2 * r12; + M(rowIdx, 9) = kWeightTimesY2 * x1; + M(rowIdx, 10) = kWeightTimesY2 * y1; + M(rowIdx, 11) = kWeightTimesY2; + ++rowIdx; + + M(rowIdx, 0) = -2 * (x1*s2*c1 + y1*s1*s2); + M(rowIdx, 1) = -s2 * c1; + M(rowIdx, 2) = -s1 * s2; + M(rowIdx, 3) = 0; + M(rowIdx, 4) = 2 * (x1*c1*c2 + y1*s1*c2); + M(rowIdx, 5) = c1*c2; + M(rowIdx, 6) = s1*c2; + M(rowIdx, 7) = 0; + M(rowIdx, 8) = 2*x1*(x2*s2*c1 - y2*c1*c2) + 2*y1*(x2*s1*s2 - y2*s1*c2); + M(rowIdx, 9) = x2*s2*c1 - y2*c1*c2; + M(rowIdx, 10) = x2*s1*s2 - y2*s1*c2; + M(rowIdx, 11) = 0; + ++rowIdx; + } + + // Compute nullspace using QR + Eigen::Matrix Q = M.transpose().householderQr().householderQ(); + + std::vector output; + if (M.rows() < 12) { + Eigen::Matrix N = Q.rightCols(3); + + // Create generalized eigenvalue problem + Eigen::Matrix3d A; + A << N.row(0), N.row(4), N.row(8); + Eigen::Matrix3d B; + B << N.row(3), N.row(7), N.row(11); + + Eigen::GeneralizedEigenSolver ges; + ges.compute(A, B, true); + Eigen::Vector3cd ks = ges.eigenvalues(); + Eigen::Matrix3d X = ges.eigenvectors().real(); + + // Keep only real solutions (up to 3) + for (int i = 0; i < 3; i++) { + if (std::abs(ks(i).imag()) < 1e-14) { + double k = ks(i).real(); + Eigen::Vector3d alpha = X.col(i); + Eigen::Matrix g = alpha[0] * N.col(0) + alpha[1] * N.col(1) + alpha[2] * N.col(2); + Eigen::Matrix G = Eigen::Map>(g.data()).transpose(); + Eigen::Matrix3d H = G.bottomRightCorner(3, 3); + + // Package output + HomLib::PoseData pd; + pd.homography = H.inverse(); + pd.distortion_parameter = k; + output.push_back(pd); + } + } + } else { + // Non-minimal + Eigen::Matrix g = Q.rightCols(1); + Eigen::Matrix G = Eigen::Map>(g.data()).transpose(); + Eigen::Matrix3d H = G.bottomRightCorner(3, 3); + // Package output + HomLib::PoseData pd; + pd.homography = H; // Note: not inverse now + pd.distortion_parameter = (double) (G.col(0).transpose() * G.col(3)) / G.col(3).squaredNorm(); + pd.distortion_parameter2 = 0.0; + pd.focal_length = 0.0; + output.push_back(pd); + } + return output; + } + +} // namespace ValtonenOrnhagICPR2026 +} // namespace HomLib diff --git a/README.md b/README.md index 9d0ca08..3bb3b19 100644 --- a/README.md +++ b/README.md @@ -2,6 +2,7 @@ ![GitHub release (latest by date)](https://img.shields.io/github/v/release/marcusvaltonen/HomLib) ![GitHub](https://img.shields.io/github/license/marcusvaltonen/HomLib) ![PyPI](https://img.shields.io/pypi/v/homlib) +[![Documentation Status](https://readthedocs.org/projects/homlib/badge/?version=latest)](https://homlib.readthedocs.io/en/latest/?badge=latest) C++ library for computing homographies with support in MATLAB and Python. @@ -10,24 +11,29 @@ C++ library for computing homographies with support in MATLAB and Python. This repository contains the following solvers for computing homographies with simultaneous radial distortion correction and/or incorporating IMU data. -| Authors (year) | Number of points | Minimal | Radial distortion coeff. | IMU data | General homography | Separarate intrinsic/extrinsic | -| ----------------------------- | ---------------- | ------------------ | ------------------------ | ------------------ | ------------------ | ------------------------------ | -| Fitzgibbon (2001) | 5 | | :heavy_check_mark: (e) | | :heavy_check_mark: | | -| | 5 | | :heavy_check_mark: (1) | | :heavy_check_mark: | | -| Kukelova et al. (2015) | 5 | :heavy_check_mark: | :heavy_check_mark: (2) | | :heavy_check_mark: | | -| | 6 | | :heavy_check_mark: (2) | | :heavy_check_mark: | | -| Valtonen Örnhag et al. (2020) | 4 | :heavy_check_mark: | | :heavy_check_mark: | | :heavy_check_mark: | -| Valtonen Örnhag et al. (2021) | 3 | :heavy_check_mark: | | :heavy_check_mark: | | :heavy_check_mark: | -| | 4 | :heavy_check_mark: | :heavy_check_mark: (e) | :heavy_check_mark: | | :heavy_check_mark: | -| Nakano (2024) | 5 | :heavy_check_mark: | :heavy_check_mark: (1) | | :heavy_check_mark: | | -| Wadenbäck et al. (2026) | 5 | :heavy_check_mark: | :heavy_check_mark: (1) | | :heavy_check_mark: | | -| | 5 | :heavy_check_mark: | :heavy_check_mark: (e) | | :heavy_check_mark: | | -| | 5 | :heavy_check_mark: | :heavy_check_mark: (2) | | :heavy_check_mark: | | - -We use the following convention for the different cases: (1) - single-sided, (e) two-sided and equal, and (2) two-sided. - -> [!NOTE] -> New paper to be presented at ICPR 2026. Code is coming soon. +We further support affine-covariant and orientation-covariant solvers, requiring +fewer correspondences than their point-based counterparts by leveraging the geometric +nformation of the corresponding descriptors. + +| Authors (year) | #Corrs | Minimal | Radial distortion coeff. | IMU data | General homography | Descriptor | +| ---------------------------------------- | ------ | ------------------ | ------------------------ | ------------------ | ------------------ | ---------- | +| Fitzgibbon (2001) | 5 | | :heavy_check_mark: (e) | | :heavy_check_mark: | P | +| | 5 | | :heavy_check_mark: (1) | | :heavy_check_mark: | P | +| Kukelova et al. (2015) | 5 | :heavy_check_mark: | :heavy_check_mark: (2) | | :heavy_check_mark: | P | +| | 6 | | :heavy_check_mark: (2) | | :heavy_check_mark: | P | +| Valtonen Örnhag et al. (2020) | 4 | :heavy_check_mark: | | :heavy_check_mark: | | P | +| Valtonen Örnhag et al. (2021) | 3 | :heavy_check_mark: | | :heavy_check_mark: | | P | +| | 4 | :heavy_check_mark: | :heavy_check_mark: (e) | :heavy_check_mark: | | P | +| Nakano (2024) | 5 | :heavy_check_mark: | :heavy_check_mark: (1) | | :heavy_check_mark: | P | +| Wadenbäck et al. (2026) | 5 | :heavy_check_mark: | :heavy_check_mark: (1) | | :heavy_check_mark: | P | +| | 5 | :heavy_check_mark: | :heavy_check_mark: (e) | | :heavy_check_mark: | P | +| | 5 | :heavy_check_mark: | :heavy_check_mark: (2) | | :heavy_check_mark: | P | +| Barath and Hajder (2016) | 2 | | | | :heavy_check_mark: | A | +| Valtonen Örnhag and Adalbjörnsson (2026) | 2 | | :heavy_check_mark: (1) | | :heavy_check_mark: | A | +| | 3 | :heavy_check_mark: | :heavy_check_mark: (1) | | :heavy_check_mark: | O | + +We use the following convention for the different distortion cases: (1) - single-sided, (e) two-sided and equal, and (2) two-sided. +The following descriptor types: (P) - point-based, (A) affine-covariant, and (O) orientation-covariant. The solvers by Valtonen Örnhag et al. and Wadenbäck et al. are original implementations, the others are re-implementations. If you use the code in your work, please cite diff --git a/examples/example.cpp b/examples/example.cpp index cd0b76a..f72fb8a 100644 --- a/examples/example.cpp +++ b/examples/example.cpp @@ -27,12 +27,14 @@ #include #include #include +#include "get_barath_visapp_2016.hpp" #include "get_fitzgibbon_cvpr_2001.hpp" #include "get_kukelova_cvpr_2015.hpp" #include "get_nakano_icpr_2025.hpp" // #include "get_valtonenornhag_icpr_2020.hpp" // #include "get_valtonenornhag_wacv_2021.hpp" #include "get_wadenback_3dv_2026.hpp" +#include "get_valtonenornhag_icpr_2026.hpp" #include "problem_instance.hpp" #include "generate_problem_instance.hpp" #include "posedata.hpp" @@ -98,7 +100,21 @@ struct SolverWadenbackOne { return HomLib::Wadenback3DV2026::get_one_sided(inst.x1, inst.x2, false); } }; - +struct SolverAffineNoDist { + static inline std::vector solve(const HomLib::ProblemInstance inst) { + return HomLib::BarathVISAPP2016::get_affine(inst.x1, inst.x2, inst.A); + } +}; +struct SolverAffineWithDist { + static inline std::vector solve(const HomLib::ProblemInstance inst) { + return HomLib::ValtonenOrnhagICPR2026::get_affine(inst.x1, inst.x2, inst.A, false); + } +}; +struct SolverOrientation { + static inline std::vector solve(const HomLib::ProblemInstance inst) { + return HomLib::ValtonenOrnhagICPR2026::get_ori(inst.x1, inst.x2, inst.ori); + } +}; template void print_csv_file(std::string name, std::vector v) { std::ofstream fd(name); @@ -142,11 +158,7 @@ template BenchmarkResults benchmark_solver(HomLib::ProblemConf for (size_t j = 0; j < pd.size(); j++) { hom_err_put.push_back(inst.hom_error(pd[j].homography)); - if (config.one_sided || config.equal) { - dist_err_put.push_back(inst.dist_error(pd[j].distortion_parameter)); - } else { - dist_err_put.push_back(inst.dist_error(pd[j].distortion_parameter, pd[j].distortion_parameter2)); - } + dist_err_put.push_back(inst.dist_error(pd[j].distortion_parameter, pd[j].distortion_parameter2)); } // Handle case when no solution is found @@ -199,8 +211,8 @@ int main(int argc, char *argv[]) { std::cout << "================================== BENCHMARKING ==================================" << std::endl; std::cout << "nbr_iter = " << nbr_iter << " and point_noise = " << point_noise << std::endl; std::cout << "\nDOUBLE (NOT EQUAL)" << std::endl; - config.one_sided = false; - config.equal = false; + + config.distortion = HomLib::DistortionCase::TWO_SIDED; std::cout << "Kukelova et al., CVPR 2015 (5 pt)" << std::endl; br = benchmark_solver(config, nbr_iter); @@ -220,8 +232,7 @@ int main(int argc, char *argv[]) { print_files(br, point_noise, "wadenback_two_sided"); std::cout << "\nDOUBLE (EQUAL)" << std::endl; - config.one_sided = false; - config.equal = true; + config.distortion = HomLib::DistortionCase::TWO_SIDED_EQUAL; std::cout << "Fitzgibbon, CVPR 2001 (5 pt)" << std::endl; br = benchmark_solver(config, nbr_iter); @@ -246,8 +257,7 @@ int main(int argc, char *argv[]) { print_files(br, point_noise, "wadenback_two_sided_equal"); std::cout << "\nSINGLE" << std::endl; - config.one_sided = true; // Does not matter - config.equal = true; + config.distortion = HomLib::DistortionCase::ONE_SIDED_LEFT; std::cout << "Nakano 2025, ICPR 2025 (4.5 pt)" << std::endl; br = benchmark_solver(config, nbr_iter); @@ -263,10 +273,41 @@ int main(int argc, char *argv[]) { br = benchmark_solver(config, nbr_iter); if (print_to_file) print_files(br, point_noise, "wadenback_one_sided"); + + + // Affine solvers + config.number_points = 2; + config.distortion = HomLib::DistortionCase::NO_DISTORTION; + + std::cout << "Affine no dist (Barath et al.)" << std::endl; + br = benchmark_solver(config, nbr_iter); + if (print_to_file) + print_files(br, point_noise, "affine_no_dist"); + + std::cout << "Affine dist (Valtonen Ornhag, ICPR 2026) - no dist added" << std::endl; + br = benchmark_solver(config, nbr_iter); + if (print_to_file) + print_files(br, point_noise, "affine_dist_no_dist"); + + config.distortion = HomLib::DistortionCase::ONE_SIDED_RIGHT; + + std::cout << "Affine dist (Valtonen Ornhag, ICPR 2026)" << std::endl; + br = benchmark_solver(config, nbr_iter); + if (print_to_file) + print_files(br, point_noise, "affine_dist"); + + // Orientation solver + std::cout << "Orientation" << std::endl; + config.number_points = 4; + + br = benchmark_solver(config, nbr_iter); + if (print_to_file) + print_files(br, point_noise, "ori"); std::cout << "\n\nNOTE: Errors are log10" << std::endl; - std::cout << "\n\nTesting nonlinear refinement - one sided" << std::endl; + std::cout << "\n\n==== Testing JACOBIANS ====" << std::endl; + std::cout << "\n\nTesting nonlinear refinement - one sided left" << std::endl; config.number_points = 12; HomLib::ProblemInstance inst = HomLib::generate_problem_instance(config); @@ -274,7 +315,8 @@ int main(int argc, char *argv[]) { HomLib::PoseData pd; pd.homography = inst.posedata.homography; - pd.distortion_parameter = inst.posedata.distortion_parameter2; + pd.distortion_parameter = 0.0; + pd.distortion_parameter2 = inst.posedata.distortion_parameter2; pd.homography(0,0) += 0.001; pd.homography(1,0) -= 0.001; @@ -285,18 +327,45 @@ int main(int argc, char *argv[]) { pd.homography(0,2) -= 0.0001; pd.homography(1,2) += 0.002; pd.homography(2,2) += 0.001; - pd.distortion_parameter -= 0.0005; + pd.distortion_parameter2 -= 0.0005; - double error_before = inst.hom_error(pd.homography) + inst.dist_error(pd.distortion_parameter); + double error_before = inst.hom_error(pd.homography) + inst.dist_error(pd.distortion_parameter, pd.distortion_parameter2); std::cout << "before error: " << error_before << std::endl; HomLib::refinement_onesided(inst.x1, inst.x2, pd); - double error_after = inst.hom_error(pd.homography) + inst.dist_error(pd.distortion_parameter); + double error_after = inst.hom_error(pd.homography) + inst.dist_error(pd.distortion_parameter, pd.distortion_parameter2); + std::cout << "before after: " << error_after << std::endl; + + std::cout << "\n\nTesting nonlinear refinement - one sided right" << std::endl; + config.number_points = 12; + config.distortion = HomLib::DistortionCase::ONE_SIDED_RIGHT; + inst = HomLib::generate_problem_instance(config); + + std::cout << "Number of points: " << inst.x1.size() << std::endl; + + pd.homography = inst.posedata.homography; + pd.distortion_parameter = inst.posedata.distortion_parameter; + pd.distortion_parameter2 = 0.0; + + pd.homography(0,0) += 0.001; + pd.homography(1,0) -= 0.001; + pd.homography(2,0) += 0.002; + pd.homography(0,1) += 0.001; + pd.homography(1,1) += 0.001; + pd.homography(2,1) -= 0.001; + pd.homography(0,2) -= 0.0001; + pd.homography(1,2) += 0.002; + pd.homography(2,2) += 0.001; + pd.distortion_parameter -= 0.0005; + + error_before = inst.hom_error(pd.homography) + inst.dist_error(pd.distortion_parameter, pd.distortion_parameter2); + std::cout << "before error: " << error_before << std::endl; + HomLib::refinement_onesided_right(inst.x1, inst.x2, pd); + error_after = inst.hom_error(pd.homography) + inst.dist_error(pd.distortion_parameter, pd.distortion_parameter2); std::cout << "before after: " << error_after << std::endl; std::cout << "\n\nTesting nonlinear refinement - two sided equal" << std::endl; config.number_points = 12; - config.one_sided = false; - config.equal = true; + config.distortion = HomLib::DistortionCase::TWO_SIDED_EQUAL; inst = HomLib::generate_problem_instance(config); std::cout << "Number of points: " << inst.x1.size() << std::endl; @@ -314,17 +383,17 @@ int main(int argc, char *argv[]) { pd.homography(1,2) += 0.002; pd.homography(2,2) += 0.001; pd.distortion_parameter -= 0.0005; + pd.distortion_parameter2 -= 0.0005; - error_before = inst.hom_error(pd.homography) + inst.dist_error(pd.distortion_parameter); + error_before = inst.hom_error(pd.homography) + inst.dist_error(pd.distortion_parameter, pd.distortion_parameter2); std::cout << "before error: " << error_before << std::endl; HomLib::refinement_twosided_equal(inst.x1, inst.x2, pd); - error_after = inst.hom_error(pd.homography) + inst.dist_error(pd.distortion_parameter); + error_after = inst.hom_error(pd.homography) + inst.dist_error(pd.distortion_parameter, pd.distortion_parameter2); std::cout << "before after: " << error_after << std::endl; std::cout << "\n\nTesting nonlinear refinement - two sided" << std::endl; config.number_points = 12; - config.one_sided = false; - config.equal = false; + config.distortion = HomLib::DistortionCase::TWO_SIDED; inst = HomLib::generate_problem_instance(config); std::cout << "Number of points: " << inst.x1.size() << std::endl; @@ -351,5 +420,92 @@ int main(int argc, char *argv[]) { error_after = inst.hom_error(pd.homography) + inst.dist_error(pd.distortion_parameter, pd.distortion_parameter2); std::cout << "before after: " << error_after << std::endl; + std::cout << "\n\nTesting nonlinear refinement - no dist" << std::endl; + config.number_points = 12; + config.distortion = HomLib::DistortionCase::NO_DISTORTION; + inst = HomLib::generate_problem_instance(config); + + std::cout << "Number of points: " << inst.x1.size() << std::endl; + + pd.homography = inst.posedata.homography; + pd.distortion_parameter = inst.posedata.distortion_parameter; + pd.distortion_parameter2 = inst.posedata.distortion_parameter2; + + pd.homography(0,0) += 0.001; + pd.homography(1,0) -= 0.001; + pd.homography(2,0) += 0.002; + pd.homography(0,1) += 0.001; + pd.homography(1,1) += 0.001; + pd.homography(2,1) -= 0.001; + pd.homography(0,2) -= 0.0001; + pd.homography(1,2) += 0.002; + pd.homography(2,2) += 0.001; + + error_before = inst.hom_error(pd.homography) + inst.dist_error(pd.distortion_parameter, pd.distortion_parameter2); + std::cout << "before error: " << error_before << std::endl; + HomLib::refinement_no_dist(inst.x1, inst.x2, pd); + error_after = inst.hom_error(pd.homography) + inst.dist_error(pd.distortion_parameter, pd.distortion_parameter2); + std::cout << "before after: " << error_after << std::endl; + + std::cout << "\n\n==== Testing Guo's method vs DLT ====" << std::endl; + config.number_points = 4; + config.distortion = HomLib::DistortionCase::NO_DISTORTION; + inst = HomLib::generate_problem_instance(config); + + int n_prob = 1e4; + + std::vector runtimes_guo; + runtimes_guo.reserve(n_prob); + std::vector err_guo; + err_guo.reserve(n_prob); + std::vector runtimes_dlt; + runtimes_dlt.reserve(n_prob); + std::vector err_dlt; + err_dlt.reserve(n_prob); + + + for (int i = 0; i < n_prob; i++) { + auto start = std::chrono::high_resolution_clock::now(); + Eigen::Matrix3d H1 = HomLib::Wadenback3DV2026::homography_4pt_guo(inst.x1, inst.x2); + auto end = std::chrono::high_resolution_clock::now(); + runtimes_guo.push_back(std::chrono::duration_cast(end - start).count()); + err_guo.push_back(inst.hom_error(H1)); + Eigen::Matrix3d H2; + + std::vector x1, x2; + Eigen::Vector3d tmp; + for (int i = 0; i < 4; i++) { + tmp = inst.x1[i].homogeneous(); + x1.push_back(tmp); + tmp = inst.x2[i].homogeneous(); + x2.push_back(tmp); + } + start = std::chrono::high_resolution_clock::now(); + poselib::homography_4pt(x1, x2, &H2, false); + end = std::chrono::high_resolution_clock::now(); + runtimes_dlt.push_back(std::chrono::duration_cast(end - start).count()); + err_dlt.push_back(inst.hom_error(H2)); + } + + std::sort(runtimes_guo.begin(), runtimes_guo.end()); + long runtime_guo_median = runtimes_guo[runtimes_guo.size() / 2]; + std::sort(err_guo.begin(), err_guo.end()); + double err_guo_median = err_guo[err_guo.size() / 2]; + + std::sort(runtimes_dlt.begin(), runtimes_dlt.end()); + long runtime_dlt_median = runtimes_dlt[runtimes_dlt.size() / 2]; + std::sort(err_dlt.begin(), err_dlt.end()); + double err_dlt_median = err_dlt[err_dlt.size() / 2]; + + + std::cout << "Guo's method:" << std::endl; + std::cout << "\tMedian execution time: " << runtime_guo_median << " ns" << std::endl; + std::cout << "\tHomography error: " << err_guo_median << " (median)" << std::endl; + std::cout << "DLT method:" << std::endl; + std::cout << "\tMedian execution time: " << runtime_dlt_median << " ns" << std::endl; + std::cout << "\tHomography error: " << err_dlt_median << " (median)" << std::endl; + + + return 0; } diff --git a/examples/example_ransac.cpp b/examples/example_ransac.cpp index 2cd4777..9c6b636 100644 --- a/examples/example_ransac.cpp +++ b/examples/example_ransac.cpp @@ -21,9 +21,12 @@ #include "get_fitzgibbon_cvpr_2001.hpp" #include "get_nakano_icpr_2025.hpp" #include "get_wadenback_3dv_2026.hpp" +#include "get_valtonenornhag_icpr_2026.hpp" #include "get_kukelova_cvpr_2015.hpp" #include "ransac_estimator.h" +#include "affine_ransac_estimator.h" +#include "orientation_ransac_estimator.h" #include "problem_instance.hpp" #include "generate_problem_instance.hpp" @@ -32,7 +35,6 @@ struct BenchmarkResults { std::vector runtimes; std::vector hom_err; std::vector dist_err; - std::vector> inlier_history; }; template void print_csv_file(std::string name, std::vector v) { @@ -62,21 +64,6 @@ void print_files(BenchmarkResults br, int nbr_outliers, std::string method_name) std::string(method_name + "_timing_" + str.str() + ".csv"), br.runtimes ); - - std::ofstream fd(std::string(method_name + "_inlier_history_" + str.str() + ".csv")); - if (fd.is_open()) { - for (size_t i = 0; i < br.inlier_history.size(); i++) { - std::vector v = br.inlier_history[i]; - std::copy(v.begin(), v.end()-1, std::ostream_iterator(fd, ",")); - std::copy(v.end()-1, v.end(), std::ostream_iterator(fd)); - if (i < br.inlier_history.size() -1) { - fd << std::endl; - } - } - fd.close(); - } else { - std::cout << "Unable to open file" << std::endl; - } } template BenchmarkResults test_loransac( @@ -116,8 +103,9 @@ template BenchmarkResults test_loransac( options.final_least_squares_ = false; options.min_num_iterations_ = nbr_ransac_iter; options.max_num_iterations_ = nbr_ransac_iter; - options.lo_starting_iterations_ = nbr_ransac_iter + 1; + options.lo_starting_iterations_ = nbr_ransac_iter+1; options.num_lsq_iterations_ = 0; + options.num_lo_steps_ = 0; std::srand(std::time({})); // use current time as seed for random generator options.random_seed_ = (unsigned int) std::rand(); @@ -130,40 +118,183 @@ template BenchmarkResults test_loransac( auto start = std::chrono::high_resolution_clock::now(); int num_ransac_inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats); - (void)num_ransac_inliers; // Suppress warnings auto end = std::chrono::high_resolution_clock::now(); br.runtimes.push_back(std::chrono::duration_cast(end - start).count()); - + br.hom_err.push_back(inst.hom_error(best_model.homography)); - //std::cout << "Running LOMSAC experiment with " << config.number_points - //<< " points of which " << nbr_outliers << " are outliers for " << name << std::endl; - //std::cout << "Homography error: " << inst.hom_error(best_model.homography) << std::endl; - if (config.one_sided || config.equal) { - br.dist_err.push_back(inst.dist_error(best_model.distortion_parameter)); - //std::cout << "Dist. coeff. error: " << inst.dist_error(best_model.distortion_parameter) << std::endl; - } else { - br.dist_err.push_back(inst.dist_error(best_model.distortion_parameter, best_model.distortion_parameter2)); - //std::cout << "Dist. coeff. error: " << inst.dist_error(best_model.distortion_parameter, best_model.distortion_parameter2) << std::endl; - } + std::cout << "Running LOMSAC experiment with " << config.number_points + << " points of which " << nbr_outliers << " are outliers for " << name << std::endl; + std::cout << "Homography error: " << inst.hom_error(best_model.homography) << std::endl; + br.dist_err.push_back(inst.dist_error(best_model.distortion_parameter, best_model.distortion_parameter2)); + std::cout << "Dist. coeff. error: " << inst.dist_error(best_model.distortion_parameter, best_model.distortion_parameter2) << std::endl; - //br.inlier_history.push_back(ransac_stats.inlier_history); - /* std::cout << " ... LOMSAC found " << num_ransac_inliers << " inliers in " << ransac_stats.num_iterations << " iterations with an inlier ratio of " << ransac_stats.inlier_ratio << std::endl; - std::cout << "number lo iterations " << ransac_stats.number_lo_iterations - << " and inlier history length " << ransac_stats.inlier_history.size() << std::endl; + std::cout << "number lo iterations " << ransac_stats.number_lo_iterations << std::endl; + + if (ransac_stats.inlier_ratio < 0.98 * (1.0 - nbr_outliers / (double) config.number_points)) { + std::cout << "\033[1m\033[31m FAILED!\033[0m\n" << std::endl; + } + + } + return br; +} + + +template BenchmarkResults test_loransac_affine( + std::string name, + Estimator* estimator, + HomLib::ProblemConfig config, + int nbr_outliers, + int nbr_iter, + int nbr_ransac_iter +) { + + BenchmarkResults br; + for (int k = 0; k < nbr_iter; k++) { + HomLib::ProblemInstance inst = HomLib::generate_problem_instance(config); + + std::vector sample; + int n = config.number_points; + for( int i = 0 ; i < n ; ++i ){ + sample.push_back(i); + } + + auto rng = std::default_random_engine {}; + std::shuffle(std::begin(sample), std::end(sample), rng); + + for (int i = 0; i < nbr_outliers; i++) { + Eigen::Vector2d n; + n.setRandom(); + inst.x1[sample[i]] += n * 5000; + n.setRandom(); + inst.x2[sample[i]] += n * 5000; + } + + HomLib::AffineRansacEstimator solver(inst.x1, inst.x2, inst.A, *estimator); + + ransac_lib::LORansacOptions options; + options.squared_inlier_threshold_ = std::pow(0.005, 2); + options.final_least_squares_ = false; + options.min_num_iterations_ = nbr_ransac_iter; + options.max_num_iterations_ = nbr_ransac_iter; + options.lo_starting_iterations_ = nbr_ransac_iter+1; + options.num_lsq_iterations_ = 0; + options.num_lo_steps_ = 0; + std::srand(std::time({})); // use current time as seed for random generator + options.random_seed_ = (unsigned int) std::rand(); + + ransac_lib::LocallyOptimizedMSAC, + HomLib::AffineRansacEstimator> lomsac; + ransac_lib::RansacStatistics ransac_stats; + + HomLib::PoseData best_model; + + auto start = std::chrono::high_resolution_clock::now(); + int num_ransac_inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats); + auto end = std::chrono::high_resolution_clock::now(); + br.runtimes.push_back(std::chrono::duration_cast(end - start).count()); + + br.hom_err.push_back(inst.hom_error(best_model.homography)); + std::cout << "Running LOMSAC experiment with " << config.number_points + << " points of which " << nbr_outliers << " are outliers for " << name << std::endl; + std::cout << "Homography error: " << inst.hom_error(best_model.homography) << std::endl; + br.dist_err.push_back(inst.dist_error(best_model.distortion_parameter, best_model.distortion_parameter2)); + std::cout << "Dist. coeff. error: " << inst.dist_error(best_model.distortion_parameter, best_model.distortion_parameter2) << std::endl; + + std::cout << " ... LOMSAC found " << num_ransac_inliers + << " inliers in " << ransac_stats.num_iterations + << " iterations with an inlier ratio of " + << ransac_stats.inlier_ratio << std::endl; - for (size_t j=0; j < ransac_stats.inlier_history.size(); j++) { - std::cout << ransac_stats.inlier_history[j] << ", "; + std::cout << "number lo iterations " << ransac_stats.number_lo_iterations << std::endl; + + if (ransac_stats.inlier_ratio < 0.98 * (1.0 - nbr_outliers / (double) config.number_points)) { + std::cout << "\033[1m\033[31m FAILED!\033[0m\n" << std::endl; + } + + } + return br; +} + + +template BenchmarkResults test_loransac_ori( + std::string name, + Estimator* estimator, + HomLib::ProblemConfig config, + int nbr_outliers, + int nbr_iter, + int nbr_ransac_iter +) { + + BenchmarkResults br; + for (int k = 0; k < nbr_iter; k++) { + HomLib::ProblemInstance inst = HomLib::generate_problem_instance(config); + + std::vector sample; + int n = config.number_points; + for( int i = 0 ; i < n ; ++i ){ + sample.push_back(i); + } + + auto rng = std::default_random_engine {}; + std::shuffle(std::begin(sample), std::end(sample), rng); + + for (int i = 0; i < nbr_outliers; i++) { + Eigen::Vector2d n; + n.setRandom(); + inst.x1[sample[i]] += n * 5000; + n.setRandom(); + inst.x2[sample[i]] += n * 5000; } + HomLib::OrientationRansacEstimator solver(inst.x1, inst.x2, inst.ori, *estimator); + + ransac_lib::LORansacOptions options; + options.squared_inlier_threshold_ = std::pow(0.005, 2); + options.final_least_squares_ = false; + options.min_num_iterations_ = nbr_ransac_iter; + options.max_num_iterations_ = nbr_ransac_iter; + options.lo_starting_iterations_ = nbr_ransac_iter+1; + options.num_lsq_iterations_ = 0; + options.num_lo_steps_ = 0; + std::srand(std::time({})); // use current time as seed for random generator + options.random_seed_ = (unsigned int) std::rand(); + + ransac_lib::LocallyOptimizedMSAC, + HomLib::OrientationRansacEstimator> lomsac; + ransac_lib::RansacStatistics ransac_stats; + + HomLib::PoseData best_model; + + auto start = std::chrono::high_resolution_clock::now(); + int num_ransac_inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats); + auto end = std::chrono::high_resolution_clock::now(); + br.runtimes.push_back(std::chrono::duration_cast(end - start).count()); + + br.hom_err.push_back(inst.hom_error(best_model.homography)); + std::cout << "Running LOMSAC experiment with " << config.number_points + << " points of which " << nbr_outliers << " are outliers for " << name << std::endl; + std::cout << "Homography error: " << inst.hom_error(best_model.homography) << std::endl; + br.dist_err.push_back(inst.dist_error(best_model.distortion_parameter, best_model.distortion_parameter2)); + std::cout << "Dist. coeff. error: " << inst.dist_error(best_model.distortion_parameter, best_model.distortion_parameter2) << std::endl; + + std::cout << " ... LOMSAC found " << num_ransac_inliers + << " inliers in " << ransac_stats.num_iterations + << " iterations with an inlier ratio of " + << ransac_stats.inlier_ratio << std::endl; + + std::cout << "number lo iterations " << ransac_stats.number_lo_iterations << std::endl; + if (ransac_stats.inlier_ratio < 0.98 * (1.0 - nbr_outliers / (double) config.number_points)) { std::cout << "\033[1m\033[31m FAILED!\033[0m\n" << std::endl; } - */ + } return br; } @@ -198,17 +329,17 @@ int main(int argc, char *argv[]) { HomLib::ProblemConfig config; config.number_points = 500; config.point_noise = point_noise; - config.one_sided = true; - config.equal = true; bool print_to_file = true; BenchmarkResults br; + + std::cout << "======== SINGLE-SIDED ========" << std::endl; + + config.distortion = HomLib::DistortionCase::ONE_SIDED_LEFT; HomLib::FitzgibbonCVPR2001::SolverSingleSided estimator_fitzgibbon_single; HomLib::NakanoICPR2025::SolverSingleSided estimator_nakano_single; HomLib::Wadenback3DV2026::SolverSingleSided estimator_wadenback_single; - - std::cout << "======== SINGLE-SIDED ========" << std::endl; br = test_loransac("fitzgibbon_one_sided", &estimator_fitzgibbon_single, config, nbr_outliers, nbr_iter, nbr_ransac_iter); if (print_to_file) @@ -220,9 +351,19 @@ int main(int argc, char *argv[]) { if (print_to_file) print_files(br, nbr_outliers, "wadenback_one_sided"); + std::cout << "======== SINGLE-SIDED RIGHT ========" << std::endl; + + config.distortion = HomLib::DistortionCase::ONE_SIDED_RIGHT; + + HomLib::NakanoICPR2025::SolverSingleSidedRight estimator_nakano_single_right; + + br = test_loransac("nakano_one_sided_right", &estimator_nakano_single_right, config, nbr_outliers, nbr_iter, nbr_ransac_iter); + if (print_to_file) + print_files(br, nbr_outliers, "nakano_one_sided_right"); + std::cout << "======== TWO-SIDED EQUAL ========" << std::endl; - config.one_sided = false; + config.distortion = HomLib::DistortionCase::TWO_SIDED_EQUAL; HomLib::FitzgibbonCVPR2001::SolverTwoSidedEqual estimator_fitzgibbon_two_sided_equal; HomLib::KukelovaCVPR2015::SolverTwoSidedEqual estimator_kukelova_two_sided_equal; @@ -244,7 +385,7 @@ int main(int argc, char *argv[]) { std::cout << "======== TWO-SIDED ========" << std::endl; - config.equal = false; + config.distortion = HomLib::DistortionCase::TWO_SIDED; HomLib::KukelovaCVPR2015::SolverTwoSided estimator_kukelova_two_sided; HomLib::KukelovaCVPR2015::SolverTwoSided6Pt estimator_kukelova_two_sided_6pt; @@ -259,14 +400,19 @@ int main(int argc, char *argv[]) { br = test_loransac("wadenback_two_sided", &estimator_wadenback_two_sided, config, nbr_outliers, nbr_iter, nbr_ransac_iter); if (print_to_file) print_files(br, nbr_outliers, "wadenback_two_sided"); - - // Check normalization - /* - std::cout << "======== Test... no normalization of image coordinates ========" << std::endl; - estimator_wadenback_two_sided.normalize_image_coord = false; - test_loransac("Wadenback two-sided", &estimator_wadenback_two_sided, config); - */ - - - + + std::cout << "======== AFFINE and ORI ========" << std::endl; + config.distortion = HomLib::DistortionCase::ONE_SIDED_RIGHT; + + HomLib::ValtonenOrnhagICPR2026::AffineSolverSingleSided estimator_affine_dist; + + br = test_loransac_affine("affine", &estimator_affine_dist, config, nbr_outliers, nbr_iter, nbr_ransac_iter); + if (print_to_file) + print_files(br, nbr_outliers, "affine"); + + HomLib::ValtonenOrnhagICPR2026::OrientationSolverSingleSided estimator_ori; + + br = test_loransac_ori("ori", &estimator_ori, config, nbr_outliers, nbr_iter, nbr_ransac_iter); + if (print_to_file) + print_files(br, nbr_outliers, "ori"); } diff --git a/pyproject.toml b/pyproject.toml index cc99360..93bcaea 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "scikit_build_core.build" [project] name = "homlib" -version = "0.1.1" +version = "0.1.2" description = "State-of-the-art homography estimators." readme = "README.md" authors = [ diff --git a/python/example/example_real_data.py b/python/example/example_real_data.py new file mode 100644 index 0000000..cf71e6a --- /dev/null +++ b/python/example/example_real_data.py @@ -0,0 +1,465 @@ +""" +Homography estimation with radial distortion +============================================ + +This example demonstrates: + +1. Loading and pre‑processing two images (including adding a small radial distortion). +2. Detecting SIFT features and matching them with Lowe's ratio test. +3. Estimating a homography with OpenCV RANSAC and several homlib methods. +4. Comparing the results visually and numerically against ground truth. +5. Using AffNet + HardNet features and affine correspondences. + +""" + +import random +import ctypes +from time import time + +import cv2 +import numpy as np +import matplotlib.pyplot as plt + +import homlib + + +############################################################################### +# Constants / Configuration + +IMAGE1_PATH = 'img/grafA.png' +IMAGE2_PATH = 'img/grafB.png' +GT_HOMOGRAPHY_PATH = 'img/graf_model.txt' + +DESIRED_KEYPOINTS = 8000 # Maximum number of SIFT keypoints +DIST_COEFF_GT = -0.000003 # Ground-truth radial distortion coefficient +SNN_THRESHOLD = 0.8 # Lowe's ratio‑test threshold +INLIER_THRESHOLD = 3.0 # Pixel reprojection error for RANSAC + +# Create a reproducible but random seed +RANDOM_SEED = random.randint(0, ctypes.c_uint32(-1).value) + +# homlib RANSAC options +RANSAC_OPTIONS = homlib.LORansacOptions() +RANSAC_OPTIONS.squared_inlier_threshold = INLIER_THRESHOLD ** 2 +RANSAC_OPTIONS.final_least_squares = True +RANSAC_OPTIONS.random_seed = RANDOM_SEED +RANSAC_OPTIONS.lo_starting_iterations = 8 +RANSAC_OPTIONS.min_num_iterations = 70 +RANSAC_OPTIONS.max_num_iterations = 500 + + +############################################################################### +# Image loading and pre‑processing + +def load_images(img1_path, img2_path, dist_coeff): + """Load the two images, apply radial distortion to the first one, and return RGB versions.""" + img1 = cv2.cvtColor(cv2.imread(img1_path), cv2.COLOR_BGR2RGB) + img2 = cv2.cvtColor(cv2.imread(img2_path), cv2.COLOR_BGR2RGB) + + # Intrinsic matrix (assumed identity, principal point at image centre) + K = np.eye(3) + K[0, 2] = img2.shape[1] / 2.0 # cx = width/2 + K[1, 2] = img2.shape[0] / 2.0 # cy = height/2 + R = np.eye(3) + + # Apply radial distortion to img1 only (to simulate a real‑world effect) + map_x, map_y = cv2.initUndistortRectifyMap( + K, + np.array([0, 0, 0, 0, 0, dist_coeff, 0, 0]), + R, + K, + (img2.shape[1], img2.shape[0]), + cv2.CV_32FC1, + ) + img1 = cv2.remap(img1, map_x, map_y, cv2.INTER_LINEAR) + + return img1, img2 + + +def load_ground_truth_homography(path): + """Load ground‑truth homography from a text file and return its inverse.""" + return np.linalg.inv(np.loadtxt(path)) + + +############################################################################### +# Let us look at the images we are working with + +img1, img2 = load_images(IMAGE1_PATH, IMAGE2_PATH, DIST_COEFF_GT) +H_gt = load_ground_truth_homography(GT_HOMOGRAPHY_PATH) + +# Display original images (optional) +plt.figure() +plt.imshow(img1) +plt.title('Image 1 (distorted)') +plt.figure() +plt.imshow(img2) +plt.title('Image 2') + + +########################################################################### +# Feature detection and matching + + +def detect_and_match_sift(img1, img2, max_kpts=DESIRED_KEYPOINTS): + """ + Detect SIFT keypoints/descriptors and match them with a brute‑force + matcher followed by Lowe's ratio test. + """ + sift = cv2.SIFT_create(max_kpts) + kps1, descs1 = sift.detectAndCompute(img1, None) + kps2, descs2 = sift.detectAndCompute(img2, None) + + bf = cv2.BFMatcher() + knn_matches = bf.knnMatch(descs1, descs2, k=2) + + # Apply ratio test and keep only good matches + good_matches = [] + snn_ratios = [] + for m, n in knn_matches: + if m.distance < SNN_THRESHOLD * n.distance: + good_matches.append(m) + snn_ratios.append(m.distance / n.distance) + + # Sort matches by their distance ratio (better matches first) + sorted_indices = np.argsort(snn_ratios) + good_matches = list(np.array(good_matches)[sorted_indices]) + + return kps1, kps2, descs1, descs2, good_matches + + +######################### +# Visualization utilities + +def decolorize(img): + """Convert an RGB image to grayscale and back to RGB (for drawing).""" + gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY) + return cv2.cvtColor(gray, cv2.COLOR_GRAY2RGB) + + +def draw_matches(kps1, kps2, tentatives, img1, img2, H, H_gt, mask, title=""): + """ + Draw tentative correspondences and the estimated/ground‑truth homography + quadrilaterals on the images. + """ + if H is None: + print("No homography found") + return + + matches_mask = mask.ravel().tolist() + + h, w, _ = img1.shape + corners = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1], [w - 1, 0]]).reshape(-1, 1, 2) + + # Project image1 corners to image2 using estimated and ground‑truth homographies + dst_est = cv2.perspectiveTransform(corners, H) + dst_gt = cv2.perspectiveTransform(corners, H_gt) + + # Draw polygons on a copy of image2 + img2_vis = decolorize(img2) + img2_vis = cv2.polylines(img2_vis, [np.int32(dst_est)], True, (255, 0, 0), 3, cv2.LINE_AA) # blue + img2_vis = cv2.polylines(img2_vis, [np.int32(dst_gt)], True, (0, 255, 0), 3, cv2.LINE_AA) # green + + draw_params = dict( + matchColor=(255, 255, 0), # yellow + singlePointColor=None, + matchesMask=matches_mask, + flags=2, + ) + img_out = cv2.drawMatches(decolorize(img1), kps1, img2_vis, kps2, tentatives, None, **draw_params) + + plt.figure(figsize=(12, 8)) + plt.imshow(img_out) + plt.title(title) + plt.axis('off') + + +############################### +# Homography estimation helpers + +def center_points_and_get_transforms(points, width, height): + """ + Shift point coordinates so that (0,0) is at the image centre. + Also return the matrices that transform original homogeneous coordinates + to centered ones (T_center) and back to original (T_back). + + Parameters + ---------- + points : np.ndarray, shape (2, N) + Point coordinates (first row = x, second row = y). + width, height : int + Image dimensions. + + Returns + ------- + points_centered : np.ndarray, shape (2, N) + Shifted point coordinates. + T_center : np.ndarray, shape (3, 3) + Matrix mapping original homogeneous points to centered points. + T_back : np.ndarray, shape (3, 3) + Inverse of T_center, mapping centered points back to original. + """ + shift = np.array([[width / 2.0], [height / 2.0]]) + points_centered = points - shift + + T_center = np.eye(3) + T_center[0, 2] = -width / 2.0 + T_center[1, 2] = -height / 2.0 + + T_back = np.eye(3) + T_back[0, 2] = width / 2.0 + T_back[1, 2] = height / 2.0 + + return points_centered, T_center, T_back + + +def denormalize_homography(H_centered, T1, T2_inv): + """ + Convert a homography estimated in centered coordinates back to original image coordinates. + """ + return T2_inv @ H_centered @ T1 + + +def homography_error(H, H_gt): + """Compute normalised error between two homographies.""" + H_norm = H / np.linalg.norm(H) + H_gt_norm = H_gt / np.linalg.norm(H_gt) + return np.linalg.norm(H_norm - H_gt_norm) + + +def print_estimation_results(method_name, distortion_parameter, H, H_gt): + """Print the common information for every homography estimation method.""" + print(f"homlib {method_name}:") + print(f"H error = {homography_error(H, H_gt):.4e}") + print(f"Dist. coeff. error = {abs(distortion_parameter - DIST_COEFF_GT):.4e}") + + +####################### +# Let us first try OpenCV, which does not handle radial distortion. +# --- SIFT + BFMatcher + ratio test --- +kps1, kps2, _, _, tentatives = detect_and_match_sift(img1, img2) + +def verify_cv2(kps1, kps2, tentatives, H_gt, inlier_thresh=INLIER_THRESHOLD): + """Estimate homography with OpenCV RANSAC.""" + src_pts = np.float32([kps1[m.queryIdx].pt for m in tentatives]).reshape(-1, 1, 2) + dst_pts = np.float32([kps2[m.trainIdx].pt for m in tentatives]).reshape(-1, 1, 2) + + H, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, inlier_thresh) + + print(f"OpenCV RANSAC: {mask.astype(np.float32).sum():.0f} inliers") + print(f"H error = {homography_error(H, H_gt):.4e}") + return H, mask + +t = time() +cv2_H, cv2_mask = verify_cv2(kps1, kps2, tentatives, H_gt) +print(f"{time() - t:.3f} sec (OpenCV RANSAC)\n") +draw_matches(kps1, kps2, tentatives, img1, img2, cv2_H, H_gt, cv2_mask, + title='OpenCV RANSAC') + +####################### +# The results are not terrible, but there is room for improvement. Let us take a look at +# the homlib versions. + +def verify_homlib_point(kps1, kps2, tentatives, H_gt, + width1, height1, width2, height2, + options=RANSAC_OPTIONS): + """Estimate homography with homlib using point correspondences (one‑sided radial distortion).""" + src_pts = np.float32([kps1[m.queryIdx].pt for m in tentatives]).reshape(-1, 2).T + dst_pts = np.float32([kps2[m.trainIdx].pt for m in tentatives]).reshape(-1, 2).T + + # Normalise coordinates to image centre for both images + src_centered, T1, _ = center_points_and_get_transforms(src_pts, width1, height1) + dst_centered, _, T2_inv = center_points_and_get_transforms(dst_pts, width2, height2) + + # This routine assumes the left images is distorted, hence switch places with src and dst + estimate, stats = homlib.lomsac_nakano_icpr_2025_one_sided( + dst_centered, src_centered, options + ) + + # Since we swapped dst and src, we seek the inverse + H = denormalize_homography(np.linalg.inv(estimate.homography), T1, T2_inv) + + mask = np.array([i in stats.inlier_indices for i in range(len(tentatives))], dtype=np.uint8) + + print_estimation_results("point method", estimate.distortion_parameter2, H, H_gt) + return H, mask + + +def verify_homlib_ori(kps1, kps2, tentatives, H_gt, + width1, height1, width2, height2, + options=RANSAC_OPTIONS): + """Estimate homography with homlib using point correspondences + keypoint orientations.""" + src_pts = np.float32([kps1[m.queryIdx].pt for m in tentatives]).reshape(-1, 2).T + dst_pts = np.float32([kps2[m.trainIdx].pt for m in tentatives]).reshape(-1, 2).T + + # Orientation data (in radians) + src_ori = np.float32([kps1[m.queryIdx].angle for m in tentatives]) + dst_ori = np.float32([kps2[m.trainIdx].angle for m in tentatives]) + ori = np.vstack((src_ori, dst_ori)) / 180.0 * np.pi + + src_centered, T1, _ = center_points_and_get_transforms(src_pts, width1, height1) + dst_centered, _, T2_inv = center_points_and_get_transforms(dst_pts, width2, height2) + + estimate, stats = homlib.lomsac_valtonenornhag_icpr_2026_one_sided_ori( + src_centered, dst_centered, ori, options + ) + + H = denormalize_homography(estimate.homography, T1, T2_inv) + + mask = np.array([i in stats.inlier_indices for i in range(len(tentatives))], dtype=np.uint8) + + print_estimation_results("orientation method", estimate.distortion_parameter, H, H_gt) + return H, mask + + +# --- 2. homlib: point‑based, one‑sided radial distortion --- +t = time() +homlib_H, homlib_mask = verify_homlib_point( + kps1, kps2, tentatives, H_gt, + img1.shape[1], img1.shape[0], img2.shape[1], img2.shape[0] +) +print(f"{time() - t:.3f} sec (homlib point)\n") +draw_matches(kps1, kps2, tentatives, img1, img2, homlib_H, H_gt, homlib_mask, + title='homlib point') + +# --- 3. homlib: point + orientation --- +t = time() +homlib_H_ori, homlib_mask_ori = verify_homlib_ori( + kps1, kps2, tentatives, H_gt, + img1.shape[1], img1.shape[0], img2.shape[1], img2.shape[0] +) +print(f"{time() - t:.3f} sec (homlib orientation)\n") +draw_matches(kps1, kps2, tentatives, img1, img2, homlib_H_ori, H_gt, homlib_mask_ori, + title='homlib orientation') + +############################################################ +# We can use affine features too. Let's compute them first. + + +def verify_homlib_affine(src_pts, dst_pts, A, tentatives, H_gt, + width1, height1, width2, height2, + options=RANSAC_OPTIONS): + """Estimate homography using affine correspondences (ACs).""" + # src_pts and dst_pts are 2xN arrays + src_centered, T1, _ = center_points_and_get_transforms(src_pts.T, width1, height1) + dst_centered, _, T2_inv = center_points_and_get_transforms(dst_pts.T, width2, height2) + + estimate, stats = homlib.lomsac_valtonenornhag_icpr_2026_one_sided_affine( + src_centered, dst_centered, A.T, options + ) + + H = denormalize_homography(estimate.homography, T1, T2_inv) + + mask = np.array([i in stats.inlier_indices for i in range(len(tentatives))], dtype=np.uint8) + + print_estimation_results("affine method", estimate.distortion_parameter, H, H_gt) + return H, mask + + +# ----------------------------------------------------------------------------- +# AffNet + HardNet feature handling +# ----------------------------------------------------------------------------- + +def compute_affnet_hardnet_features(img1, img2, desired_kpts): + """ + Compute AffNet + HardNet features for the two input images. + Returns local affine frames (LAFs) and descriptors for both images. + """ + print("Computing AffNet + HardNet features (this may take a while) ...") + import kornia as K + import kornia.feature as KF + import torch + from kornia_moons.feature import OpenCVDetectorWithAffNetKornia + + # Convert images to torch tensors (already RGB, so no channel swap) + img1_torch = K.image_to_tensor(img1, False).float() / 255.0 + img2_torch = K.image_to_tensor(img2, False).float() / 255.0 + + device = "cuda" if torch.cuda.is_available() else "cpu" + img1_torch = img1_torch.to(device) + img2_torch = img2_torch.to(device) + + detector = OpenCVDetectorWithAffNetKornia(cv2.SIFT_create(desired_kpts), max_kpts=desired_kpts) + descriptor = KF.LAFDescriptor(KF.HardNet(True)).eval() + feature = KF.LocalFeature(detector, descriptor) + + with torch.no_grad(): + lafs1, _, descs1 = feature(img1_torch) + lafs2, _, descs2 = feature(img2_torch) + + lafs1np = np.squeeze(lafs1.cpu().detach().numpy()) + descs1np = np.squeeze(descs1.cpu().detach().numpy()) + lafs2np = np.squeeze(lafs2.cpu().detach().numpy()) + descs2np = np.squeeze(descs2.cpu().detach().numpy()) + + return lafs1np, descs1np, lafs2np, descs2np + + +def get_laf_centroids(lafs1, lafs2): + """ + Extract point coordinates (centroids) from LAFs. + Used only for visualization of matches. + Returns two lists of (x, y) tuples. + """ + kps1 = [(lafs1[i, 0, 2], lafs1[i, 1, 2]) for i in range(lafs1.shape[0])] + kps2 = [(lafs2[i, 0, 2], lafs2[i, 1, 2]) for i in range(lafs2.shape[0])] + return kps1, kps2 + + +def get_affine_correspondences(lafs1, lafs2, tentatives): + """ + Convert pairs of LAFs to affine correspondences (ACs). + Returns: + xs, ys : np.float32 arrays of shape (N, 2) containing centroids. + A : np.float32 array of shape (N, 4) containing the affine transformation + (flattened 2x2 matrix) between each pair of LAFs. + """ + xs = np.zeros((len(tentatives), 2), dtype=np.float32) + ys = np.zeros((len(tentatives), 2), dtype=np.float32) + ACs = np.zeros((len(tentatives), 4), dtype=np.float32) + for row, m in enumerate(tentatives): + LAF1 = lafs1[m.queryIdx] + LAF2 = lafs2[m.trainIdx] + # Local affine transformation: A = LAF2 * inv(LAF1) + A = np.matmul(LAF2[:, :2], np.linalg.inv(LAF1[:, :2])) + xs[row, 0] = LAF1[0, 2] + xs[row, 1] = LAF1[1, 2] + ys[row, 0] = LAF2[0, 2] + ys[row, 1] = LAF2[1, 2] + ACs[row, 0] = A[0, 0] + ACs[row, 1] = A[0, 1] + ACs[row, 2] = A[1, 0] + ACs[row, 3] = A[1, 1] + return xs, ys, ACs + +# --- 4. AffNet + HardNet features & affine correspondences --- +lafs1, descs1, lafs2, descs2 = compute_affnet_hardnet_features( + img1, img2, DESIRED_KEYPOINTS +) + +# Match descriptors with ratio test +bf = cv2.BFMatcher() +knn_matches = bf.knnMatch(descs1, descs2, k=2) +tentatives_aff = [] +for m, n in knn_matches: + if m.distance < SNN_THRESHOLD * n.distance: + tentatives_aff.append(m) + +# Convert LAFs to affine correspondences +xs, ys, A = get_affine_correspondences(lafs1, lafs2, tentatives_aff) + +t = time() +homlib_aff_H, homlib_aff_mask = verify_homlib_affine( + xs, ys, A, tentatives_aff, H_gt, + img1.shape[1], img1.shape[0], img2.shape[1], img2.shape[0] +) +print(f"{time() - t:.3f} sec (homlib affine)\n") + +# For visualization, create keypoints from LAF centroids +kps1_aff, kps2_aff = get_laf_centroids(lafs1, lafs2) +# Convert to cv2.KeyPoint objects for draw_matches +kps1_aff_cv = tuple(cv2.KeyPoint(x, y, 1) for x, y in kps1_aff) +kps2_aff_cv = tuple(cv2.KeyPoint(x, y, 1) for x, y in kps2_aff) + +draw_matches(kps1_aff_cv, kps2_aff_cv, tentatives_aff, + img1, img2, homlib_aff_H, H_gt, homlib_aff_mask, + title='homlib affine') diff --git a/python/example/requirements.txt b/python/example/requirements.txt index 6ccafc3..ee43742 100644 --- a/python/example/requirements.txt +++ b/python/example/requirements.txt @@ -1 +1,3 @@ matplotlib +kornia +kornia_moons \ No newline at end of file diff --git a/python/src/homlib/__init__.py b/python/src/homlib/__init__.py index a21b2a0..b5f5b51 100644 --- a/python/src/homlib/__init__.py +++ b/python/src/homlib/__init__.py @@ -22,8 +22,15 @@ lomsac_wadenback_3dv_2026_one_sided, lomsac_wadenback_3dv_2026_two_sided_equal, lomsac_wadenback_3dv_2026_two_sided, + lomsac_valtonenornhag_icpr_2026_one_sided_affine, + lomsac_valtonenornhag_icpr_2026_one_sided_ori, + lomsac_barath_visapp_2016_affine, LORansacOptions, RansacStatistics, + DistortionCase, + ProblemConfig, + ProblemInstance, + generate_problem_instance, ) __all__ = [ @@ -48,6 +55,13 @@ "lomsac_wadenback_3dv_2026_one_sided", "lomsac_wadenback_3dv_2026_two_sided_equal", "lomsac_wadenback_3dv_2026_two_sided", + "lomsac_valtonenornhag_icpr_2026_one_sided_affine", + "lomsac_valtonenornhag_icpr_2026_one_sided_ori", + "lomsac_barath_visapp_2016_affine", "LORansacOptions", "RansacStatistics", + "DistortionCase", + "ProblemConfig", + "ProblemInstance", + "generate_problem_instance", ] diff --git a/python/src/main.cpp b/python/src/main.cpp index 2d378ec..9870dee 100644 --- a/python/src/main.cpp +++ b/python/src/main.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -11,16 +12,30 @@ #include #include +#include #include #include #include #include +#include #include - +#include +#include +#include +#include namespace py = pybind11; using namespace pybind11::literals; +template +std::string to_string_with_precision(const T a_value, const int n = 6) +{ + std::ostringstream out; + out.precision(n); + out << std::fixed << a_value; + return std::move(out).str(); +} + void preprocess( const Eigen::Matrix &x_, const Eigen::Matrix &y_, @@ -36,6 +51,51 @@ void preprocess( } } +void preprocess_affine( + const Eigen::Matrix &x_, + const Eigen::Matrix &y_, + const Eigen::Matrix &A_, + std::vector *x, + std::vector *y, + std::vector *A +) { + if (x_.cols() != y_.cols()) { + throw std::invalid_argument("x and y should be of equal size."); + } + if (x_.cols() != A_.cols()) { + throw std::invalid_argument("x and y should be of equal size."); + } + for (size_t i=0; i < x_.cols(); i++) { + x->push_back(x_.col(i)); + y->push_back(y_.col(i)); + Eigen::Matrix2d tmp; + tmp << A_.col(i); + tmp.transposeInPlace(); + A->push_back(tmp); + } +} + +void preprocess_orientation( + const Eigen::Matrix &x_, + const Eigen::Matrix &y_, + const Eigen::Matrix &ori_, + std::vector *x, + std::vector *y, + std::vector *ori +) { + if (x_.cols() != y_.cols()) { + throw std::invalid_argument("x and y should be of equal size."); + } + if (x_.cols() != ori_.cols()) { + throw std::invalid_argument("x and y should be of equal size."); + } + for (size_t i=0; i < x_.cols(); i++) { + x->push_back(x_.col(i)); + y->push_back(y_.col(i)); + ori->push_back(ori_.col(i)); + } +} + std::vector estimate_fitzgibbon_cvpr_2001_one_sided_wrapper( const Eigen::Matrix &x_, @@ -135,6 +195,10 @@ template std::tuple solver(x, y, *estimator); ransac_lib::LocallyOptimizedMSAC< @@ -248,6 +312,99 @@ std::tuple lomsac_wadenback_3dv_ return output; } +template std::tuple lomsac_affine_wrapper( + Estimator* estimator, + const Eigen::Matrix &x_, + const Eigen::Matrix &y_, + const Eigen::Matrix &A_, + const ransac_lib::LORansacOptions &options +) { + std::vector x, y; + std::vector A; + preprocess_affine(x_, y_, A_, &x, &y, &A); + + ransac_lib::RansacStatistics ransac_stats; + int inliers = 0; + HomLib::PoseData best_model; + best_model.homography = Eigen::Matrix3d::Identity(); + best_model.focal_length = 0.0; + best_model.distortion_parameter = 0.0; + best_model.distortion_parameter2 = 0.0; + + HomLib::AffineRansacEstimator solver(x, y, A, *estimator); + ransac_lib::LocallyOptimizedMSAC< + HomLib::PoseData, + std::vector, + HomLib::AffineRansacEstimator> lomsac; + inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats); + + return std::make_tuple(std::move(best_model), std::move(ransac_stats)); +} + + +template std::tuple lomsac_orientation_wrapper( + Estimator* estimator, + const Eigen::Matrix &x_, + const Eigen::Matrix &y_, + const Eigen::Matrix &ori_, + const ransac_lib::LORansacOptions &options +) { + std::vector x, y; + std::vector ori; + preprocess_orientation(x_, y_, ori_, &x, &y, &ori); + + ransac_lib::RansacStatistics ransac_stats; + int inliers = 0; + HomLib::PoseData best_model; + best_model.homography = Eigen::Matrix3d::Identity(); + best_model.focal_length = 0.0; + best_model.distortion_parameter = 0.0; + best_model.distortion_parameter2 = 0.0; + + HomLib::OrientationRansacEstimator solver(x, y, ori, *estimator); + ransac_lib::LocallyOptimizedMSAC< + HomLib::PoseData, + std::vector, + HomLib::OrientationRansacEstimator> lomsac; + inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats); + + return std::make_tuple(std::move(best_model), std::move(ransac_stats)); +} + +std::tuple lomsac_valtonenornhag_icpr_2026_one_sided_affine_wrapper( + const Eigen::Matrix &x_, + const Eigen::Matrix &y_, + const Eigen::Matrix &A_, + const ransac_lib::LORansacOptions &options +) { + HomLib::ValtonenOrnhagICPR2026::AffineSolverSingleSided estimator; + auto output = lomsac_affine_wrapper(&estimator, x_, y_, A_, options); + return output; +} + +std::tuple lomsac_valtonenornhag_icpr_2026_one_sided_ori_wrapper( + const Eigen::Matrix &x_, + const Eigen::Matrix &y_, + const Eigen::Matrix &ori_, + const ransac_lib::LORansacOptions &options +) { + HomLib::ValtonenOrnhagICPR2026::OrientationSolverSingleSided estimator; + auto output = lomsac_orientation_wrapper(&estimator, x_, y_, ori_, options); + return output; +} + +std::tuple lomsac_barath_visapp_2016_affine_wrapper( + const Eigen::Matrix &x_, + const Eigen::Matrix &y_, + const Eigen::Matrix &A_, + const ransac_lib::LORansacOptions &options +) { + HomLib::BarathVISAPP2016::AffineSolver estimator; + auto output = lomsac_affine_wrapper(&estimator, x_, y_, A_, options); + return output; +} + + PYBIND11_MODULE(_core, m) { m.doc() = R"pbdoc( @@ -283,6 +440,14 @@ PYBIND11_MODULE(_core, m) { lomsac_wadenback_3dv_2026_two_sided_equal lomsac_wadenback_3dv_2026_two_sided + lomsac_valtonenornhag_icpr_2026_one_sided_affine + lomsac_valtonenornhag_icpr_2026_one_sided_ori + lomsac_barath_visapp_2016_affine + + DistortionCase + ProblemConfig + ProblemInstance + generate_problem_instance )pbdoc"; @@ -310,13 +475,106 @@ PYBIND11_MODULE(_core, m) { return "PoseData(" "H=[3x3 np.array], " "focal_length=" + std::to_string(p.focal_length) + ", " - "distortion_parameter=" + std::to_string(p.distortion_parameter) + ", " - "distortion_parameter2=" + std::to_string(p.distortion_parameter2) + + "distortion_parameter=" + to_string_with_precision(p.distortion_parameter, 16) + ", " + "distortion_parameter2=" + to_string_with_precision(p.distortion_parameter2, 16) + ")"; } + ); + + py::enum_(m, "DistortionCase") + .value("NO_DISTORTION", HomLib::DistortionCase::NO_DISTORTION) + .value("ONE_SIDED_LEFT", HomLib::DistortionCase::ONE_SIDED_LEFT) + .value("ONE_SIDED_RIGHT", HomLib::DistortionCase::ONE_SIDED_RIGHT) + .value("TWO_SIDED_EQUAL", HomLib::DistortionCase::TWO_SIDED_EQUAL) + .value("TWO_SIDED", HomLib::DistortionCase::TWO_SIDED) + .export_values(); + + py::class_(m, "ProblemConfig") + .def( + py::init(), + "Constructor for ProblemConfig.", + "distortion"_a, + "point_noise"_a, + "number_points"_a ) - .doc() = "Primary return class for HomLib functions."; + .def_readwrite("distortion", &HomLib::ProblemConfig::distortion) + .def_readwrite("point_noise", &HomLib::ProblemConfig::point_noise) + .def_readwrite("number_points", &HomLib::ProblemConfig::number_points) + .def_readwrite("camera_fov", &HomLib::ProblemConfig::camera_fov_) + .def_readwrite("min_depth", &HomLib::ProblemConfig::min_depth_) + .def_readwrite("max_depth", &HomLib::ProblemConfig::max_depth_) + .def_readwrite("min_focal", &HomLib::ProblemConfig::min_focal_) + .def_readwrite("max_focal", &HomLib::ProblemConfig::max_focal_) + .def_readwrite("min_dist", &HomLib::ProblemConfig::min_dist_) + .def_readwrite("max_dist", &HomLib::ProblemConfig::max_dist_) + .def("__repr__", + [](const HomLib::ProblemConfig &p) { + std::string distortion; + switch (p.distortion) { + case HomLib::DistortionCase::NO_DISTORTION: + distortion = "NO_DISTORTION"; + break; + case HomLib::DistortionCase::ONE_SIDED_LEFT: + distortion = "ONE_SIDED_LEFT"; + break; + case HomLib::DistortionCase::ONE_SIDED_RIGHT: + distortion = "ONE_SIDED_RIGHT"; + break; + case HomLib::DistortionCase::TWO_SIDED_EQUAL: + distortion = "TWO_SIDED_EQUAL"; + break; + case HomLib::DistortionCase::TWO_SIDED: + distortion = "TWO_SIDED"; + break; + } + return "ProblemConfig(" + "distortion=" + distortion + ", " + "point_noise=" + to_string_with_precision(p.point_noise) + ", " + "number_points=" +std::to_string(p.number_points) + ", " + "camera_fov=" + to_string_with_precision(p.camera_fov_, 2) + ", " + "min_depth=" + to_string_with_precision(p.min_depth_, 2) + ", " + "max_depth=" + to_string_with_precision(p.max_depth_, 2) + ", " + "min_focal=" + to_string_with_precision(p.min_focal_, 2) + ", " + "max_focal=" + to_string_with_precision(p.max_focal_, 2) + ", " + "min_dist=" + to_string_with_precision(p.min_dist_, 2) + ", " + "max_dist=" + to_string_with_precision(p.max_dist_, 2) + + ")"; + } + ); + py::class_(m, "ProblemInstance") + .def( + py::init, std::vector, std::vector>(), + "Constructor for ProblemInstance.", + "posedata"_a, + "x1"_a, + "x2"_a, + "A"_a + ) + .def_readwrite("posedata", &HomLib::ProblemInstance::posedata) + .def_readwrite("x1", &HomLib::ProblemInstance::x1) + .def_readwrite("x2", &HomLib::ProblemInstance::x2) + .def_readwrite("A", &HomLib::ProblemInstance::A) + .def("hom_error", &HomLib::ProblemInstance::hom_error) + .def("dist_error", &HomLib::ProblemInstance::dist_error) + .def("__repr__", + [](const HomLib::ProblemInstance &p) { + return "ProblemInstance(" + "posedata=PoseData(), " + "x1=[list of length " + std::to_string(p.x1.size()) + "], " + "x2=[list of length " + std::to_string(p.x2.size()) + "], " + "A=[list of length " + std::to_string(p.A.size()) + "]" + ")"; + } + ); + m.def( + "generate_problem_instance", + &HomLib::generate_problem_instance, + R"pbdoc( + Generate a synthetic problem instance. + )pbdoc", + "settings"_a + ); py::class_(m, "LORansacOptions") .def( py::init<>() @@ -371,7 +629,7 @@ PYBIND11_MODULE(_core, m) { "best_model_score=" + std::to_string(s.best_model_score) + ", " "inlier_ratio=" + std::to_string(s.inlier_ratio) + ", " "inlier_indices=[int list of length " + std::to_string(s.inlier_indices.size()) + "], " - "number_lo_iterations=" + std::to_string(s.number_lo_iterations) + + "number_lo_iterations=" + std::to_string(s.number_lo_iterations) + ", " ")"; } ) @@ -687,4 +945,58 @@ PYBIND11_MODULE(_core, m) { "y"_a, "options"_a ); + m.def( + "lomsac_valtonenornhag_icpr_2026_one_sided_affine", + &lomsac_valtonenornhag_icpr_2026_one_sided_affine_wrapper, + R"pbdoc( + Solver from [1]_ in a LOMSAC framework [2]_ modified for affine-covariant features according to [3]_. + + .. [1] Gaku Nakano. "Inverse DLT Method for One-Sided Radial Distortion Homography", In + International Conference on Pattern Recognition (ICPR), 2024. + .. [2] Karel Lebeda, Jiri Matas, and Ondrej Chum. "Fixing the Locally Optimized RANSAC", In the + Proceedings of the British Machine Vision Conference (BMVC), 2012. + .. [3] Marcus Valtonen Ornhag and Stefan Adalbjornsson. "Radial Distortion Homography Estimation From + Affine-Covariant Or Orientation-Covariant Features", In the Proceedings of the International + Conference on Pattern Recognition (ICPR), 2026. + )pbdoc", + "x"_a, + "y"_a, + "A"_a, + "options"_a + ); + m.def( + "lomsac_valtonenornhag_icpr_2026_one_sided_ori", + &lomsac_valtonenornhag_icpr_2026_one_sided_ori_wrapper, + R"pbdoc( + Solver from [1]_ in a LOMSAC framework [2]_ modified for orientation-covariant features according to [3]_. + + .. [1] Gaku Nakano. "Inverse DLT Method for One-Sided Radial Distortion Homography", In + International Conference on Pattern Recognition (ICPR), 2024. + .. [2] Karel Lebeda, Jiri Matas, and Ondrej Chum. "Fixing the Locally Optimized RANSAC", In the + Proceedings of the British Machine Vision Conference (BMVC), 2012. + .. [3] Marcus Valtonen Ornhag and Stefan Adalbjornsson. "Radial Distortion Homography Estimation From + Affine-Covariant Or Orientation-Covariant Features", In the Proceedings of the International + Conference on Pattern Recognition (ICPR), 2026. + )pbdoc", + "x"_a, + "y"_a, + "ori"_a, + "options"_a + ); + m.def( + "lomsac_barath_visapp_2016_affine", + &lomsac_barath_visapp_2016_affine_wrapper, + R"pbdoc( + Solver from [1]_ in a LOMSAC framework [2]_. + + .. [1] Daniel Barath and Levente Hajder. "Novel Ways to Estimate Homography from Local Affine + Transformations", In International Conference on Computer Vision Theory and Application (VISAPP), 2016. + .. [2] Karel Lebeda, Jiri Matas, and Ondrej Chum. "Fixing the Locally Optimized RANSAC", In the + Proceedings of the British Machine Vision Conference (BMVC), 2012. + )pbdoc", + "x"_a, + "y"_a, + "A"_a, + "options"_a + ); }; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 01eb2c0..a3387b1 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -23,6 +23,7 @@ add_executable(HomLibTests test_kukelova_cvpr_2015.cpp test_nakano_icpr_2025.cpp test_valtonenornhag_icpr_2020.cpp + test_valtonenornhag_icpr_2026.cpp test_valtonenornhag_wacv_2021.cpp test_wadenback_3dv_2026.cpp ) diff --git a/tests/test_valtonenornhag_icpr_2026.cpp b/tests/test_valtonenornhag_icpr_2026.cpp new file mode 100644 index 0000000..9285598 --- /dev/null +++ b/tests/test_valtonenornhag_icpr_2026.cpp @@ -0,0 +1,115 @@ +// Copyright (c) 2020 Marcus Valtonen Örnhag +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include +#include +#include +#include +#include "get_valtonenornhag_icpr_2026.hpp" +#include "posedata.hpp" + +TEST_CASE("Valtonen Ornhag ICPR 2026 - AFFINE") { + + std::vector p1 = { + Eigen::Vector2d(-0.304372059145248, -0.174125682172663), + Eigen::Vector2d(-0.308082306270758, 0.100342577802908) + }; + + std::vector p2 = { + Eigen::Vector2d(0.596062339231747, 0.449851750693698), + Eigen::Vector2d(0.284590553127076, 0.20125992440591) + }; + + std::vector A = { + (Eigen::Matrix2d() << -0.742737063351745, -1.25720051435581, + 1.29931162730836, -0.953395533218031).finished(), + (Eigen::Matrix2d() << -0.763314941736403, -1.05311577674003, + 1.13911319830585, -0.839438880618645).finished() + }; + + std::vector posedata = HomLib::ValtonenOrnhagICPR2026::get_affine(p1, p2, A, false); + + double tol = 1e-12; + + // Test size + REQUIRE(posedata.size() == 1); + + // Test distortion parameters + REQUIRE(posedata[0].distortion_parameter == Catch::Approx(-0.15508855476035885).margin(tol)); + REQUIRE(posedata[0].distortion_parameter2 == Catch::Approx(0.0).margin(tol)); + + // Test homographies + tol = 1e-7; + Eigen::Matrix3d expected; + + expected << -0.389294613852058, -0.452105197122382, 0.0574163046746314, + 0.475012108793918, -0.355897552980124, 0.279279658894568, + -0.152511379317942, 0.121821752305461, 0.407920793089342; + + + REQUIRE(posedata[0].homography.isApprox(expected, tol)); + +} + + +TEST_CASE("Valtonen Ornhag ICPR 2026 - ORI") { + std::vector p1 = { + Eigen::Vector2d(0.611733207361777, -0.100815813483642), + Eigen::Vector2d(0.506500688761625, 0.22654451870654), + Eigen::Vector2d(0.618492431042353, 0.490634389234033), + Eigen::Vector2d(0.446207436531052, -0.221543454776215) + }; + + std::vector p2 = { + Eigen::Vector2d(0.152378076324475, 0.471173894309057), + Eigen::Vector2d(0.35249479911981, 0.604057206211103), + Eigen::Vector2d(0.576429540272504, 0.546190419126011), + Eigen::Vector2d(-0.0960825885072252, 0.57183541266364) + }; + + std::vector ori = { + Eigen::Vector2d(2.82616873462069, 2.11084467936878), + Eigen::Vector2d(2.76670870212448, 1.5791745485192), + Eigen::Vector2d(3.03912230291483, 1.40242855410085), + Eigen::Vector2d(2.54398570702913, 2.11604504644583) + }; + + std::vector posedata = HomLib::ValtonenOrnhagICPR2026::get_ori(p1, p2, ori); + + double tol = 1e-12; + + // Test size + REQUIRE(posedata.size() == 1); + + // Test distortion parameters + REQUIRE(posedata[0].distortion_parameter == Catch::Approx(-0.18221254202290402).margin(tol)); + REQUIRE(posedata[0].distortion_parameter2 == Catch::Approx(0.0).margin(tol)); + + // Test homographies + tol = 1e-7; + Eigen::Matrix3d expected; + + expected << 0.400486830270973, 0.357931239582734, -0.141724104171464, + 0.114670003794941, 0.0864762777928815, 0.190330050981093, + 0.795029931876192, 0.0101246929389779, 0.0222653484078536; + + REQUIRE(posedata[0].homography.isApprox(expected, tol)); + +}