简介
介绍最小二乘法拟合圆的方法，以及公式的推导过程，将求解圆参数的方程组转化为求解矩阵来求解。最后以C++编码实现基于最小二乘法的圆拟合算法。
推导
圆曲线公式
$$R^2 = (x - A)^2 + (y - B)^2$$
$$=> R^2 = x^2 - 2Ax + A^2 + y^2 - 2Bx + B^2$$
令
$$a = -2A$$
$$b = -2B$$
$$c = A^2 + B^2 - R^2$$
则可以得到圆曲线方程的另一个表达形式
$$x^2 + y^2 + ax + by+ c =0$$
如果求出参数$a,b,c$，就确定了圆。圆心半径参数也可以通过以下公式求出
$$A = -\frac{a}{2}$$
$$B = -\frac{b}{2}$$
$$R = \frac{\sqrt{a^2 + b^2 -4c}}{2}$$
现有样本集$(X_i,Y_i) i \epsilon(1,2,3...N)$,点到圆心距离为$d_i$,根据最小二乘法限定误差点到圆心距离减去半径，那么目标函数应为
$$S_{\epsilon^2} = \sum_{i=1}^n(d_i - R)^2 = \sum_{i=1}^n (\sqrt{(X_i - A)^2 + (Y_i - B)^2} - \frac{\sqrt{a^2 + b^2 -4c}}{2})^2$$
而$A = -\frac{a}{2}, B = -\frac{b}{2}$, 代入上式可得
$$S_{\epsilon^2} = \sum_{i=1}^n (\sqrt{X_i^2 + Y_i^2 + aX_i + bY_i + \frac{a^2 + b^2}{4}} - \frac{\sqrt{a^2 + b^2 -4c}}{2})^2 $$
到这一步按理来说应该是求$S_{\epsilon^2}$对$a,b,c$的偏导数，在偏导数为0时可以求得$S_{\epsilon^2}$的极值，上式求偏导涉及到开根号，推导过程极其复杂，而且还没有解析解。所以转而采用近似的目标函数来求解
$$S_{\epsilon^2} = \sum_{i=1}^n(d_i^2 - R^2)^2$$
$$= \sum_{i=1}^n((X_i- A)^2 + (Y_i - B)^2 - \frac{a^2 + b^2 -4c}{4})^2$$
$$= \sum_{i=1}^n(X_i^2 + Y_i^2 + aX_i + bY_i +c)^2$$
下一步求偏导，令偏导为0求取目标函数极值
$$\frac{\partial S_{\epsilon^2}}{\partial a} = \sum_{i=1}^n 2(X_i^2 + Y_i^2 + aX_i + bY_i +c)X_i =0$$
$$\frac{\partial S_{\epsilon^2}}{\partial b} = \sum_{i=1}^n 2(X_i^2 + Y_i^2 + aX_i + bY_i +c)Y_i =0$$
$$\frac{\partial S_{\epsilon^2}}{\partial c} = \sum_{i=1}^n 2(X_i^2 + Y_i^2 + aX_i + bY_i +c) =0$$
$$=>$$
$$\frac{\partial S_{\epsilon^2}}{\partial a} = \sum_{i=1}^n (X_i^2 + Y_i^2 + aX_i + bY_i +c)X_i =0$$
$$\frac{\partial S_{\epsilon^2}}{\partial b} = \sum_{i=1}^n(X_i^2 + Y_i^2 + aX_i + bY_i +c)Y_i =0$$
$$\frac{\partial S_{\epsilon^2}}{\partial c} = \sum_{i=1}^n(X_i^2 + Y_i^2 + aX_i + bY_i +c) =0$$
$$=>$$
$$\frac{\partial S_{\epsilon^2}}{\partial a} = \sum_{i=1}^n (X_i^3 + Y_i^2X_i + aX_i^2 + bX_iY_i +cX_i) =0$$
$$\frac{\partial S_{\epsilon^2}}{\partial b} = \sum_{i=1}^n (X_i^2Y_i + Y_i^3 + aX_iY_i + bY_i^2 +cY_i) =0$$
$$\frac{\partial S_{\epsilon^2}}{\partial c} = \sum_{i=1}^n (X_i^2 + Y_i^2 + aX_i + bY_i +c) =0$$
可得方程组
$$ \sum_{i=1}^n (X_i^3 + Y_i^2X_i) + (\sum_{i=1}^nX_i^2)a + (\sum_{i=1}^nX_iY_i)b + (\sum_{i=1}^nX_i)c =0$$
$$ \sum_{i=1}^n (X_i^2Y_i + Y_i^3) + (\sum_{i=1}^nX_iY_i)a + (\sum_{i=1}^nY_i^2)b +(\sum_{i=1}^nY_i)c =0$$
$$ \sum_{i=1}^n (X_i^2 + Y_i^2) + (\sum_{i=1}^nX_i)a + (\sum_{i=1}^nY_i)b + nc =0$$
即
$$ (\sum_{i=1}^nX_i^2)a + (\sum_{i=1}^nX_iY_i)b + (\sum_{i=1}^nX_i)c = -\sum_{i=1}^n (X_i^3 + Y_i^2X_i) $$
$$ (\sum_{i=1}^nX_iY_i)a + (\sum_{i=1}^nY_i^2)b +(\sum_{i=1}^nY_i)c =-\sum_{i=1}^n (X_i^2Y_i + Y_i^3) $$
$$ (\sum_{i=1}^nX_i)a + (\sum_{i=1}^nY_i)b + nc = -\sum_{i=1}^n (X_i^2 + Y_i^2)$$
用矩阵来表示
$$\begin{bmatrix}\sum_{i=1}^nX_i^2 && \sum_{i=1}^nX_iY_i && \sum_{i=1}^nX_i \\\sum_{i=1}^nX_iY_i &&\sum_{i=1}^nY_i^2 &&\sum_{i=1}^nY_i \\\sum_{i=1}^nX_i &&\sum_{i=1}^nY_i &&n\end{bmatrix} \begin{bmatrix} a \\b \\c\end{bmatrix} = \begin{bmatrix} -\sum_{i=1}^n (X_i^3 + Y_i^2X_i) \\-\sum_{i=1}^n (X_i^2Y_i + Y_i^3) \\-\sum_{i=1}^n (X_i^2 + Y_i^2) \end{bmatrix}$$
通过解矩阵运算即可求出$a,b,c$。
倘若给点集的每个点加上权重，方程组则变为
$$ (\sum_{i=1}^nX_i^2w_i)a + (\sum_{i=1}^nX_iY_iw_i)b + (\sum_{i=1}^nX_iw_i)c = -\sum_{i=1}^n (X_i^3 + Y_i^2X_i)w_i $$
$$ (\sum_{i=1}^nX_iY_iw_i)a + (\sum_{i=1}^nY_i^2w_i)b +(\sum_{i=1}^nY_iw_i)c =-\sum_{i=1}^n (X_i^2Y_i + Y_i^3)w_i $$
$$ (\sum_{i=1}^nX_iw_i)a + (\sum_{i=1}^nY_iw_i)b + (\sum_{i=1}^nw_i)c = -\sum_{i=1}^n (X_i^2 + Y_i^2)w_i$$
用矩阵来表示
$$\begin{bmatrix}\sum_{i=1}^nX_i^2w_i && \sum_{i=1}^nX_iY_iw_i && \sum_{i=1}^nX_iw_i \\\sum_{i=1}^nX_iY_iw_i &&\sum_{i=1}^nY_i^2w_i &&\sum_{i=1}^nY_iw_i \\\sum_{i=1}^nX_iw_i &&\sum_{i=1}^nY_iw_i &&\sum_{i=1}^nw_i\end{bmatrix} \begin{bmatrix} a \\b \\c\end{bmatrix} = \begin{bmatrix} -\sum_{i=1}^n (X_i^3 + Y_i^2X_i)w_i \\-\sum_{i=1}^n (X_i^2Y_i + Y_i^3)w_i \\-\sum_{i=1}^n (X_i^2 + Y_i^2)w_i \end{bmatrix}$$
编码实现
思路：
（1）根据样本集$(X_i,Y_i) i \epsilon(1,2,3...N)$以及对应的权重系数求出
$$\begin{bmatrix}\sum_{i=1}^nX_i^2w_i && \sum_{i=1}^nX_iY_iw_i && \sum_{i=1}^nX_iw_i \\\sum_{i=1}^nX_iY_iw_i &&\sum_{i=1}^nY_i^2w_i &&\sum_{i=1}^nY_iw_i \\\sum_{i=1}^nX_iw_i &&\sum_{i=1}^nY_iw_i &&\sum_{i=1}^nw_i\end{bmatrix} $$
以及
$$\begin{bmatrix} -\sum_{i=1}^n (X_i^3 + Y_i^2X_i)w_i \\-\sum_{i=1}^n (X_i^2Y_i + Y_i^3)w_i \\-\sum_{i=1}^n (X_i^2 + Y_i^2)w_i \end{bmatrix}$$
两个矩阵
（2）使用OpenCV函数cv::solve()求解
$$\begin{bmatrix} a \\b \\c\end{bmatrix}$$
#include <iostream>
#include <cassert>
#include<opencv2/opencv.hpp>
void CirleFit(const std::vector<cv::Point2d>& points, const std::vector<double>& weights, cv::Point2d& circleCenter, double& radius)
{
//检查输入参数 | Check input parameters
assert(!points.empty() && points.size() == weights.size());
//构造矩阵 | Construct mat
double XiSum = 0;
double Xi2Sum = 0;
double Xi3Sum = 0;
double YiSum = 0;
double Yi2Sum = 0;
double Yi3Sum = 0;
double XiYiSum = 0;
double Xi2YiSum = 0;
double XiYi2Sum = 0;
double WiSum = 0;
for (size_t i = 0; i < points.size(); i++)
{
XiSum += points.at(i).x * weights.at(i);
Xi2Sum += points.at(i).x * points.at(i).x * weights.at(i);
Xi3Sum += points.at(i).x * points.at(i).x * points.at(i).x * weights.at(i);
YiSum += points.at(i).y * weights.at(i);
Yi2Sum += points.at(i).y * points.at(i).y * weights.at(i);
Yi3Sum += points.at(i).y * points.at(i).y * points.at(i).y * weights.at(i);
XiYiSum += points.at(i).x * points.at(i).y * weights.at(i);
Xi2YiSum += points.at(i).x * points.at(i).x * points.at(i).y * weights.at(i);
XiYi2Sum += points.at(i).x * points.at(i).y * points.at(i).y * weights.at(i);
WiSum += weights.at(i);
}
const int N = 3;
cv::Mat A = cv::Mat::zeros(N, N, CV_64FC1);
cv::Mat B = cv::Mat::zeros(N, 1, CV_64FC1);
A.at<double>(0, 0) = Xi2Sum;
A.at<double>(0, 1) = XiYiSum;
A.at<double>(0, 2) = XiSum;
A.at<double>(1, 0) = XiYiSum;
A.at<double>(1, 1) = Yi2Sum;
A.at<double>(1, 2) = YiSum;
A.at<double>(2, 0) = XiSum;
A.at<double>(2, 1) = YiSum;
A.at<double>(2, 2) = WiSum;
B.at<double>(0, 0) = -(Xi3Sum + XiYi2Sum);
B.at<double>(1, 0) = -(Xi2YiSum + Yi3Sum);
B.at<double>(2, 0) = -(Xi2Sum + Yi2Sum);
//解矩阵 | Solve
//求解A*X = B | Solve the A*X = B
cv::Mat X;
cv::solve(A, B, X, cv::DECOMP_LU);
double a = X.at<double>(0, 0);
double b = X.at<double>(1, 0);
double c = X.at<double>(2, 0);
//计算圆心和半径 | Calculate center and radius.
circleCenter.x = -0.5 * a;
circleCenter.y = -0.5 * b;
radius = 0.5 * std::sqrt(a * a + b * b - 4 * c);
}
int main()
{
cv::Mat img = cv::imread("circle.png");
cv::Mat dstImage = img.clone();
cv::cvtColor(img, img, cv::COLOR_BGR2GRAY); //转为灰度图
cv::threshold(img, img, 100, 255, cv::THRESH_BINARY);//图像二值化，value>threshold（即100）?255:0
if (!img.empty())
{
std::vector<std::vector<cv::Point>>contours;
std::vector<cv::Vec4i>hierarchy;
findContours(img, contours, hierarchy, cv::RETR_LIST,cv::CHAIN_APPROX_NONE);
cv::Scalar color(0, 0, 255);
//drawContours(dstImage, contours, 0, color, 1, 8, hierarchy);
std::vector<cv::Point2d> pts(contours[0].begin(), contours[0].end());
std::vector<double> w(pts.size(), 1);
cv::Point2d center;
double radius = 0;
clock_t ts = clock();
CirleFit(pts, w, center, radius);
clock_t te = clock();
std::cout << "Circle fit cost time: " << static_cast<double>(te - ts) <<"ms"<< std::endl;
cv::circle(dstImage, center, radius, color);
cv::imshow("Circle Fit", dstImage);
cv::waitKey(0);
}
return 0;
}
References
本文由芒果浩明发布，转载请注明出处。
本文链接：https://mangoroom.cn/opencv/least-square-circle-fit.html
