Untitled
unknown
c_cpp
a month 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