Untitled
unknown
c_cpp
12 days ago
35 kB
3
Indexable
#include <HXLDCont.h> #include <common/CommonFunctions.h> namespace { struct circleParams { double centerRow; double centerCol; double radius; double startPhi; double endPhi; std::string pointOrder; }; circleParams fitContourAlgebraic(const std::vector<cv::Point2f>& points) { circleParams res = {0, 0, 0, 0, 0, ""}; int n = points.size(); if (n < 3) { return res; } cv::Mat A(n, 3, CV_64F); cv::Mat b(n, 1, CV_64F); for (int i = 0; i < n; i++) { double r = points[i].y; double c = points[i].x; A.at<double>(i, 0) = r; A.at<double>(i, 1) = c; A.at<double>(i, 2) = 1.0; b.at<double>(i, 0) = -(r * r + c * c); } cv::Mat sol; cv::solve(A, b, sol, cv::DECOMP_SVD); double D = sol.at<double>(0, 0); double E = sol.at<double>(1, 0); double F = sol.at<double>(2, 0); double centerRow = -D / 2.0; double centerCol = -E / 2.0; double radius = std::sqrt(centerRow * centerRow + centerCol * centerCol - F); res.centerRow = centerRow; res.centerCol = centerCol; res.radius = radius; res.startPhi = std::atan2(points.front().y - centerRow, points.front().x - centerCol); res.endPhi = std::atan2(points.back().y - centerRow, points.back().x - centerCol); double crossSum = 0.0; for (int i = 0; i < n - 1; i++) { double x1 = points[i].x - centerCol; double y1 = points[i].y - centerRow; double x2 = points[i + 1].x - centerCol; double y2 = points[i + 1].y - centerRow; crossSum += (x1 * y2 - y1 * x2); } res.pointOrder = (crossSum > 0) ? "positive" : "negative"; return res; } static double robustSigma(const std::vector<double>& errors) { std::vector<double> absErrors; absErrors.reserve(errors.size()); for (double e : errors) { absErrors.push_back(std::fabs(e)); } std::nth_element(absErrors.begin(), absErrors.begin() + absErrors.size() / 2, absErrors.end()); double median = absErrors[absErrors.size() / 2]; return 1.4826 * median; } circleParams fitCircleAHuber(const std::vector<cv::Point2f>& points, double maxClosureDist, long clippingEndPoints, long iterations, double clippingFactor) { std::vector<cv::Point2f> selectedPoints; selectedPoints = points; size_t n = selectedPoints.size(); size_t startIdx = std::min(static_cast<size_t>(clippingEndPoints), n); size_t endIdx = (n > static_cast<size_t>(clippingEndPoints)) ? n - clippingEndPoints : n; if (endIdx <= startIdx) { startIdx = 0; endIdx = n; } std::vector<cv::Point2f> fitPoints(selectedPoints.begin() + startIdx, selectedPoints.begin() + endIdx); double S_xx = 0, S_xy = 0, S_yy = 0, S_x = 0, S_y = 0, S_1 = 0; double S_x_r = 0, S_y_r = 0, S_r = 0; for (auto pt : fitPoints) { double x = pt.x; double y = pt.y; double r2 = x * x + y * y; S_xx += x * x; S_xy += x * y; S_yy += y * y; S_x += x; S_y += y; S_1 += 1; S_x_r += x * r2; S_y_r += y * r2; S_r += r2; } cv::Mat M = (cv::Mat_<double>(3, 3) << S_xx, S_xy, S_x, S_xy, S_yy, S_y, S_x, S_y, S_1); cv::Mat b = (cv::Mat_<double>(3, 1) << -S_x_r, -S_y_r, -S_r); cv::Mat params; bool solved = cv::solve(M, b, params, cv::DECOMP_SVD); double A = params.at<double>(0); double B = params.at<double>(1); double C = params.at<double>(2); double cx = -A / 2.0; double cy = -B / 2.0; double r = std::sqrt(cx * cx + cy * cy - C); for (long it = 0; it < iterations; ++it) { std::vector<double> residuals; residuals.reserve(fitPoints.size()); for (auto pt : fitPoints) { double dist = std::sqrt((pt.x - cx) * (pt.x - cx) + (pt.y - cy) * (pt.y - cy)); double res = dist - r; residuals.push_back(res); } double sigma = robustSigma(residuals); double delta = clippingFactor * sigma; if (sigma < 1e-8) break; std::vector<double> weights; weights.reserve(fitPoints.size()); for (double res : residuals) { double abs_res = std::fabs(res); double w = (abs_res <= delta ? 1.0 : delta / abs_res); weights.push_back(w); } double Wxx = 0, Wxy = 0, Wyy = 0, Wx = 0, Wy = 0, W1 = 0; double Wx_r = 0, Wy_r = 0, Wr = 0; for (size_t i = 0; i < fitPoints.size(); ++i) { double w = weights[i]; double x = fitPoints[i].x; double y = fitPoints[i].y; double r2 = x * x + y * y; Wxx += w * x * x; Wxy += w * x * y; Wyy += w * y * y; Wx += w * x; Wy += w * y; W1 += w; Wx_r += w * x * r2; Wy_r += w * y * r2; Wr += w * r2; } cv::Mat WM = (cv::Mat_<double>(3, 3) << Wxx, Wxy, Wx, Wxy, Wyy, Wy, Wx, Wy, W1); cv::Mat Wb = (cv::Mat_<double>(3, 1) << -Wx_r, -Wy_r, -Wr); cv::Mat Wparams; bool solvedW = cv::solve(WM, Wb, Wparams, cv::DECOMP_SVD); if (!solvedW) break; A = Wparams.at<double>(0); B = Wparams.at<double>(1); C = Wparams.at<double>(2); double new_cx = -A / 2.0; double new_cy = -B / 2.0; double new_r = std::sqrt(new_cx * new_cx + new_cy * new_cy - C); if (std::fabs(new_cx - cx) < 1e-6 && std::fabs(new_cy - cy) < 1e-6 && std::fabs(new_r - r) < 1e-6) { cx = new_cx; cy = new_cy; r = new_r; break; } cx = new_cx; cy = new_cy; r = new_r; } double startPhi, endPhi; { double dx1 = points.front().x - cx; double dy1 = points.front().y - cy; double dx2 = points.back().x - cx; double dy2 = points.back().y - cy; startPhi = std::atan2(dy1, dx1); endPhi = std::atan2(dy2, dx2); if (startPhi < 0) startPhi += 2 * M_PI; if (endPhi < 0) endPhi += 2 * M_PI; } std::string pointOrder; double first_last_dist = std::sqrt(std::pow(points.front().x - points.back().x, 2) + std::pow(points.front().y - points.back().y, 2)); if (first_last_dist <= maxClosureDist) { startPhi = 0; endPhi = 2 * M_PI; pointOrder = "closed"; } else { double cross = ((points.front().x - cx) * (points.back().y - cy) - (points.front().y - cy) * (points.back().x - cx)); pointOrder = (cross >= 0 ? "positive" : "negative"); } circleParams result; result.centerRow = cy; result.centerCol = cx; result.radius = r; result.startPhi = startPhi; result.endPhi = endPhi; result.pointOrder = pointOrder; return result; } circleParams fitCircleATukey(const std::vector<cv::Point2f>& points, long maxNumPoints, double maxClosureDist, long clippingEndPoints, long iterations, double clippingFactor) { std::vector<cv::Point2f> selectedPoints; if (maxNumPoints > 0 && points.size() > static_cast<size_t>(maxNumPoints)) { size_t step = points.size() / maxNumPoints; for (size_t i = 0; i < points.size(); i += step) selectedPoints.push_back(points[i]); if (selectedPoints.size() > static_cast<size_t>(maxNumPoints)) selectedPoints.resize(maxNumPoints); } else { selectedPoints = points; } size_t n = selectedPoints.size(); size_t startIdx = std::min(static_cast<size_t>(clippingEndPoints), n); size_t endIdx = (n > static_cast<size_t>(clippingEndPoints)) ? n - clippingEndPoints : n; if (endIdx <= startIdx) { startIdx = 0; endIdx = n; } std::vector<cv::Point2f> fitPoints(selectedPoints.begin() + startIdx, selectedPoints.begin() + endIdx); double S_xx = 0, S_xy = 0, S_yy = 0, S_x = 0, S_y = 0, S_1 = 0; double S_x_r = 0, S_y_r = 0, S_r = 0; for (auto pt : fitPoints) { double x = pt.x; double y = pt.y; double r2 = x * x + y * y; S_xx += x * x; S_xy += x * y; S_yy += y * y; S_x += x; S_y += y; S_1 += 1; S_x_r += x * r2; S_y_r += y * r2; S_r += r2; } cv::Mat M = (cv::Mat_<double>(3, 3) << S_xx, S_xy, S_x, S_xy, S_yy, S_y, S_x, S_y, S_1); cv::Mat b = (cv::Mat_<double>(3, 1) << -S_x_r, -S_y_r, -S_r); cv::Mat params; bool solved = cv::solve(M, b, params, cv::DECOMP_SVD); double A = params.at<double>(0); double B = params.at<double>(1); double C = params.at<double>(2); double cx = -A / 2.0; double cy = -B / 2.0; double r = std::sqrt(cx * cx + cy * cy - C); for (long it = 0; it < iterations; ++it) { std::vector<double> residuals; residuals.reserve(fitPoints.size()); for (auto pt : fitPoints) { double dist = std::sqrt((pt.x - cx) * (pt.x - cx) + (pt.y - cy) * (pt.y - cy)); double res = dist - r; residuals.push_back(res); } double sigma = robustSigma(residuals); double delta = clippingFactor * sigma; if (sigma < 1e-8) break; std::vector<double> weights; weights.reserve(fitPoints.size()); for (double res : residuals) { double abs_res = std::fabs(res); double w = (abs_res <= delta) ? std::pow(1.0 - (abs_res / delta) * (abs_res / delta), 2.0) : 0.0; weights.push_back(w); } double Wxx = 0, Wxy = 0, Wyy = 0, Wx = 0, Wy = 0, W1 = 0; double Wx_r = 0, Wy_r = 0, Wr = 0; for (size_t i = 0; i < fitPoints.size(); ++i) { double w = weights[i]; double x = fitPoints[i].x; double y = fitPoints[i].y; double r2 = x * x + y * y; Wxx += w * x * x; Wxy += w * x * y; Wyy += w * y * y; Wx += w * x; Wy += w * y; W1 += w; Wx_r += w * x * r2; Wy_r += w * y * r2; Wr += w * r2; } cv::Mat WM = (cv::Mat_<double>(3, 3) << Wxx, Wxy, Wx, Wxy, Wyy, Wy, Wx, Wy, W1); cv::Mat Wb = (cv::Mat_<double>(3, 1) << -Wx_r, -Wy_r, -Wr); cv::Mat Wparams; bool solvedW = cv::solve(WM, Wb, Wparams, cv::DECOMP_SVD); if (!solvedW) break; A = Wparams.at<double>(0); B = Wparams.at<double>(1); C = Wparams.at<double>(2); double new_cx = -A / 2.0; double new_cy = -B / 2.0; double new_r = std::sqrt(new_cx * new_cx + new_cy * new_cy - C); if (std::fabs(new_cx - cx) < 1e-6 && std::fabs(new_cy - cy) < 1e-6 && std::fabs(new_r - r) < 1e-6) { cx = new_cx; cy = new_cy; r = new_r; break; } cx = new_cx; cy = new_cy; r = new_r; } double startPhi, endPhi; { double dx1 = points.front().x - cx; double dy1 = points.front().y - cy; double dx2 = points.back().x - cx; double dy2 = points.back().y - cy; startPhi = std::atan2(dy1, dx1); endPhi = std::atan2(dy2, dx2); if (startPhi < 0) startPhi += 2 * M_PI; if (endPhi < 0) endPhi += 2 * M_PI; } std::string pointOrder; double first_last_dist = std::sqrt(std::pow(points.front().x - points.back().x, 2) + std::pow(points.front().y - points.back().y, 2)); if (first_last_dist <= maxClosureDist) { startPhi = 0; endPhi = 2 * M_PI; pointOrder = "closed"; } else { double cross = ((points.front().x - cx) * (points.back().y - cy) - (points.front().y - cy) * (points.back().x - cx)); pointOrder = (cross >= 0 ? "positive" : "negative"); } circleParams result; result.centerRow = cy; result.centerCol = cx; result.radius = r; result.startPhi = startPhi; result.endPhi = endPhi; result.pointOrder = pointOrder; return result; } circleParams fitCircleGeometric(const std::vector<cv::Point2f>& points, long maxNumPoints, double maxClosureDist, long clippingEndPoints, long iterations, double clippingFactor) { std::vector<cv::Point2f> selectedPoints; if (maxNumPoints > 0 && points.size() > static_cast<size_t>(maxNumPoints)) { size_t step = points.size() / maxNumPoints; for (size_t i = 0; i < points.size(); i += step) selectedPoints.push_back(points[i]); if (selectedPoints.size() > static_cast<size_t>(maxNumPoints)) selectedPoints.resize(maxNumPoints); } else { selectedPoints = points; } size_t n = selectedPoints.size(); size_t startIdx = std::min(static_cast<size_t>(clippingEndPoints), n); size_t endIdx = (n > static_cast<size_t>(clippingEndPoints)) ? n - clippingEndPoints : n; if (endIdx <= startIdx) { startIdx = 0; endIdx = n; } std::vector<cv::Point2f> fitPoints(selectedPoints.begin() + startIdx, selectedPoints.begin() + endIdx); double S_xx = 0, S_xy = 0, S_yy = 0, S_x = 0, S_y = 0, S_1 = 0; double S_x_r = 0, S_y_r = 0, S_r = 0; for (auto pt : fitPoints) { double x = pt.x; double y = pt.y; double r2 = x * x + y * y; S_xx += x * x; S_xy += x * y; S_yy += y * y; S_x += x; S_y += y; S_1 += 1; S_x_r += x * r2; S_y_r += y * r2; S_r += r2; } cv::Mat M = (cv::Mat_<double>(3, 3) << S_xx, S_xy, S_x, S_xy, S_yy, S_y, S_x, S_y, S_1); cv::Mat b = (cv::Mat_<double>(3, 1) << -S_x_r, -S_y_r, -S_r); cv::Mat params; bool solved = cv::solve(M, b, params, cv::DECOMP_SVD); double A = params.at<double>(0); double B = params.at<double>(1); double C = params.at<double>(2); double cx = -A / 2.0; double cy = -B / 2.0; double r = std::sqrt(cx * cx + cy * cy - C); const int maxIter = 20; const double tol = 1e-6; for (int iter = 0; iter < maxIter; ++iter) { double A11 = 0, A12 = 0, A13 = 0; double A22 = 0, A23 = 0; double A33 = 0; double b1 = 0, b2 = 0, b3 = 0; int validCount = 0; for (auto pt : fitPoints) { double dx = cx - pt.x; double dy = cy - pt.y; double d = std::sqrt(dx * dx + dy * dy); if (d < 1e-8) continue; double f_i = d - r; double J0 = dx / d; double J1 = dy / d; double J2 = -1; A11 += J0 * J0; A12 += J0 * J1; A13 += J0 * J2; A22 += J1 * J1; A23 += J1 * J2; A33 += J2 * J2; b1 += J0 * f_i; b2 += J1 * f_i; b3 += J2 * f_i; validCount++; } if (validCount == 0) break; cv::Mat A_mat = (cv::Mat_<double>(3, 3) << A11, A12, A13, A12, A22, A23, A13, A23, A33); cv::Mat b_mat = (cv::Mat_<double>(3, 1) << b1, b2, b3); cv::Mat delta; bool solvedGN = cv::solve(A_mat, b_mat, delta, cv::DECOMP_SVD); if (!solvedGN) break; double d_cx = delta.at<double>(0); double d_cy = delta.at<double>(1); double d_r = delta.at<double>(2); cx -= d_cx; cy -= d_cy; r -= d_r; if (std::sqrt(d_cx * d_cx + d_cy * d_cy + d_r * d_r) < tol) break; } double startPhi, endPhi; { double dx1 = points.front().x - cx; double dy1 = points.front().y - cy; double dx2 = points.back().x - cx; double dy2 = points.back().y - cy; startPhi = std::atan2(dy1, dx1); endPhi = std::atan2(dy2, dx2); if (startPhi < 0) startPhi += 2 * M_PI; if (endPhi < 0) endPhi += 2 * M_PI; } std::string pointOrder; double first_last_dist = std::sqrt(std::pow(points.front().x - points.back().x, 2) + std::pow(points.front().y - points.back().y, 2)); if (first_last_dist <= maxClosureDist) { startPhi = 0; endPhi = 2 * M_PI; pointOrder = "closed"; } else { double cross = ((points.front().x - cx) * (points.back().y - cy) - (points.front().y - cy) * (points.back().x - cx)); pointOrder = (cross >= 0 ? "positive" : "negative"); } circleParams result; result.centerRow = cy; result.centerCol = cx; result.radius = r; result.startPhi = startPhi; result.endPhi = endPhi; result.pointOrder = pointOrder; return result; } circleParams fitCircleGeoHuber(const std::vector<cv::Point2f>& points, long maxNumPoints, double maxClosureDist, long clippingEndPoints, double clippingFactor) { std::vector<cv::Point2f> selectedPoints; if (maxNumPoints > 0 && points.size() > static_cast<size_t>(maxNumPoints)) { size_t step = points.size() / maxNumPoints; for (size_t i = 0; i < points.size(); i += step) selectedPoints.push_back(points[i]); if (selectedPoints.size() > static_cast<size_t>(maxNumPoints)) selectedPoints.resize(maxNumPoints); } else { selectedPoints = points; } size_t n = selectedPoints.size(); size_t startIdx = std::min(static_cast<size_t>(clippingEndPoints), n); size_t endIdx = (n > static_cast<size_t>(clippingEndPoints)) ? n - clippingEndPoints : n; if (endIdx <= startIdx) { startIdx = 0; endIdx = n; } std::vector<cv::Point2f> fitPoints(selectedPoints.begin() + startIdx, selectedPoints.begin() + endIdx); double S_xx = 0, S_xy = 0, S_yy = 0, S_x = 0, S_y = 0, S_1 = 0; double S_x_r = 0, S_y_r = 0, S_r = 0; for (auto pt : fitPoints) { double x = pt.x; double y = pt.y; double r2 = x * x + y * y; S_xx += x * x; S_xy += x * y; S_yy += y * y; S_x += x; S_y += y; S_1 += 1; S_x_r += x * r2; S_y_r += y * r2; S_r += r2; } cv::Mat M = (cv::Mat_<double>(3, 3) << S_xx, S_xy, S_x, S_xy, S_yy, S_y, S_x, S_y, S_1); cv::Mat b = (cv::Mat_<double>(3, 1) << -S_x_r, -S_y_r, -S_r); cv::Mat params; bool solved = cv::solve(M, b, params, cv::DECOMP_SVD); double A = params.at<double>(0); double B = params.at<double>(1); double C = params.at<double>(2); double cx = -A / 2.0; double cy = -B / 2.0; double r = std::sqrt(cx * cx + cy * cy - C); const int maxIter = 20; const double tol = 1e-6; for (int iter = 0; iter < maxIter; ++iter) { std::vector<double> residuals; residuals.reserve(fitPoints.size()); for (auto pt : fitPoints) { double dx = cx - pt.x; double dy = cy - pt.y; double d = std::sqrt(dx * dx + dy * dy); if (d < 1e-8) continue; double f_val = d - r; residuals.push_back(f_val); } double sigma = robustSigma(residuals); double deltaThreshold = clippingFactor * sigma; if (sigma < 1e-8) break; double A11 = 0, A12 = 0, A13 = 0; double A22 = 0, A23 = 0; double A33 = 0; double b1 = 0, b2 = 0, b3 = 0; int validCount = 0; for (auto pt : fitPoints) { double dx = cx - pt.x; double dy = cy - pt.y; double d = std::sqrt(dx * dx + dy * dy); if (d < 1e-8) continue; double f_val = d - r; double abs_f = std::fabs(f_val); double w = (abs_f <= deltaThreshold) ? 1.0 : (deltaThreshold / abs_f); double J0 = dx / d; double J1 = dy / d; double J2 = -1.0; A11 += w * J0 * J0; A12 += w * J0 * J1; A13 += w * J0 * J2; A22 += w * J1 * J1; A23 += w * J1 * J2; A33 += w * J2 * J2; b1 += w * J0 * f_val; b2 += w * J1 * f_val; b3 += w * J2 * f_val; validCount++; } if (validCount == 0) break; cv::Mat A_mat = (cv::Mat_<double>(3, 3) << A11, A12, A13, A12, A22, A23, A13, A23, A33); cv::Mat b_mat = (cv::Mat_<double>(3, 1) << b1, b2, b3); cv::Mat deltaParams; bool solvedGN = cv::solve(A_mat, b_mat, deltaParams, cv::DECOMP_SVD); if (!solvedGN) break; double d_cx = deltaParams.at<double>(0); double d_cy = deltaParams.at<double>(1); double d_r = deltaParams.at<double>(2); cx -= d_cx; cy -= d_cy; r -= d_r; if (std::sqrt(d_cx * d_cx + d_cy * d_cy + d_r * d_r) < tol) break; } double startPhi, endPhi; { double dx1 = points.front().x - cx; double dy1 = points.front().y - cy; double dx2 = points.back().x - cx; double dy2 = points.back().y - cy; startPhi = std::atan2(dy1, dx1); endPhi = std::atan2(dy2, dx2); if (startPhi < 0) startPhi += 2 * M_PI; if (endPhi < 0) endPhi += 2 * M_PI; } std::string pointOrder; double first_last_dist = std::sqrt(std::pow(points.front().x - points.back().x, 2) + std::pow(points.front().y - points.back().y, 2)); if (first_last_dist <= maxClosureDist) { startPhi = 0; endPhi = 2 * M_PI; pointOrder = "closed"; } else { double cross = ((points.front().x - cx) * (points.back().y - cy) - (points.front().y - cy) * (points.back().x - cx)); pointOrder = (cross >= 0 ? "positive" : "negative"); } circleParams result; result.centerRow = cy; result.centerCol = cx; result.radius = r; result.startPhi = startPhi; result.endPhi = endPhi; result.pointOrder = pointOrder; return result; } circleParams fitCircleGeoTukey(const std::vector<cv::Point2f>& points, long maxNumPoints, double maxClosureDist, long clippingEndPoints, double clippingFactor) { std::vector<cv::Point2f> selectedPoints; if (maxNumPoints > 0 && points.size() > static_cast<size_t>(maxNumPoints)) { size_t step = points.size() / maxNumPoints; for (size_t i = 0; i < points.size(); i += step) selectedPoints.push_back(points[i]); if (selectedPoints.size() > static_cast<size_t>(maxNumPoints)) selectedPoints.resize(maxNumPoints); } else { selectedPoints = points; } size_t n = selectedPoints.size(); size_t startIdx = std::min(static_cast<size_t>(clippingEndPoints), n); size_t endIdx = (n > static_cast<size_t>(clippingEndPoints)) ? n - clippingEndPoints : n; if (endIdx <= startIdx) { startIdx = 0; endIdx = n; } std::vector<cv::Point2f> fitPoints(selectedPoints.begin() + startIdx, selectedPoints.begin() + endIdx); double S_xx = 0, S_xy = 0, S_yy = 0, S_x = 0, S_y = 0, S_1 = 0; double S_x_r = 0, S_y_r = 0, S_r = 0; for (auto pt : fitPoints) { double x = pt.x; double y = pt.y; double r2 = x * x + y * y; S_xx += x * x; S_xy += x * y; S_yy += y * y; S_x += x; S_y += y; S_1 += 1; S_x_r += x * r2; S_y_r += y * r2; S_r += r2; } cv::Mat M = (cv::Mat_<double>(3, 3) << S_xx, S_xy, S_x, S_xy, S_yy, S_y, S_x, S_y, S_1); cv::Mat b = (cv::Mat_<double>(3, 1) << -S_x_r, -S_y_r, -S_r); cv::Mat params; bool solved = cv::solve(M, b, params, cv::DECOMP_SVD); double A = params.at<double>(0); double B = params.at<double>(1); double C = params.at<double>(2); double cx = -A / 2.0; double cy = -B / 2.0; double r = std::sqrt(cx * cx + cy * cy - C); const int maxIter = 20; const double tol = 1e-6; for (int iter = 0; iter < maxIter; ++iter) { std::vector<double> residuals; residuals.reserve(fitPoints.size()); for (auto pt : fitPoints) { double dx = cx - pt.x; double dy = cy - pt.y; double d = std::sqrt(dx * dx + dy * dy); if (d < 1e-8) continue; double f_val = d - r; residuals.push_back(f_val); } double sigma = robustSigma(residuals); double deltaThreshold = clippingFactor * sigma; if (sigma < 1e-8) break; double A11 = 0, A12 = 0, A13 = 0; double A22 = 0, A23 = 0; double A33 = 0; double b1 = 0, b2 = 0, b3 = 0; int validCount = 0; for (auto pt : fitPoints) { double dx = cx - pt.x; double dy = cy - pt.y; double d = std::sqrt(dx * dx + dy * dy); if (d < 1e-8) continue; double f_val = d - r; double abs_f = std::fabs(f_val); double w = (abs_f <= deltaThreshold) ? std::pow(1.0 - (abs_f / deltaThreshold) * (abs_f / deltaThreshold), 2.0) : 0.0; double J0 = dx / d; double J1 = dy / d; double J2 = -1.0; A11 += w * J0 * J0; A12 += w * J0 * J1; A13 += w * J0 * J2; A22 += w * J1 * J1; A23 += w * J1 * J2; A33 += w * J2 * J2; b1 += w * J0 * f_val; b2 += w * J1 * f_val; b3 += w * J2 * f_val; validCount++; } if (validCount == 0) break; cv::Mat A_mat = (cv::Mat_<double>(3, 3) << A11, A12, A13, A12, A22, A23, A13, A23, A33); cv::Mat b_mat = (cv::Mat_<double>(3, 1) << b1, b2, b3); cv::Mat deltaParams; bool solvedGN = cv::solve(A_mat, b_mat, deltaParams, cv::DECOMP_SVD); if (!solvedGN) break; double d_cx = deltaParams.at<double>(0); double d_cy = deltaParams.at<double>(1); double d_r = deltaParams.at<double>(2); cx -= d_cx; cy -= d_cy; r -= d_r; if (std::sqrt(d_cx * d_cx + d_cy * d_cy + d_r * d_r) < tol) break; } double startPhi, endPhi; { double dx1 = points.front().x - cx; double dy1 = points.front().y - cy; double dx2 = points.back().x - cx; double dy2 = points.back().y - cy; startPhi = std::atan2(dy1, dx1); endPhi = std::atan2(dy2, dx2); if (startPhi < 0) startPhi += 2 * M_PI; if (endPhi < 0) endPhi += 2 * M_PI; } std::string pointOrder; double first_last_dist = std::sqrt(std::pow(points.front().x - points.back().x, 2) + std::pow(points.front().y - points.back().y, 2)); if (first_last_dist <= maxClosureDist) { startPhi = 0; endPhi = 2 * M_PI; pointOrder = "closed"; } else { double cross = ((points.front().x - cx) * (points.back().y - cy) - (points.front().y - cy) * (points.back().x - cx)); pointOrder = (cross >= 0 ? "positive" : "negative"); } circleParams result; result.centerRow = cy; result.centerCol = cx; result.radius = r; result.startPhi = startPhi; result.endPhi = endPhi; result.pointOrder = pointOrder; return result; } } // namespace namespace Huawei { const std::set<std::string> allowedModes = {"algebraic", "ahuber", "atukey", "geometric", "geohuber", "geotukey"}; void HXLDCont::HwFitCircleContourXld(const std::string& algorithm, Hlong maxNumPoints, double maxClosureDist, Hlong clippingEndPoints, Hlong iterations, double clippingFactor, HTuple& row, HTuple& column, HTuple& radius, HTuple& startPhi, HTuple& endPhi, HTuple& pointOrder) const { double r, c, rd, sP, eP; std::string pO; this->HwFitCircleContourXld(algorithm, maxNumPoints, maxClosureDist, clippingEndPoints, iterations, clippingFactor, r, c, rd, sP, eP, pO); row = r; column = c; radius = rd; startPhi = sP; endPhi = eP; pointOrder = pO; return; }; void HXLDCont::HwFitCircleContourXld(const std::string& algorithm, Hlong maxNumPoints, double maxClosureDist, Hlong clippingEndPoints, Hlong iterations, double clippingFactor, double& row, double& column, double& radius, double& startPhi, double& endPhi, std::string& pointOrder) const { if (!this->m_objects.size()) { throw LINE_RT_ERROR("object is empty"); } if (!allowedModes.count(algorithm)) { throw LINE_RT_ERROR("non supported algorithm is requested"); } if (!(maxNumPoints == -1 || maxNumPoints >= 8)) { throw LINE_RT_ERROR("maxNumPoints should be >=3 or equal to -1"); } // if (maxClosureDist < 0) { // throw LINE_RT_ERROR("maxClosureDist should be larger or equal zero"); //} // if (this->m_objects.size() - clippingEndPoints * 2 < 3) { // throw LINE_RT_ERROR("not enough valid points for fitting the model"); //} if (clippingEndPoints < 0) { throw LINE_RT_ERROR("clippingEndPoints should be larger or equal zero"); } if (iterations < 0) { throw LINE_RT_ERROR("iterations should be larger or equal zero"); } // if (clippingFactor <= 0) { // throw LINE_RT_ERROR("clippingFactor should be larger than zero"); //} if ((int)this->m_objects.size() - clippingEndPoints * 2 <= 0) throw LINE_RT_ERROR(" Not enough valid points for fitting the model"); HTuple rows, cols; HwGetContourXld(rows, cols); if (rows.Length() < 3) { throw std::runtime_error("Not enough points to fit a circle. Minimum 3 points are required."); } std::vector<cv::Point2f> points; for (int i = clippingEndPoints; i < rows.Length() - clippingEndPoints; ++i) { points.emplace_back((cols[i].D()), (rows[i].D())); } circleParams result; if (algorithm == "algebraic") { result = fitContourAlgebraic(points); } else if (algorithm == "ahuber") { result = fitCircleAHuber(points, maxClosureDist, clippingEndPoints, iterations, clippingFactor); } else if (algorithm == "atukey") { result = fitCircleATukey(points, maxNumPoints, maxClosureDist, clippingEndPoints, iterations, clippingFactor); } else if (algorithm == "geometric") { result = fitCircleGeometric(points, maxNumPoints, maxClosureDist, clippingEndPoints, iterations, clippingFactor); } else if (algorithm == "geohuber") { result = fitCircleGeoHuber(points, maxNumPoints, maxClosureDist, clippingEndPoints, clippingFactor); } else if (algorithm == "geotukey") { result = fitCircleGeoTukey(points, maxNumPoints, maxClosureDist, clippingEndPoints, clippingFactor); } startPhi = result.startPhi; endPhi = result.endPhi; column = result.centerCol; row = result.centerRow; radius = result.radius; pointOrder = result.pointOrder; return; }; void HwFitCircleContourXld(const HXLDCont& contours, const HTuple& algorithm, const HTuple& maxNumPoints, const HTuple& maxClosureDist, const HTuple& clippingEndPoints, const HTuple& iterations, const HTuple& clippingFactor, HTuple& row, HTuple& column, HTuple& radius, HTuple& startPhi, HTuple& endPhi, HTuple& pointOrder) { if (!maxNumPoints.isNumericalOnly() || !maxClosureDist.isNumericalOnly() || !clippingEndPoints.isNumericalOnly() || !iterations.isNumericalOnly() || !clippingFactor.isNumericalOnly()) { throw LINE_RT_ERROR("one of the arguments is NaN"); } if (algorithm.Length() != 1 || maxNumPoints.Length() != 1 || maxClosureDist.Length() != 1 || clippingEndPoints.Length() != 1 || iterations.Length() != 1 || clippingFactor.Length() != 1) { throw LINE_RT_ERROR("one of the arguments is empty"); } contours.HwFitCircleContourXld(algorithm.S(), maxNumPoints.I(), maxClosureDist.D(), clippingEndPoints.I(), iterations.I(), clippingFactor.D(), row, column, radius, startPhi, endPhi, pointOrder); return; }; } // namespace Huawei
Editor is loading...
Leave a Comment