Mirrors/opencv
0
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2026-01-18 17:21:42 +01:00
Code Packages Releases Activity

Merge pull request #27993 from akretz:undistortPoints_convergence

Browse Source 
Undistort points convergence #27993

I have looked into the `undistortPoints()` problem of issue #27916 and have found a solution. The problem is, as @Linhuihang has correctly pointed out, that the fixed-point iterations do not converge. Here are the functions which are optimized for the undistortion problem:

$$
\begin{aligned}
  r^2  &= x'^2 + y'^2 \\
  f_1(x') &= \frac{1 + k_4 r^2 + k_5 r^4 + k_6 r^6}{1 + k_1 r^2 + k_2 r^4 + k_3 r^6} (x'' - 2p_1 x' y' - p_2(r^2 + 2 x'^2) - s_1 r^2 + s_2 r^4) = x' \\
  f_2(y') &=  \frac{1 + k_4 r^2 + k_5 r^4 + k_6 r^6}{1 + k_1 r^2 + k_2 r^4 + k_3 r^6} (y'' - p_1 (r^2 + 2 y'^2) - 2 p_2 x' y' - s_3 r^2 - s_4 r^4) = y'
\end{aligned}
$$

where $x', y'$ are the undistorted points we want to compute and and $x'', y''$ are the given distorted points. This problem is solved using fixed-point iterations like

$$
  x'_{k+1} = f_1(x'_k),\quad
  y'_{k+1} = f_2(y'_k)
$$

I guess the issue here is that the distortion function does not necessarily satisfy the [Banach fixed-point theorem](https://en.wikipedia.org/wiki/Banach_fixed-point_theorem), i.e. the slope of the function can be too large. This can be seen in @Linhuihang's comment https://github.com/opencv/opencv/issues/27916#issuecomment-3417883642 - the point series jumps around and doesn't converge.

A common solution is to instead do damped fixed-point iterations, so that the updates are "more smooth".

$$
  x'_{k+1} = (1 - \alpha) x'_k + \alpha f_1(x'_k),\quad
  y'_{k+1} = (1 - \alpha) y'_k + \alpha f_2(y'_k)
$$

I have implemented a simple logic which starts with $\alpha = 1$ (so just like it is now) and reduces $\alpha$ whenever the optimization error would increase. This seems reasonable to me: the initial logic is to do normal fixed-point iterations and to gradually become "more damped" when we notice that we don't converge. Perhaps there is a better way to ensure convergence, but this is the most straightforward modification to the current code that I have found.

This problem is not due to the $\tau_x, \tau_y$ parameters; it also occurs when they are zero. In fact, the fixed-point iterations are done when the tilt correction of $\tau_x, \tau_y$ has already been applied. I have added a test to reproduce the problem. This PR fixes #27916.


### Pull Request Readiness Checklist

See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request

- [x] I agree to contribute to the project under Apache 2 License.
- [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV
- [x] The PR is proposed to the proper branch
- [x] There is a reference to the original bug report and related work
- [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable
      Patch to opencv_extra has the same branch name.
- [x] The feature is well documented and sample code can be built with the project CMake
This commit is contained in:
Adrian Kretz
2025-11-21 08:52:02 +01:00
committed by GitHub
parent fdf0332954
commit c0364e4e31
2 changed files with 80 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
modules/calib3d/src/undistort.dispatch.cpp
View File

@@ -445,8 +445,12 @@ static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvM
y0 = y = invProj * vecUntilt(1);
double error = std::numeric_limits<double>::max();
double prevError = std::numeric_limits<double>::max();
// compensate distortion iteratively using fixed-point iteration
// parameter for damped fixed-point iteration
double alpha = 1.;
for( int j = 0; ; j++ )
{
if ((criteria.type & cv::TermCriteria::COUNT) && j >= criteria.maxCount)
@@ -470,10 +474,11 @@ static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvM
// [x'', y'']^T = [x' / icdist + deltaX, y' / icdist + deltaY]^T =>
// [x', y']^T = [(x'' - deltaX) * icdist, (y'' - deltaY) * icdist]^T =>
// x' = f1(x') := (x'' - deltaX) * icdist, y' = f2(y') := (y'' - deltaY) * icdist
// Fixed-point iteration:
// new_x' = f1(x') = (x'' - deltaX) * icdist, new_y' = f2(y') = (y'' - deltaY) * icdist
x = (x0 - deltaX)*icdist;
y = (y0 - deltaY)*icdist;
// Damped fixed-point iteration:
// f1(x') = (x'' - deltaX) * icdist, f2(y') = (y'' - deltaY) * icdist
// new_x' = (1 - alpha) * x' + alpha * f1(x'), new_y' = (1 - alpha) * y' + alpha * f2(y')
double new_x = (1. - alpha)*x + alpha*(x0 - deltaX)*icdist;
double new_y = (1. - alpha)*y + alpha*(y0 - deltaY)*icdist;
if(criteria.type & cv::TermCriteria::EPS)
{
@@ -482,20 +487,20 @@ static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvM
cv::Vec3d vecTilt;
// r^2 = x'^2 + y'^2
r2 = x*x + y*y;
r2 = new_x*new_x + new_y*new_y;
r4 = r2*r2;
r6 = r4*r2;
a1 = 2*x*y;
a2 = r2 + 2*x*x;
a3 = r2 + 2*y*y;
a1 = 2*new_x*new_y;
a2 = r2 + 2*new_x*new_x;
a3 = r2 + 2*new_y*new_y;
// cdist := 1 + k1 * r^2 + k2 * r^4 + k3 * r^6
cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
// icdist2 := 1 / (1 + k4 * r^2 + k5 * r^4 + k6 * r^6)
icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
// x'' = x' * cdist * icdist2 + 2 * p1 * x' * y' + p2 * (r^2 + 2 * x'^2) + s1 * r^2 + s2 * r^4
// y'' = y' * cdist * icdist2 + p1 * (r^2 + 2 * y'^2) + 2 * p2 * x' * y' + s3 * r^2 + s4 * r^4
xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
xd0 = new_x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
yd0 = new_y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
// s * [x''', y''', 1]^T = matTilt * [x'', y'', 1]^T =>
// (vecTilt := matTilt * [x'', y'', 1]^T)
@@ -513,6 +518,13 @@ static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvM
error = sqrt( pow(x_proj - u, 2) + pow(y_proj - v, 2) );
}
if (error > prevError) {
alpha *= .5;
} else {
x = new_x;
y = new_y;
}
prevError = error;
}
}

58
modules/calib3d/test/test_undistort_points.cpp
View File

@@ -16,6 +16,7 @@ protected:
void generateCameraMatrix(Mat& cameraMatrix);
void generateDistCoeffs(Mat& distCoeffs, int count);
cv::Mat generateRotationVector();
std::vector<cv::Point2d> distortPoints(const cv::Mat &cameraMatrix, const cv::Mat &dist, const std::vector<cv::Point2d> &points);
double thresh = 1.0e-2;
};
@@ -60,6 +61,34 @@ cv::Mat UndistortPointsTest::generateRotationVector()
return rvec;
}
std::vector<cv::Point2d> UndistortPointsTest::distortPoints(const cv::Mat &cameraMatrix, const cv::Mat &dist, const std::vector<cv::Point2d> &points)
{
CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3);
CV_Assert(cameraMatrix.type() == CV_64F);
CV_Assert(dist.rows * dist.cols == 12);
CV_Assert(dist.type() == CV_64F);
double *k = reinterpret_cast<double *>(dist.data);
double fx = cameraMatrix.at<double>(0, 0);
double fy = cameraMatrix.at<double>(1, 1);
double cx = cameraMatrix.at<double>(0, 2);
double cy = cameraMatrix.at<double>(1, 2);
std::vector<cv::Point2d> distortedPoints;
distortedPoints.reserve(points.size());
for (const cv::Point2d p : points) {
double x = (p.x - cx) / fx;
double y = (p.y - cy) / fy;
double r2 = x*x + y*y;
double cdist = (1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2)/(1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2);
CV_Assert(cdist >= 0);
double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x)+ k[8]*r2+k[9]*r2*r2;
double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y+ k[10]*r2+k[11]*r2*r2;
distortedPoints.push_back(cv::Point2d((x * cdist + deltaX) * fx + cx, (y * cdist + deltaY) * fy + cy));
}
return distortedPoints;
}
TEST_F(UndistortPointsTest, accuracy)
{
Mat intrinsics, distCoeffs;
@@ -196,4 +225,33 @@ TEST_F(UndistortPointsTest, regression_14583)
<< "undistort point: " << undistort_pt;
}
TEST_F(UndistortPointsTest, regression_27916)
{
cv::Mat K = (cv::Mat_<double>(3, 3) <<
1570.8956145992222, 0., 744.87337646727406, 0.,
1570.3494207432338, 575.55087456337526, 0., 0., 1.);
cv::Mat dist = (cv::Mat_<double>(1, 12) <<
-2.8247717583453804, -0.80078070764368037,
-0.014595359484103326, 0.0018820998949700702, 1.9827795585249783,
-2.7306773773930897, -1.217725820479524, 2.4052243546080136,
-0.0020670359760441713, 3.4660880793174063e-05,
0.014100351510458799, -3.0935329736207612e-05);
const cv::TermCriteria termCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS, 100, thresh / 2);
std::vector<cv::Point2d> distortedPoints, distortedPoints2;
std::vector<cv::Point2d> undistortedPoints;
for (int i = 0; i < 50; i++)
{
for (int j = 0; j < 50; j++)
{
distortedPoints.push_back(cv::Point2d(i, j));
}
}
cv::undistortPoints(distortedPoints, undistortedPoints, K, dist, cv::noArray(), K, termCriteria);
distortedPoints2 = distortPoints(K, dist, undistortedPoints);
EXPECT_MAT_NEAR(distortedPoints2, distortedPoints, thresh);
}
}} // namespace