diff --git a/doc/opencv.bib b/doc/opencv.bib index e3363a181c..a0c6fb0f9d 100644 --- a/doc/opencv.bib +++ b/doc/opencv.bib @@ -230,6 +230,13 @@ volume = {9}, publisher = {Walter de Gruyter} } +@misc{Chatfield2017, + author = {Chatfield, Carl}, + title = {A Simple Method for Distance to Ellipse}, + year = {2017}, + publisher = {GitHub}, + howpublished = {\url{https://blog.chatfield.io/simple-method-for-distance-to-ellipse/}}, +} @article{Chaumette06, author = {Chaumette, Fran{\c c}ois and Hutchinson, S.}, title = {{Visual servo control, Part I: Basic approaches}}, diff --git a/modules/imgproc/include/opencv2/imgproc.hpp b/modules/imgproc/include/opencv2/imgproc.hpp index 19c73079e6..693e342f0d 100644 --- a/modules/imgproc/include/opencv2/imgproc.hpp +++ b/modules/imgproc/include/opencv2/imgproc.hpp @@ -4297,6 +4297,9 @@ ellipse/rotatedRect data contains negative indices, due to the data points being border of the containing Mat element. @param points Input 2D point set, stored in std::vector\<\> or Mat + +@note Input point types are @ref Point2i or @ref Point2f and at least 5 points are required. +@note @ref getClosestEllipsePoints function can be used to compute the ellipse fitting error. */ CV_EXPORTS_W RotatedRect fitEllipse( InputArray points ); @@ -4334,6 +4337,9 @@ CV_EXPORTS_W RotatedRect fitEllipse( InputArray points ); \f} @param points Input 2D point set, stored in std::vector\<\> or Mat + + @note Input point types are @ref Point2i or @ref Point2f and at least 5 points are required. + @note @ref getClosestEllipsePoints function can be used to compute the ellipse fitting error. */ CV_EXPORTS_W RotatedRect fitEllipseAMS( InputArray points ); @@ -4379,9 +4385,26 @@ CV_EXPORTS_W RotatedRect fitEllipseAMS( InputArray points ); The scaling factor guarantees that \f$A^T C A =1\f$. @param points Input 2D point set, stored in std::vector\<\> or Mat + + @note Input point types are @ref Point2i or @ref Point2f and at least 5 points are required. + @note @ref getClosestEllipsePoints function can be used to compute the ellipse fitting error. */ CV_EXPORTS_W RotatedRect fitEllipseDirect( InputArray points ); +/** @brief Compute for each 2d point the nearest 2d point located on a given ellipse. + + The function computes the nearest 2d location on a given ellipse for a vector of 2d points and is based on @cite Chatfield2017 code. + This function can be used to compute for instance the ellipse fitting error. + + @param ellipse_params Ellipse parameters + @param points Input 2d points + @param closest_pts For each 2d point, their corresponding closest 2d point located on a given ellipse + + @note Input point types are @ref Point2i or @ref Point2f + @see fitEllipse, fitEllipseAMS, fitEllipseDirect + */ +CV_EXPORTS_W void getClosestEllipsePoints( const RotatedRect& ellipse_params, InputArray points, OutputArray closest_pts ); + /** @brief Fits a line to a 2D or 3D point set. The function fitLine fits a line to a 2D or 3D point set by minimizing \f$\sum_i \rho(r_i)\f$ where diff --git a/modules/imgproc/src/shapedescr.cpp b/modules/imgproc/src/shapedescr.cpp index 007bf9ac62..197800d18e 100644 --- a/modules/imgproc/src/shapedescr.cpp +++ b/modules/imgproc/src/shapedescr.cpp @@ -862,6 +862,123 @@ cv::RotatedRect cv::fitEllipseDirect( InputArray _points ) return box; } +namespace cv +{ +// @misc{Chatfield2017, +// author = {Chatfield, Carl}, +// title = {A Simple Method for Distance to Ellipse}, +// year = {2017}, +// publisher = {GitHub}, +// howpublished = {\url{https://blog.chatfield.io/simple-method-for-distance-to-ellipse/}}, +// } +// https://github.com/0xfaded/ellipse_demo/blob/master/ellipse_trig_free.py +static void solveFast(float semi_major, float semi_minor, const cv::Point2f& pt, cv::Point2f& closest_pt) +{ + float px = std::abs(pt.x); + float py = std::abs(pt.y); + + float tx = 0.707f; + float ty = 0.707f; + + float a = semi_major; + float b = semi_minor; + + for (int iter = 0; iter < 3; iter++) + { + float x = a * tx; + float y = b * ty; + + float ex = (a*a - b*b) * tx*tx*tx / a; + float ey = (b*b - a*a) * ty*ty*ty / b; + + float rx = x - ex; + float ry = y - ey; + + float qx = px - ex; + float qy = py - ey; + + float r = std::hypotf(rx, ry); + float q = std::hypotf(qx, qy); + + tx = std::min(1.0f, std::max(0.0f, (qx * r / q + ex) / a)); + ty = std::min(1.0f, std::max(0.0f, (qy * r / q + ey) / b)); + float t = std::hypotf(tx, ty); + tx /= t; + ty /= t; + } + + closest_pt.x = std::copysign(a * tx, pt.x); + closest_pt.y = std::copysign(b * ty, pt.y); +} +} // namespace cv + +void cv::getClosestEllipsePoints( const RotatedRect& ellipse_params, InputArray _points, OutputArray closest_pts ) +{ + CV_INSTRUMENT_REGION(); + + Mat points = _points.getMat(); + int n = points.checkVector(2); + int depth = points.depth(); + CV_Assert(depth == CV_32F || depth == CV_32S); + CV_Assert(n > 0); + + bool is_float = depth == CV_32F; + const Point* ptsi = points.ptr(); + const Point2f* ptsf = points.ptr(); + + float semi_major = ellipse_params.size.width / 2.0f; + float semi_minor = ellipse_params.size.height / 2.0f; + float angle_deg = ellipse_params.angle; + if (semi_major < semi_minor) + { + std::swap(semi_major, semi_minor); + angle_deg += 90; + } + + Matx23f align_T_ori_f32; + float theta_rad = static_cast(angle_deg * M_PI / 180); + float co = std::cos(theta_rad); + float si = std::sin(theta_rad); + float shift_x = ellipse_params.center.x; + float shift_y = ellipse_params.center.y; + + align_T_ori_f32(0,0) = co; + align_T_ori_f32(0,1) = si; + align_T_ori_f32(0,2) = -co*shift_x - si*shift_y; + align_T_ori_f32(1,0) = -si; + align_T_ori_f32(1,1) = co; + align_T_ori_f32(1,2) = si*shift_x - co*shift_y; + + Matx23f ori_T_align_f32; + ori_T_align_f32(0,0) = co; + ori_T_align_f32(0,1) = -si; + ori_T_align_f32(0,2) = shift_x; + ori_T_align_f32(1,0) = si; + ori_T_align_f32(1,1) = co; + ori_T_align_f32(1,2) = shift_y; + + std::vector closest_pts_list; + for (int i = 0; i < n; i++) + { + Point2f p = is_float ? ptsf[i] : Point2f((float)ptsi[i].x, (float)ptsi[i].y); + Matx31f pmat; + pmat(0,0) = p.x; + pmat(1,0) = p.y; + pmat(2,0) = 1; + + Mat X_align = Mat(align_T_ori_f32) * pmat; + Point2f closest_pt; + solveFast(semi_major, semi_minor, Point2f(X_align.at(0,0), X_align.at(1,0)), closest_pt); + + pmat(0,0) = closest_pt.x; + pmat(1,0) = closest_pt.y; + Mat closest_pt_ori = Mat(ori_T_align_f32) * pmat; + closest_pts_list.push_back(Point2f(closest_pt_ori.at(0,0), closest_pt_ori.at(1,0))); + } + + cv::Mat(closest_pts_list).convertTo(closest_pts, CV_32F); +} + ////////////////////////////////////////////// C API /////////////////////////////////////////// CV_IMPL int diff --git a/modules/imgproc/test/test_fitellipse.cpp b/modules/imgproc/test/test_fitellipse.cpp index b564db96da..7fa35d0878 100644 --- a/modules/imgproc/test/test_fitellipse.cpp +++ b/modules/imgproc/test/test_fitellipse.cpp @@ -102,4 +102,197 @@ TEST(Imgproc_FitEllipse_JavaCase, accuracy) { EXPECT_NEAR(e.size.height, sqrt(2.)*2, 0.4); } +template +static float get_ellipse_fitting_error(const std::vector& points, const Mat& closest_points) { + float mse = 0.0f; + for (int i = 0; i < static_cast(points.size()); i++) + { + Point2f pt_err = Point2f(static_cast(points[i].x), static_cast(points[i].y)) - closest_points.at(i); + mse += pt_err.x*pt_err.x + pt_err.y*pt_err.y; + } + return mse / points.size(); +} + +TEST(Imgproc_getClosestEllipsePoints, ellipse_mse) { + // https://github.com/opencv/opencv/issues/26078 + std::vector points_list; + + // [1434, 308], [1434, 309], [1433, 310], [1427, 310], [1427, 312], [1426, 313], [1422, 313], [1422, 314], + points_list.push_back(Point2i(1434, 308)); + points_list.push_back(Point2i(1434, 309)); + points_list.push_back(Point2i(1433, 310)); + points_list.push_back(Point2i(1427, 310)); + points_list.push_back(Point2i(1427, 312)); + points_list.push_back(Point2i(1426, 313)); + points_list.push_back(Point2i(1422, 313)); + points_list.push_back(Point2i(1422, 314)); + + // [1421, 315], [1415, 315], [1415, 316], [1414, 317], [1408, 317], [1408, 319], [1407, 320], [1403, 320], + points_list.push_back(Point2i(1421, 315)); + points_list.push_back(Point2i(1415, 315)); + points_list.push_back(Point2i(1415, 316)); + points_list.push_back(Point2i(1414, 317)); + points_list.push_back(Point2i(1408, 317)); + points_list.push_back(Point2i(1408, 319)); + points_list.push_back(Point2i(1407, 320)); + points_list.push_back(Point2i(1403, 320)); + + // [1403, 321], [1402, 322], [1396, 322], [1396, 323], [1395, 324], [1389, 324], [1389, 326], [1388, 327], + points_list.push_back(Point2i(1403, 321)); + points_list.push_back(Point2i(1402, 322)); + points_list.push_back(Point2i(1396, 322)); + points_list.push_back(Point2i(1396, 323)); + points_list.push_back(Point2i(1395, 324)); + points_list.push_back(Point2i(1389, 324)); + points_list.push_back(Point2i(1389, 326)); + points_list.push_back(Point2i(1388, 327)); + + // [1382, 327], [1382, 328], [1381, 329], [1376, 329], [1376, 330], [1375, 331], [1369, 331], [1369, 333], + points_list.push_back(Point2i(1382, 327)); + points_list.push_back(Point2i(1382, 328)); + points_list.push_back(Point2i(1381, 329)); + points_list.push_back(Point2i(1376, 329)); + points_list.push_back(Point2i(1376, 330)); + points_list.push_back(Point2i(1375, 331)); + points_list.push_back(Point2i(1369, 331)); + points_list.push_back(Point2i(1369, 333)); + + // [1368, 334], [1362, 334], [1362, 335], [1361, 336], [1359, 336], [1359, 1016], [1365, 1016], [1366, 1017], + points_list.push_back(Point2i(1368, 334)); + points_list.push_back(Point2i(1362, 334)); + points_list.push_back(Point2i(1362, 335)); + points_list.push_back(Point2i(1361, 336)); + points_list.push_back(Point2i(1359, 336)); + points_list.push_back(Point2i(1359, 1016)); + points_list.push_back(Point2i(1365, 1016)); + points_list.push_back(Point2i(1366, 1017)); + + // [1366, 1019], [1430, 1019], [1430, 1017], [1431, 1016], [1440, 1016], [1440, 308] + points_list.push_back(Point2i(1366, 1019)); + points_list.push_back(Point2i(1430, 1019)); + points_list.push_back(Point2i(1430, 1017)); + points_list.push_back(Point2i(1431, 1016)); + points_list.push_back(Point2i(1440, 1016)); + points_list.push_back(Point2i(1440, 308)); + + RotatedRect fit_ellipse_params( + Point2f(1442.97900390625, 662.1879272460938), + Size2f(579.5570678710938, 730.834228515625), + 20.190902709960938 + ); + + // Point2i + { + Mat pointsi(points_list); + Mat closest_pts; + getClosestEllipsePoints(fit_ellipse_params, pointsi, closest_pts); + EXPECT_TRUE(pointsi.rows == closest_pts.rows); + EXPECT_TRUE(pointsi.cols == closest_pts.cols); + EXPECT_TRUE(pointsi.channels() == closest_pts.channels()); + + float fit_ellipse_mse = get_ellipse_fitting_error(points_list, closest_pts); + EXPECT_NEAR(fit_ellipse_mse, 1.61994, 1e-4); + } + + // Point2f + { + Mat pointsf; + Mat(points_list).convertTo(pointsf, CV_32F); + + Mat closest_pts; + getClosestEllipsePoints(fit_ellipse_params, pointsf, closest_pts); + EXPECT_TRUE(pointsf.rows == closest_pts.rows); + EXPECT_TRUE(pointsf.cols == closest_pts.cols); + EXPECT_TRUE(pointsf.channels() == closest_pts.channels()); + + float fit_ellipse_mse = get_ellipse_fitting_error(points_list, closest_pts); + EXPECT_NEAR(fit_ellipse_mse, 1.61994, 1e-4); + } +} + +static std::vector sample_ellipse_pts(const RotatedRect& ellipse_params) { + // Sample N points using the ellipse parametric form + float xc = ellipse_params.center.x; + float yc = ellipse_params.center.y; + float a = ellipse_params.size.width / 2; + float b = ellipse_params.size.height / 2; + float theta = static_cast(ellipse_params.angle * M_PI / 180); + + float cos_th = std::cos(theta); + float sin_th = std::sin(theta); + int nb_samples = 180; + std::vector ellipse_pts(nb_samples); + for (int i = 0; i < nb_samples; i++) { + float ax = a * cos_th; + float ay = a * sin_th; + float bx = -b * sin_th; + float by = b * cos_th; + + float t = static_cast(i / static_cast(nb_samples) * 2*M_PI); + float cos_t = std::cos(t); + float sin_t = std::sin(t); + + ellipse_pts[i].x = xc + ax*cos_t + bx*sin_t; + ellipse_pts[i].y = yc + ay*cos_t + by*sin_t; + } + + return ellipse_pts; +} + +TEST(Imgproc_getClosestEllipsePoints, ellipse_mse_2) { + const float tol = 1e-3f; + + // bb height > width + // Chech correctness of the minor/major axes swapping and updated angle in getClosestEllipsePoints + { + RotatedRect ellipse_params( + Point2f(-142.97f, -662.1878f), + Size2f(539.557f, 730.83f), + 27.09960938f + ); + std::vector ellipse_pts = sample_ellipse_pts(ellipse_params); + + Mat pointsf, closest_pts; + Mat(ellipse_pts).convertTo(pointsf, CV_32F); + getClosestEllipsePoints(ellipse_params, pointsf, closest_pts); + + float ellipse_pts_mse = get_ellipse_fitting_error(ellipse_pts, closest_pts); + EXPECT_NEAR(ellipse_pts_mse, 0, tol); + } + + // bb height > width + negative angle + { + RotatedRect ellipse_params( + Point2f(-142.97f, 562.1878f), + Size2f(53.557f, 730.83f), + -75.09960938f + ); + std::vector ellipse_pts = sample_ellipse_pts(ellipse_params); + + Mat pointsf, closest_pts; + Mat(ellipse_pts).convertTo(pointsf, CV_32F); + getClosestEllipsePoints(ellipse_params, pointsf, closest_pts); + + float ellipse_pts_mse = get_ellipse_fitting_error(ellipse_pts, closest_pts); + EXPECT_NEAR(ellipse_pts_mse, 0, tol); + } + + // Negative angle + { + RotatedRect ellipse_params( + Point2f(742.97f, -462.1878f), + Size2f(535.57f, 130.83f), + -75.09960938f + ); + std::vector ellipse_pts = sample_ellipse_pts(ellipse_params); + + Mat pointsf, closest_pts; + Mat(ellipse_pts).convertTo(pointsf, CV_32F); + getClosestEllipsePoints(ellipse_params, pointsf, closest_pts); + + float ellipse_pts_mse = get_ellipse_fitting_error(ellipse_pts, closest_pts); + EXPECT_NEAR(ellipse_pts_mse, 0, tol); + } +} + }} // namespace