diff --git a/modules/geometry/src/fisheye.cpp b/modules/geometry/src/fisheye.cpp index 34e89753a5..e52f20de70 100644 --- a/modules/geometry/src/fisheye.cpp +++ b/modules/geometry/src/fisheye.cpp @@ -308,6 +308,36 @@ void cv::fisheye::distortPoints(InputArray _undistorted, OutputArray distorted, cv::fisheye::distortPoints(normalized, distorted, K, D, alpha); } +// Compensate distortion iteratively using Newton's method. The vectorized loop in +// undistortPoints() mirrors this solver. +// theta_d = theta * (1 + k1 * theta^2 + k2 * theta^4 + k3 * theta^6 + k4 * theta^8) => +// f(theta) := theta * (1 + k1 * theta^2 + k2 * theta^4 + k3 * theta^6 + k4 * theta^8) - theta_d = 0 +// Newton's method: new_theta = theta - theta_fix, theta_fix := f(theta) / f'(theta) +// f'(theta) = 1 + 3 * k1 * theta^2 + 5 * k2 * theta^4 + 7 * k3 * theta^6 + 9 * k4 * theta^8 +static inline double solveThetaFromThetaD(double theta_d, const Vec4d& k, int maxCount, + bool isEps, double epsilon, bool& converged) +{ + double theta = theta_d; + converged = false; + + for (int j = 0; j < maxCount; j++) + { + double theta2 = theta*theta, theta4 = theta2*theta2, theta6 = theta4*theta2, theta8 = theta6*theta2; + double k0_theta2 = k[0] * theta2, k1_theta4 = k[1] * theta4, k2_theta6 = k[2] * theta6, k3_theta8 = k[3] * theta8; + double theta_fix = (theta * (1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - theta_d) / + (1 + 3*k0_theta2 + 5*k1_theta4 + 7*k2_theta6 + 9*k3_theta8); + theta = theta - theta_fix; + + if (isEps && (fabs(theta_fix) < epsilon)) + { + converged = true; + break; + } + } + + return theta; +} + void cv::fisheye::undistortPoints( InputArray distorted, OutputArray undistorted, InputArray K, InputArray D, InputArray R, InputArray P, TermCriteria criteria) { @@ -373,8 +403,93 @@ void cv::fisheye::undistortPoints( InputArray distorted, OutputArray undistorted maxCount = criteria.maxCount; } + size_t i = 0; +#if (CV_SIMD_SCALABLE_64F || CV_SIMD_64F) + // Vectorized fast path: fixed iteration count (no EPS early-exit), no reprojection (R, P empty) + // and double I/O. The per-point Newton iteration is independent, so a whole vector of points is + // solved at once. Every other case, and the trailing points, go to the scalar loop below. + if( !isEps && R.empty() && P.empty() && sdepth == CV_64F ) + { + const int vl = VTraits::vlanes(); + if( n >= (size_t)vl ) + { + const size_t totalBlocks = n / vl; + auto processBlocks = [&](size_t blockBegin, size_t blockEnd) + { + const v_float64 v_fx = vx_setall_f64(f[0]), v_fy = vx_setall_f64(f[1]); + const v_float64 v_cx = vx_setall_f64(c[0]), v_cy = vx_setall_f64(c[1]); + const v_float64 v_k0 = vx_setall_f64(k[0]), v_k1 = vx_setall_f64(k[1]); + const v_float64 v_k2 = vx_setall_f64(k[2]), v_k3 = vx_setall_f64(k[3]); + const v_float64 v_one = vx_setall_f64(1.0), v_zero = vx_setzero_f64(); + const v_float64 v_three = vx_setall_f64(3.0), v_five = vx_setall_f64(5.0); + const v_float64 v_seven = vx_setall_f64(7.0), v_nine = vx_setall_f64(9.0); + const v_float64 v_lo = vx_setall_f64(-CV_PI/2.), v_hi = vx_setall_f64(CV_PI/2.); + const v_float64 v_sent = vx_setall_f64(-1000000.0); + AutoBuffer tbuf(vl); - for(size_t i = 0; i < n; i++ ) + for( size_t block = blockBegin; block < blockEnd; block++ ) + { + const size_t j = block * vl; + v_float64 px, py; + v_load_deinterleave((const double*)(srcd + j), px, py); + v_float64 pwx = v_div(v_sub(px, v_cx), v_fx); + v_float64 pwy = v_div(v_sub(py, v_cy), v_fy); + v_float64 theta_d = v_sqrt(v_add(v_mul(pwx, pwx), v_mul(pwy, pwy))); + theta_d = v_min(v_max(theta_d, v_lo), v_hi); + v_float64 theta = theta_d; + + for( int it = 0; it < maxCount; it++ ) + { + v_float64 t2 = v_mul(theta, theta), t4 = v_mul(t2, t2); + v_float64 t6 = v_mul(t4, t2), t8 = v_mul(t6, t2); + v_float64 a0 = v_mul(t2, v_k0), a1 = v_mul(t4, v_k1); + v_float64 a2 = v_mul(t6, v_k2), a3 = v_mul(t8, v_k3); + v_float64 poly = v_add(v_one, v_add(v_add(a0, a1), v_add(a2, a3))); + v_float64 num = v_sub(v_mul(theta, poly), theta_d); + v_float64 den = v_add(v_one, v_add( + v_add(v_mul(a0, v_three), v_mul(a1, v_five)), + v_add(v_mul(a2, v_seven), v_mul(a3, v_nine)))); + theta = v_sub(theta, v_div(num, den)); + } + + // scale = tan(theta) / theta_d (scalar pass; there is no vector transcendental) + v_store(tbuf.data(), theta); + for( int lane = 0; lane < vl; lane++ ) tbuf[lane] = std::tan(tbuf[lane]); + v_float64 scale = v_div(vx_load(tbuf.data()), theta_d); + v_float64 pux = v_mul(pwx, scale), puy = v_mul(pwy, scale); + + // theta_d is non-negative, so a sign flip is exactly theta < 0. + v_float64 flipped = v_lt(theta, v_zero); + pux = v_select(flipped, v_sent, pux); + puy = v_select(flipped, v_sent, puy); + v_store_interleave((double*)(dstd + j), pux, puy); + } + }; + + if( totalBlocks <= (size_t)std::numeric_limits::max() ) + { + // Keep small inputs single-threaded while splitting sufficiently expensive Newton + // workloads into multiple stripes. + const double nstripes = (double)n * maxCount / (1 << 16); + if( nstripes < 1.5 ) + processBlocks(0, totalBlocks); + else + { + parallel_for_(Range(0, (int)totalBlocks), [&](const Range& range) + { + processBlocks((size_t)range.start, (size_t)range.end); + }, nstripes); + } + } + else + processBlocks(0, totalBlocks); + + i = totalBlocks * vl; + } + } +#endif + + for( ; i < n; i++ ) { Vec2d pi = sdepth == CV_32F ? (Vec2d)srcf[i] : srcd[i]; // image point // u = fx * x' + cx (alpha = 0), v = fy * y' + cy => @@ -401,27 +516,7 @@ void cv::fisheye::undistortPoints( InputArray distorted, OutputArray undistorted if (!isEps || fabs(theta_d) > criteria.epsilon) { // compensate distortion iteratively using Newton method - - for (int j = 0; j < maxCount; j++) - { - // theta_d = theta * (1 + k1 * theta^2 + k2 * theta^4 + k3 * theta^6 + k4 * theta^8) => - // f(theta) := theta * (1 + k1 * theta^2 + k2 * theta^4 + k3 * theta^6 + k4 * theta^8) - theta_d = 0 - // Newton's method: new_theta = theta - theta_fix, theta_fix := f(theta) / f'(theta) - // f'(theta) = (theta * (1 + k1 * theta^2 + k2 * theta^4 + k3 * theta^6 + k4 * theta^8) - theta_d)' = - // (theta + k1 * theta^3 + k2 * theta^5 + k3 * theta^7 + k4 * theta^9 - theta_d)' = - // 1 + 3 * k1 * theta^2 + 5 * k2 * theta^4 + 7 * k3 * theta^6 + 9 * k4 * theta^8 - double theta2 = theta*theta, theta4 = theta2*theta2, theta6 = theta4*theta2, theta8 = theta6*theta2; - double k0_theta2 = k[0] * theta2, k1_theta4 = k[1] * theta4, k2_theta6 = k[2] * theta6, k3_theta8 = k[3] * theta8; - double theta_fix = (theta * (1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - theta_d) / - (1 + 3*k0_theta2 + 5*k1_theta4 + 7*k2_theta6 + 9*k3_theta8); - theta = theta - theta_fix; - - if (isEps && (fabs(theta_fix) < criteria.epsilon)) - { - converged = true; - break; - } - } + theta = solveThetaFromThetaD(theta_d, k, maxCount, isEps, criteria.epsilon, converged); // x' = (theta_d / r) * a, y' = (theta_d / r) * b => // a = x' * r / theta_d, b = y' * r / theta_d => diff --git a/modules/geometry/test/test_fisheye.cpp b/modules/geometry/test/test_fisheye.cpp index f77e14270f..a56733a7a8 100644 --- a/modules/geometry/test/test_fisheye.cpp +++ b/modules/geometry/test/test_fisheye.cpp @@ -217,6 +217,171 @@ TEST_F(fisheyeTest, distortUndistortPointsNewCameraRandom) } } +// Reference implementation of the fisheye undistortion solver, kept scalar and independent of the +// library so that the vectorized path can be checked against it. Mirrors the scalar loop in +// modules/geometry/src/fisheye.cpp for the case without rectification/new camera matrix. +static void undistortPointsScalarReference(const cv::Mat& distorted, cv::Mat& undistorted, + const cv::Matx33d& K, const cv::Vec4d& D, int maxCount) +{ + undistorted.create(distorted.size(), distorted.type()); + + const double fx = K(0, 0), fy = K(1, 1), cx = K(0, 2), cy = K(1, 2); + const bool isFloat = distorted.depth() == CV_32F; + const size_t n = distorted.total(); + + for (size_t i = 0; i < n; i++) + { + cv::Vec2d pi = isFloat ? (cv::Vec2d)distorted.ptr()[i] + : distorted.ptr()[i]; + cv::Vec2d pw((pi[0] - cx)/fx, (pi[1] - cy)/fy); + + double theta_d = std::sqrt(pw[0]*pw[0] + pw[1]*pw[1]); + theta_d = std::min(std::max(-CV_PI/2., theta_d), CV_PI/2.); + + double theta = theta_d; + for (int j = 0; j < maxCount; j++) + { + double theta2 = theta*theta, theta4 = theta2*theta2; + double theta6 = theta4*theta2, theta8 = theta6*theta2; + double k0_theta2 = D[0]*theta2, k1_theta4 = D[1]*theta4; + double k2_theta6 = D[2]*theta6, k3_theta8 = D[3]*theta8; + double theta_fix = (theta*(1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - theta_d) / + (1 + 3*k0_theta2 + 5*k1_theta4 + 7*k2_theta6 + 9*k3_theta8); + theta = theta - theta_fix; + } + double scale = std::tan(theta) / theta_d; + + bool theta_flipped = ((theta_d < 0 && theta > 0) || (theta_d > 0 && theta < 0)); + cv::Vec2d fi = theta_flipped ? cv::Vec2d(-1000000.0, -1000000.0) : pw*scale; + + if (isFloat) + undistorted.ptr()[i] = cv::Vec2f(fi); + else + undistorted.ptr()[i] = fi; + } +} + +// Largest elementwise relative difference, |a - b| / max(|b|, 1). Points far outside the valid +// field of view are ill-conditioned: theta approaches pi/2, where tan() amplifies the last bits of +// theta, so the two implementations are compared in relative rather than absolute terms. +static double maxRelativeDiff(const cv::Mat& a, const cv::Mat& b) +{ + CV_Assert(a.size() == b.size() && a.type() == b.type()); + cv::Mat a64, b64; + a.reshape(1).convertTo(a64, CV_64F); + b.reshape(1).convertTo(b64, CV_64F); + + double maxRel = 0.0; + for (int i = 0; i < a64.rows; i++) + { + for (int j = 0; j < a64.cols; j++) + { + const double x = a64.at(i, j), y = b64.at(i, j); + // The principal point yields tan(0)/0, so NaN must appear in the same places in both. + if (cvIsNaN(x) || cvIsNaN(y)) + { + if (cvIsNaN(x) != cvIsNaN(y)) + return std::numeric_limits::infinity(); + continue; + } + if (cvIsInf(x) || cvIsInf(y)) + { + if (x != y) + return std::numeric_limits::infinity(); + continue; + } + maxRel = std::max(maxRel, std::abs(x - y) / std::max(std::abs(y), 1.0)); + } + } + return maxRel; +} + +// The vectorized path of undistortPoints must agree with the scalar solver. It is selected by +// MAX_ITER-only criteria with CV_64FC2 input and no rectification, so the test uses exactly that; +// CV_32FC2 stays on the scalar path and is checked as well. The point counts include values that +// are not multiples of the vector width, so the scalar tail is exercised on 2-, 4- and 8-lane +// builds; the largest case also covers the multi-stripe parallel path. +TEST_F(fisheyeTest, undistortPointsFastPathMatchesScalar) +{ + const int counts[] = { 1, 2, 3, 7, 33, 129, 257, 10001 }; + const int maxCount = 10; + const cv::TermCriteria criteria(cv::TermCriteria::MAX_ITER, maxCount, 0); + + cv::RNG rng(0x8f3a21c5); + + for (size_t ci = 0; ci < sizeof(counts)/sizeof(counts[0]); ci++) + { + const int n = counts[ci]; + + cv::Mat points64(n, 1, CV_64FC2); + cv::Vec2d* pts = points64.ptr(); + for (int i = 0; i < n; i++) + { + pts[i] = cv::Vec2d(rng.uniform(0.0, (double)imageSize.width), + rng.uniform(0.0, (double)imageSize.height)); + } + pts[0] = cv::Vec2d(K(0, 2), K(1, 2)); // principal point + + cv::Mat expected, actual; + + undistortPointsScalarReference(points64, expected, K, D, maxCount); + cv::fisheye::undistortPoints(points64, actual, K, D, cv::noArray(), cv::noArray(), criteria); + EXPECT_LT(maxRelativeDiff(actual, expected), 1e-12) << "CV_64FC2, n = " << n; + + cv::Mat points32; + points64.convertTo(points32, CV_32FC2); + undistortPointsScalarReference(points32, expected, K, D, maxCount); + cv::fisheye::undistortPoints(points32, actual, K, D, cv::noArray(), cv::noArray(), criteria); + EXPECT_LT(maxRelativeDiff(actual, expected), 1e-6) << "CV_32FC2, n = " << n; + } +} + +// Points whose theta flips sign during the iteration are replaced by a sentinel value. Check that +// the vectorized path marks exactly the same points as the scalar solver. +TEST_F(fisheyeTest, undistortPointsSentinelMatchesScalar) +{ + const int maxCount = 10; + const cv::TermCriteria criteria(cv::TermCriteria::MAX_ITER, maxCount, 0); + // Strong negative distortion makes the iteration diverge for large radii. + const cv::Vec4d strongD(-0.5, 0.05, -0.01, 0.002); + + const int n = 129; + cv::Mat points(n, 1, CV_64FC2); + cv::Vec2d* pts = points.ptr(); + for (int i = 0; i < n; i++) + { + // Sweep the radius from inside the image out to far beyond the valid field of view. + const double radius = 40.0*(i + 1); + const double angle = 0.37*i; + pts[i] = cv::Vec2d(K(0, 2) + radius*std::cos(angle), + K(1, 2) + radius*std::sin(angle)); + } + + cv::Mat expected, actual; + undistortPointsScalarReference(points, expected, K, strongD, maxCount); + cv::fisheye::undistortPoints(points, actual, K, strongD, cv::noArray(), cv::noArray(), criteria); + + int sentinels = 0; + for (int i = 0; i < n; i++) + if (expected.ptr()[i][0] == -1000000.0) + sentinels++; + + // The test is only meaningful if the sentinel branch is actually taken. + EXPECT_GT(sentinels, 0); + EXPECT_LT(sentinels, n); + + // Both paths must mark exactly the same points, and agree on the remaining ones. + for (int i = 0; i < n; i++) + { + const bool expectedSentinel = expected.ptr()[i][0] == -1000000.0; + const bool actualSentinel = actual.ptr()[i][0] == -1000000.0; + ASSERT_EQ(expectedSentinel, actualSentinel) << "point " << i; + } + // Strong distortion makes the non-sentinel points numerically ill-conditioned. The sentinel + // mask above is exact; use a relative check only to catch large deviations in the other points. + EXPECT_LT(maxRelativeDiff(actual, expected), 1e-8); +} + TEST_F(fisheyeTest, solvePnP) { const int n = 16;