diff --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp index 3ed9ba43f1..96d74cc141 100644 --- a/modules/calib3d/src/fisheye.cpp +++ b/modules/calib3d/src/fisheye.cpp @@ -388,10 +388,18 @@ void cv::fisheye::undistortPoints( InputArray distorted, OutputArray undistorted { // compensate distortion iteratively double theta = theta_d; - for(int j = 0; j < 10; j++ ) + + const double EPS = 1e-8; // or std::numeric_limits::epsilon(); + for (int j = 0; j < 10; j++) { double theta2 = theta*theta, theta4 = theta2*theta2, theta6 = theta4*theta2, theta8 = theta6*theta2; - theta = theta_d / (1 + k[0] * theta2 + k[1] * theta4 + k[2] * theta6 + k[3] * theta8); + double k0_theta2 = k[0] * theta2, k1_theta4 = k[1] * theta4, k2_theta6 = k[2] * theta6, k3_theta8 = k[3] * theta8; + /* new_theta = theta - theta_fix, theta_fix = f0(theta) / f0'(theta) */ + 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 (fabs(theta_fix) < EPS) + break; } scale = std::tan(theta) / theta_d;