diff --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp index a74ac5cf7a..9a2e04a5eb 100644 --- a/modules/calib3d/src/fisheye.cpp +++ b/modules/calib3d/src/fisheye.cpp @@ -429,8 +429,15 @@ void cv::fisheye::undistortPoints( InputArray distorted, OutputArray undistorted for(size_t i = 0; 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 => + // x' = (u - cx) / fx, y' = (v - cy) / fy Vec2d pw((pi[0] - c[0])/f[0], (pi[1] - c[1])/f[1]); // world point + // x' = (theta_d / r) * a, y' = (theta_d / r) * b => + // x'^2 + y'^2 = theta_d^2 * (a^2 + b^2) / r^2 => + // (r^2 = a^2 + b^2) + // x'^2 + y'^2 = theta_d^2 => + // theta_d = sqrt(x'^2 + y'^2) double theta_d = sqrt(pw[0]*pw[0] + pw[1]*pw[1]); // the current camera model is only valid up to 180 FOV @@ -449,9 +456,14 @@ void cv::fisheye::undistortPoints( InputArray distorted, OutputArray undistorted 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; - /* 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; @@ -463,6 +475,10 @@ void cv::fisheye::undistortPoints( InputArray distorted, OutputArray undistorted } } + // x' = (theta_d / r) * a, y' = (theta_d / r) * b => + // a = x' * r / theta_d, b = y' * r / theta_d => + // (theta = atan(r) => r = tan(theta), scale := r / theta_d = tan(theta) / theta_d) + // a = x' * scale, b = y' * scale scale = std::tan(theta) / theta_d; } else @@ -477,6 +493,7 @@ void cv::fisheye::undistortPoints( InputArray distorted, OutputArray undistorted if ((converged || !isEps) && !theta_flipped) { + // a = x' * scale, b = y' * scale Vec2d pu = pw * scale; //undistorted point Vec2d fi;