Merge pull request opencv#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

Author: creinders

modules/calib3d/src/fisheye.cpp

@@ -377,21 +377,24 @@ 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
         // for larger FOV the loop below does not converge
         // 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<double>::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;
         }
+        else
+        {
+            converged = true;
+        }
 
-        Vec2d pu = pw * scale; //undistorted point
+        // 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));
 
-        // 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 (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;
+            if( sdepth == CV_32F )
+                dstf[i] = fi;
+            else
+                dstd[i] = fi;
+        }
         else
-            dstd[i] = fi;
+        {
+            // Vec2d fi(std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN());
+            Vec2d fi(-1000000.0, -1000000.0);
+
+            if( sdepth == CV_32F )
+                dstf[i] = fi;
+            else
+                dstd[i] = fi;
+        }
     }
 }
 

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<cv::Vec2f>();
+
+    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<Vec2f>(y,x);
+            Vec3b & pixel = image_projected.at<Vec3b>(y,x);
+
+            Vec2f & undist_vec_gt = undPointsGt.at<Vec2f>(y,x);
+            Vec3b & pixel_gt = imageGt.at<Vec3b>(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<Vec2f>(400, 640);
+    Vec2f dist_point_1_gt(640.044f, 400.041f);
+
+    Vec2f dist_point_2 = distPoints.at<Vec2f>(400, 440);
+    Vec2f dist_point_2_gt(409.731f, 403.029f);
+
+    Vec2f dist_point_3 = distPoints.at<Vec2f>(200, 640);
+    Vec2f dist_point_3_gt(643.341f, 168.896f);
+
+    Vec2f dist_point_4 = distPoints.at<Vec2f>(300, 480);
+    Vec2f dist_point_4_gt(463.402f, 290.317f);
+
+    Vec2f dist_point_5 = distPoints.at<Vec2f>(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;