Administrator/opencv-MIRROR
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2026-09-12 13:23:03 -05:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #29609 from Teddy-Yangjiale/rvv-fisheye-undistortpoints

Browse Source 
geometry: Vectorize fisheye::undistortPoints
This commit is contained in:
Abhishek Gola
2026-08-04 23:31:25 +05:30
committed by GitHub
parent 2c00686ec7 72a8f94e44
commit 6998403472
2 changed files with 282 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

139
modules/geometry/src/fisheye.cpp
View File

@@ -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<v_float64>::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<double> 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<int>::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 =>

165
modules/geometry/test/test_fisheye.cpp
View File

@@ -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<cv::Vec2f>()[i]
: distorted.ptr<cv::Vec2d>()[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<cv::Vec2f>()[i] = cv::Vec2f(fi);
else
undistorted.ptr<cv::Vec2d>()[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<double>(i, j), y = b64.at<double>(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<double>::infinity();
continue;
}
if (cvIsInf(x) || cvIsInf(y))
{
if (x != y)
return std::numeric_limits<double>::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<cv::Vec2d>();
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<cv::Vec2d>();
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<cv::Vec2d>()[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<cv::Vec2d>()[i][0] == -1000000.0;
const bool actualSentinel = actual.ptr<cv::Vec2d>()[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;