From 657c8d1c65b88173521b7955ed2f2ac725a5a5b2 Mon Sep 17 00:00:00 2001 From: Christoph Date: Sat, 4 Jul 2020 00:59:19 +0200 Subject: [PATCH] Merge pull request #17454 from creinders:master fix instable fisheye undistortPoints * remove artefacts when (un)distorting fisheye images with large distortion coefficient values * fix fisheye undistortion when theta is close to zero * add fisheye image undistort and distort test * Fixed type conversion warnings * fixed trailing whitespace --- modules/calib3d/src/fisheye.cpp | 56 ++++++++++---- modules/calib3d/test/test_fisheye.cpp | 102 ++++++++++++++++++++++++++ 2 files changed, 144 insertions(+), 14 deletions(-) diff --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp index 56fd82114d..2325b00598 100644 --- a/modules/calib3d/src/fisheye.cpp +++ b/modules/calib3d/src/fisheye.cpp @@ -377,8 +377,6 @@ void cv::fisheye::undistortPoints( InputArray distorted, OutputArray undistorted Vec2d pi = sdepth == CV_32F ? (Vec2d)srcf[i] : srcd[i]; // image point Vec2d pw((pi[0] - c[0])/f[0], (pi[1] - c[1])/f[1]); // world point - double scale = 1.0; - double theta_d = sqrt(pw[0]*pw[0] + pw[1]*pw[1]); // the current camera model is only valid up to 180 FOV @@ -386,12 +384,17 @@ void cv::fisheye::undistortPoints( InputArray distorted, OutputArray undistorted // clip values so we still get plausible results for super fisheye images > 180 grad theta_d = min(max(-CV_PI/2., theta_d), CV_PI/2.); - if (theta_d > 1e-8) + bool converged = false; + double theta = theta_d; + + double scale = 0.0; + + if (fabs(theta_d) > 1e-8) { // compensate distortion iteratively - double theta = theta_d; 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; @@ -401,22 +404,47 @@ void cv::fisheye::undistortPoints( InputArray distorted, OutputArray undistorted (1 + 3*k0_theta2 + 5*k1_theta4 + 7*k2_theta6 + 9*k3_theta8); theta = theta - theta_fix; if (fabs(theta_fix) < EPS) + { + converged = true; break; + } } scale = std::tan(theta) / theta_d; } - - Vec2d pu = pw * scale; //undistorted point - - // reproject - Vec3d pr = RR * Vec3d(pu[0], pu[1], 1.0); // rotated point optionally multiplied by new camera matrix - Vec2d fi(pr[0]/pr[2], pr[1]/pr[2]); // final - - if( sdepth == CV_32F ) - dstf[i] = fi; else - dstd[i] = fi; + { + converged = true; + } + + // theta is monotonously increasing or decreasing depending on the sign of theta + // if theta has flipped, it might converge due to symmetry but on the opposite of the camera center + // so we can check whether theta has changed the sign during the optimization + bool theta_flipped = ((theta_d < 0 && theta > 0) || (theta_d > 0 && theta < 0)); + + if (converged && !theta_flipped) + { + Vec2d pu = pw * scale; //undistorted point + + // reproject + Vec3d pr = RR * Vec3d(pu[0], pu[1], 1.0); // rotated point optionally multiplied by new camera matrix + Vec2d fi(pr[0]/pr[2], pr[1]/pr[2]); // final + + if( sdepth == CV_32F ) + dstf[i] = fi; + else + dstd[i] = fi; + } + else + { + // Vec2d fi(std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); + Vec2d fi(-1000000.0, -1000000.0); + + if( sdepth == CV_32F ) + dstf[i] = fi; + else + dstd[i] = fi; + } } } diff --git a/modules/calib3d/test/test_fisheye.cpp b/modules/calib3d/test/test_fisheye.cpp index eedc2fa4fe..5acc5cafaa 100644 --- a/modules/calib3d/test/test_fisheye.cpp +++ b/modules/calib3d/test/test_fisheye.cpp @@ -141,6 +141,108 @@ TEST_F(fisheyeTest, undistortImage) } } +TEST_F(fisheyeTest, undistortAndDistortImage) +{ + cv::Matx33d K_src = this->K; + cv::Mat D_src = cv::Mat(this->D); + std::string file = combine(datasets_repository_path, "/calib-3_stereo_from_JY/left/stereo_pair_014.jpg"); + cv::Matx33d K_dst = K_src; + cv::Mat image = cv::imread(file), image_projected; + cv::Vec4d D_dst_vec (-1.0, 0.0, 0.0, 0.0); + cv::Mat D_dst = cv::Mat(D_dst_vec); + + int imageWidth = (int)this->imageSize.width; + int imageHeight = (int)this->imageSize.height; + + cv::Mat imagePoints(imageHeight, imageWidth, CV_32FC2), undPoints, distPoints; + cv::Vec2f* pts = imagePoints.ptr(); + + for(int y = 0, k = 0; y < imageHeight; ++y) + { + for(int x = 0; x < imageWidth; ++x) + { + cv::Vec2f point((float)x, (float)y); + pts[k++] = point; + } + } + + cv::fisheye::undistortPoints(imagePoints, undPoints, K_dst, D_dst); + cv::fisheye::distortPoints(undPoints, distPoints, K_src, D_src); + cv::remap(image, image_projected, distPoints, cv::noArray(), cv::INTER_LINEAR); + + float dx, dy, r_sq; + float R_MAX = 250; + float imageCenterX = (float)imageWidth / 2; + float imageCenterY = (float)imageHeight / 2; + + cv::Mat undPointsGt(imageHeight, imageWidth, CV_32FC2); + cv::Mat imageGt(imageHeight, imageWidth, CV_8UC3); + + for(int y = 0, k = 0; y < imageHeight; ++y) + { + for(int x = 0; x < imageWidth; ++x) + { + dx = x - imageCenterX; + dy = y - imageCenterY; + r_sq = dy * dy + dx * dx; + + Vec2f & und_vec = undPoints.at(y,x); + Vec3b & pixel = image_projected.at(y,x); + + Vec2f & undist_vec_gt = undPointsGt.at(y,x); + Vec3b & pixel_gt = imageGt.at(y,x); + + if (r_sq > R_MAX * R_MAX) + { + + undist_vec_gt[0] = -1e6; + undist_vec_gt[1] = -1e6; + + pixel_gt[0] = 0; + pixel_gt[1] = 0; + pixel_gt[2] = 0; + } + else + { + undist_vec_gt[0] = und_vec[0]; + undist_vec_gt[1] = und_vec[1]; + + pixel_gt[0] = pixel[0]; + pixel_gt[1] = pixel[1]; + pixel_gt[2] = pixel[2]; + } + + k++; + } + } + + EXPECT_MAT_NEAR(undPoints, undPointsGt, 1e-10); + EXPECT_MAT_NEAR(image_projected, imageGt, 1e-10); + + Vec2f dist_point_1 = distPoints.at(400, 640); + Vec2f dist_point_1_gt(640.044f, 400.041f); + + Vec2f dist_point_2 = distPoints.at(400, 440); + Vec2f dist_point_2_gt(409.731f, 403.029f); + + Vec2f dist_point_3 = distPoints.at(200, 640); + Vec2f dist_point_3_gt(643.341f, 168.896f); + + Vec2f dist_point_4 = distPoints.at(300, 480); + Vec2f dist_point_4_gt(463.402f, 290.317f); + + Vec2f dist_point_5 = distPoints.at(550, 750); + Vec2f dist_point_5_gt(797.51f, 611.637f); + + EXPECT_MAT_NEAR(dist_point_1, dist_point_1_gt, 1e-2); + EXPECT_MAT_NEAR(dist_point_2, dist_point_2_gt, 1e-2); + EXPECT_MAT_NEAR(dist_point_3, dist_point_3_gt, 1e-2); + EXPECT_MAT_NEAR(dist_point_4, dist_point_4_gt, 1e-2); + EXPECT_MAT_NEAR(dist_point_5, dist_point_5_gt, 1e-2); + + CV_Assert(cv::imwrite(combine(datasets_repository_path, "new_distortion.png"), image_projected)); +} + TEST_F(fisheyeTest, jacobians) { int n = 10;