Merge pull request #17417 from vpisarev:fix_fitellipse

* improved fitEllipse and fitEllipseDirect accuracy in singular or close-to-singular cases (see issue #9923)

* scale points using double precision

* added normalization to fitEllipseAMS as well; fixed Java test case by raising the tolerance (it's unclear what is the correct result in this case).

* improved point perturbation a bit. make the code a little bit more clear

* trying to fix Java fitEllipseTest by slightly raising the tolerance threshold

* synchronized C++ version of Java's fitEllipse test

* removed trailing whitespaces

modules/imgproc/misc/java/test/ImgprocTest.java

@@ -797,9 +797,13 @@ public class ImgprocTest extends OpenCVTestCase {
 
         rrect = Imgproc.fitEllipse(points);
 
-        assertPointEquals(new Point(0, 0), rrect.center, EPS);
-        assertEquals(2.828, rrect.size.width, EPS);
-        assertEquals(2.828, rrect.size.height, EPS);
+        double FIT_ELLIPSE_CENTER_EPS = 0.01;
+        double FIT_ELLIPSE_SIZE_EPS = 0.4;
+
+        assertEquals(0.0, rrect.center.x, FIT_ELLIPSE_CENTER_EPS);
+        assertEquals(0.0, rrect.center.y, FIT_ELLIPSE_CENTER_EPS);
+        assertEquals(2.828, rrect.size.width, FIT_ELLIPSE_SIZE_EPS);
+        assertEquals(2.828, rrect.size.height, FIT_ELLIPSE_SIZE_EPS);
     }
 
     public void testFitLine() {

modules/imgproc/src/shapedescr.cpp

@@ -337,8 +337,15 @@ double cv::contourArea( InputArray _contour, bool oriented )
     return a00;
 }
 
+namespace cv
+{
 
-cv::RotatedRect cv::fitEllipse( InputArray _points )
+static inline Point2f getOfs(int i, float eps)
+{
+    return Point2f(((i & 1)*2 - 1)*eps, ((i & 2) - 1)*eps);
+}
+
+static RotatedRect fitEllipseNoDirect( InputArray _points )
 {
     CV_INSTRUMENT_REGION();
 
@@ -354,42 +361,84 @@ cv::RotatedRect cv::fitEllipse( InputArray _points )
 
     // New fitellipse algorithm, contributed by Dr. Daniel Weiss
     Point2f c(0,0);
-    double gfp[5] = {0}, rp[5] = {0}, t;
+    double gfp[5] = {0}, rp[5] = {0}, t, vd[25]={0}, wd[5]={0};
     const double min_eps = 1e-8;
     bool is_float = depth == CV_32F;
-    const Point* ptsi = points.ptr<Point>();
-    const Point2f* ptsf = points.ptr<Point2f>();
 
-    AutoBuffer<double> _Ad(n*5), _bd(n);
-    double *Ad = _Ad.data(), *bd = _bd.data();
+    AutoBuffer<double> _Ad(n*12+n);
+    double *Ad = _Ad.data(), *ud = Ad + n*5, *bd = ud + n*5;
+    Point2f* ptsf_copy = (Point2f*)(bd + n);
 
     // first fit for parameters A - E
     Mat A( n, 5, CV_64F, Ad );
     Mat b( n, 1, CV_64F, bd );
     Mat x( 5, 1, CV_64F, gfp );
+    Mat u( n, 1, CV_64F, ud );
+    Mat vt( 5, 5, CV_64F, vd );
+    Mat w( 5, 1, CV_64F, wd );
 
+    {
+    const Point* ptsi = points.ptr<Point>();
+    const Point2f* ptsf = points.ptr<Point2f>();
     for( i = 0; i < n; i++ )
     {
         Point2f p = is_float ? ptsf[i] : Point2f((float)ptsi[i].x, (float)ptsi[i].y);
+        ptsf_copy[i] = p;
         c += p;
     }
+    }
     c.x /= n;
     c.y /= n;
 
+    double s = 0;
     for( i = 0; i < n; i++ )
     {
-        Point2f p = is_float ? ptsf[i] : Point2f((float)ptsi[i].x, (float)ptsi[i].y);
+        Point2f p = ptsf_copy[i];
         p -= c;
+        s += fabs(p.x) + fabs(p.y);
+    }
+    double scale = 100./(s > FLT_EPSILON ? s : FLT_EPSILON);
+
+    for( i = 0; i < n; i++ )
+    {
+        Point2f p = ptsf_copy[i];
+        p -= c;
+        double px = p.x*scale;
+        double py = p.y*scale;
 
         bd[i] = 10000.0; // 1.0?
-        Ad[i*5] = -(double)p.x * p.x; // A - C signs inverted as proposed by APP
-        Ad[i*5 + 1] = -(double)p.y * p.y;
-        Ad[i*5 + 2] = -(double)p.x * p.y;
-        Ad[i*5 + 3] = p.x;
-        Ad[i*5 + 4] = p.y;
+        Ad[i*5] = -px * px; // A - C signs inverted as proposed by APP
+        Ad[i*5 + 1] = -py * py;
+        Ad[i*5 + 2] = -px * py;
+        Ad[i*5 + 3] = px;
+        Ad[i*5 + 4] = py;
     }
 
-    solve(A, b, x, DECOMP_SVD);
+    SVDecomp(A, w, u, vt);
+    if(wd[0]*FLT_EPSILON > wd[4]) {
+        float eps = (float)(s/(n*2)*1e-3);
+        for( i = 0; i < n; i++ )
+        {
+            Point2f p = ptsf_copy[i] + getOfs(i, eps);
+            ptsf_copy[i] = p;
+        }
+
+        for( i = 0; i < n; i++ )
+        {
+            Point2f p = ptsf_copy[i];
+            p -= c;
+            double px = p.x*scale;
+            double py = p.y*scale;
+            bd[i] = 10000.0; // 1.0?
+            Ad[i*5] = -px * px; // A - C signs inverted as proposed by APP
+            Ad[i*5 + 1] = -py * py;
+            Ad[i*5 + 2] = -px * py;
+            Ad[i*5 + 3] = px;
+            Ad[i*5 + 4] = py;
+        }
+        SVDecomp(A, w, u, vt);
+    }
+    SVBackSubst(w, u, vt, b, x);
 
     // now use general-form parameters A - E to find the ellipse center:
     // differentiate general form wrt x/y to get two equations for cx and cy
@@ -409,12 +458,14 @@ cv::RotatedRect cv::fitEllipse( InputArray _points )
     x = Mat( 3, 1, CV_64F, gfp );
     for( i = 0; i < n; i++ )
     {
-        Point2f p = is_float ? ptsf[i] : Point2f((float)ptsi[i].x, (float)ptsi[i].y);
+        Point2f p = ptsf_copy[i];
         p -= c;
+        double px = p.x*scale;
+        double py = p.y*scale;
         bd[i] = 1.0;
-        Ad[i * 3] = (p.x - rp[0]) * (p.x - rp[0]);
-        Ad[i * 3 + 1] = (p.y - rp[1]) * (p.y - rp[1]);
-        Ad[i * 3 + 2] = (p.x - rp[0]) * (p.y - rp[1]);
+        Ad[i * 3] = (px - rp[0]) * (px - rp[0]);
+        Ad[i * 3 + 1] = (py - rp[1]) * (py - rp[1]);
+        Ad[i * 3 + 2] = (px - rp[0]) * (py - rp[1]);
     }
     solve(A, b, x, DECOMP_SVD);
 
@@ -431,10 +482,10 @@ cv::RotatedRect cv::fitEllipse( InputArray _points )
     if( rp[3] > min_eps )
         rp[3] = std::sqrt(2.0 / rp[3]);
 
-    box.center.x = (float)rp[0] + c.x;
-    box.center.y = (float)rp[1] + c.y;
-    box.size.width = (float)(rp[2]*2);
-    box.size.height = (float)(rp[3]*2);
+    box.center.x = (float)(rp[0]/scale) + c.x;
+    box.center.y = (float)(rp[1]/scale) + c.y;
+    box.size.width = (float)(rp[2]*2/scale);
+    box.size.height = (float)(rp[3]*2/scale);
     if( box.size.width > box.size.height )
     {
         float tmp;
@@ -448,6 +499,16 @@ cv::RotatedRect cv::fitEllipse( InputArray _points )
 
     return box;
 }
+}
+
+cv::RotatedRect cv::fitEllipse( InputArray _points )
+{
+    CV_INSTRUMENT_REGION();
+
+    Mat points = _points.getMat();
+    int n = points.checkVector(2);
+    return n == 5 ? fitEllipseDirect(points) : fitEllipseNoDirect(points);
+}
 
 cv::RotatedRect cv::fitEllipseAMS( InputArray _points )
 {
@@ -483,16 +544,24 @@ cv::RotatedRect cv::fitEllipseAMS( InputArray _points )
     c.x /= (float)n;
     c.y /= (float)n;
 
+    double s = 0;
     for( i = 0; i < n; i++ )
     {
         Point2f p = is_float ? ptsf[i] : Point2f((float)ptsi[i].x, (float)ptsi[i].y);
-        p -= c;
+        s += fabs(p.x - c.x) + fabs(p.y - c.y);
+    }
+    double scale = 100./(s > FLT_EPSILON ? s : (double)FLT_EPSILON);
 
-        A.at<double>(i,0) = (double)(p.x)*(p.x);
-        A.at<double>(i,1) = (double)(p.x)*(p.y);
-        A.at<double>(i,2) = (double)(p.y)*(p.y);
-        A.at<double>(i,3) = (double)p.x;
-        A.at<double>(i,4) = (double)p.y;
+    for( i = 0; i < n; i++ )
+    {
+        Point2f p = is_float ? ptsf[i] : Point2f((float)ptsi[i].x, (float)ptsi[i].y);
+        double px = (p.x - c.x)*scale, py = (p.y - c.y)*scale;
+
+        A.at<double>(i,0) = px*px;
+        A.at<double>(i,1) = px*py;
+        A.at<double>(i,2) = py*py;
+        A.at<double>(i,3) = px;
+        A.at<double>(i,4) = py;
         A.at<double>(i,5) = 1.0;
     }
     cv::mulTransposed( A, DM, true, noArray(), 1.0, -1 );
@@ -587,10 +656,10 @@ cv::RotatedRect cv::fitEllipseAMS( InputArray _points )
             double p1 = 2.0*pVec(2) *pVec(3)  - pVec(1) *pVec(4) ;
             double p2 = 2.0*pVec(0) *pVec(4) -(pVec(1) *pVec(3) );
 
-            x0 = p1/l3 + c.x;
-            y0 = p2/l3 + c.y;
-            a = std::sqrt(2.)*sqrt((u1 - 4.0*u2)/((l1 - l2)*l3));
-            b = std::sqrt(2.)*sqrt(-1.0*((u1 - 4.0*u2)/((l1 + l2)*l3)));
+            x0 = p1/l3/scale + c.x;
+            y0 = p2/l3/scale + c.y;
+            a = std::sqrt(2.)*sqrt((u1 - 4.0*u2)/((l1 - l2)*l3))/scale;
+            b = std::sqrt(2.)*sqrt(-1.0*((u1 - 4.0*u2)/((l1 + l2)*l3)))/scale;
             if (pVec(1)  == 0) {
                 if (pVec(0)  < pVec(2) ) {
                     theta = 0;
@@ -601,8 +670,8 @@ cv::RotatedRect cv::fitEllipseAMS( InputArray _points )
                 theta = CV_PI/2. + 0.5*std::atan2(pVec(1) , (pVec(0)  - pVec(2) ));
             }
 
-            box.center.x = (float)x0; // +c.x;
-            box.center.y = (float)y0; // +c.y;
+            box.center.x = (float)x0;
+            box.center.y = (float)y0;
             box.size.width = (float)(2.0*a);
             box.size.height = (float)(2.0*b);
             if( box.size.width > box.size.height )
@@ -619,7 +688,7 @@ cv::RotatedRect cv::fitEllipseAMS( InputArray _points )
             box = cv::fitEllipseDirect( points );
         }
     } else {
-        box = cv::fitEllipse( points );
+        box = cv::fitEllipseNoDirect( points );
     }
 
     return box;
@@ -630,6 +699,7 @@ cv::RotatedRect cv::fitEllipseDirect( InputArray _points )
     Mat points = _points.getMat();
     int i, n = points.checkVector(2);
     int depth = points.depth();
+    float eps = 0;
     CV_Assert( n >= 0 && (depth == CV_32F || depth == CV_32S));
 
     RotatedRect box;
@@ -637,7 +707,7 @@ cv::RotatedRect cv::fitEllipseDirect( InputArray _points )
     if( n < 5 )
         CV_Error( CV_StsBadSize, "There should be at least 5 points to fit the ellipse" );
 
-    Point2f c(0,0);
+    Point2d c(0., 0.);
 
     bool is_float = (depth == CV_32F);
     const Point*   ptsi = points.ptr<Point>();
@@ -649,25 +719,39 @@ cv::RotatedRect cv::fitEllipseDirect( InputArray _points )
     Matx<double, 3, 1> pVec;
 
     double x0, y0, a, b, theta, Ts;
+    double s = 0;
 
     for( i = 0; i < n; i++ )
     {
         Point2f p = is_float ? ptsf[i] : Point2f((float)ptsi[i].x, (float)ptsi[i].y);
-        c += p;
+        c.x += p.x;
+        c.y += p.y;
     }
-    c.x /= (float)n;
-    c.y /= (float)n;
+    c.x /= n;
+    c.y /= n;
 
     for( i = 0; i < n; i++ )
     {
         Point2f p = is_float ? ptsf[i] : Point2f((float)ptsi[i].x, (float)ptsi[i].y);
-        p -= c;
+        s += fabs(p.x - c.x) + fabs(p.y - c.y);
+    }
+    double scale = 100./(s > FLT_EPSILON ? s : (double)FLT_EPSILON);
 
-        A.at<double>(i,0) = (double)(p.x)*(p.x);
-        A.at<double>(i,1) = (double)(p.x)*(p.y);
-        A.at<double>(i,2) = (double)(p.y)*(p.y);
-        A.at<double>(i,3) = (double)p.x;
-        A.at<double>(i,4) = (double)p.y;
+    // first, try the original pointset.
+    // if it's singular, try to shift the points a bit
+    int iter = 0;
+    for( iter = 0; iter < 2; iter++ ) {
+        for( i = 0; i < n; i++ )
+        {
+            Point2f p = is_float ? ptsf[i] : Point2f((float)ptsi[i].x, (float)ptsi[i].y);
+            Point2f delta = getOfs(i, eps);
+            double px = (p.x + delta.x - c.x)*scale, py = (p.y + delta.y - c.y)*scale;
+
+            A.at<double>(i,0) = px*px;
+            A.at<double>(i,1) = px*py;
+            A.at<double>(i,2) = py*py;
+            A.at<double>(i,3) = px;
+            A.at<double>(i,4) = py;
             A.at<double>(i,5) = 1.0;
         }
         cv::mulTransposed( A, DM, true, noArray(), 1.0, -1 );
@@ -705,7 +789,13 @@ cv::RotatedRect cv::fitEllipseDirect( InputArray _points )
         M(2,1) = (DM(0,1) + (DM(0,3)*TM(0,1) + DM(0,4)*TM(1,1) + DM(0,5)*TM(2,1))/Ts)/2.;
         M(2,2) = (DM(0,2) + (DM(0,3)*TM(0,2) + DM(0,4)*TM(1,2) + DM(0,5)*TM(2,2))/Ts)/2.;
 
-    if (fabs(cv::determinant(M)) > 1.0e-10) {
+        double det = fabs(cv::determinant(M));
+        if (fabs(det) > 1.0e-10)
+            break;
+        eps = (float)(s/(n*2)*1e-2);
+    }
+
+    if( iter < 2 ) {
         Mat eVal, eVec;
         eigenNonSymmetric(M, eVal, eVec);
 
@@ -740,10 +830,10 @@ cv::RotatedRect cv::fitEllipseDirect( InputArray _points )
         double p1 = 2*pVec(2)*Q(0,0) - pVec(1)*Q(0,1);
         double p2 = 2*pVec(0)*Q(0,1) - pVec(1)*Q(0,0);
 
-        x0 = p1/l3 + c.x;
-        y0 = p2/l3 + c.y;
-        a = sqrt(2.)*sqrt((u1 - 4.0*u2)/((l1 - l2)*l3));
-        b = sqrt(2.)*sqrt(-1.0*((u1 - 4.0*u2)/((l1 + l2)*l3)));
+        x0 = (p1/l3/scale) + c.x;
+        y0 = (p2/l3/scale) + c.y;
+        a = sqrt(2.)*sqrt((u1 - 4.0*u2)/((l1 - l2)*l3))/scale;
+        b = sqrt(2.)*sqrt(-1.0*((u1 - 4.0*u2)/((l1 + l2)*l3)))/scale;
         if (pVec(1)  == 0) {
             if (pVec(0)  < pVec(2) ) {
                 theta = 0;
@@ -767,7 +857,7 @@ cv::RotatedRect cv::fitEllipseDirect( InputArray _points )
             box.angle = (float)(fmod(theta*180/CV_PI,180.0));
         };
     } else {
-        box = cv::fitEllipse( points );
+        box = cv::fitEllipseNoDirect( points );
     }
     return box;
 }

modules/imgproc/test/test_fitellipse.cpp

@@ -66,4 +66,40 @@ TEST(Imgproc_FitEllipse_Issue_6544, accuracy) {
     EXPECT_TRUE(fit_and_check_ellipse(pts));
 }
 
+TEST(Imgproc_FitEllipse_Issue_10270, accuracy) {
+    vector<Point2f> pts;
+    float scale = 1;
+    Point2f shift(0, 0);
+    pts.push_back(Point2f(0, 1)*scale+shift);
+    pts.push_back(Point2f(0, 2)*scale+shift);
+    pts.push_back(Point2f(0, 3)*scale+shift);
+    pts.push_back(Point2f(2, 3)*scale+shift);
+    pts.push_back(Point2f(0, 4)*scale+shift);
+
+    // check that we get almost vertical ellipse centered around (1, 3)
+    RotatedRect e = fitEllipse(pts);
+    EXPECT_LT(std::min(fabs(e.angle-180), fabs(e.angle)), 10.);
+    EXPECT_NEAR(e.center.x, 1, 1);
+    EXPECT_NEAR(e.center.y, 3, 1);
+    EXPECT_LT(e.size.width*3, e.size.height);
+}
+
+TEST(Imgproc_FitEllipse_JavaCase, accuracy) {
+    vector<Point2f> pts;
+    float scale = 1;
+    Point2f shift(0, 0);
+    pts.push_back(Point2f(0, 0)*scale+shift);
+    pts.push_back(Point2f(1, 1)*scale+shift);
+    pts.push_back(Point2f(-1, 1)*scale+shift);
+    pts.push_back(Point2f(-1, -1)*scale+shift);
+    pts.push_back(Point2f(1, -1)*scale+shift);
+
+    // check that we get almost vertical ellipse centered around (1, 3)
+    RotatedRect e = fitEllipse(pts);
+    EXPECT_NEAR(e.center.x, 0, 0.01);
+    EXPECT_NEAR(e.center.y, 0, 0.01);
+    EXPECT_NEAR(e.size.width, sqrt(2.)*2, 0.4);
+    EXPECT_NEAR(e.size.height, sqrt(2.)*2, 0.4);
+}
+
 }} // namespace