opencv 算法 c++

最小二乘法圆拟合

简介

介绍最小二乘法拟合圆的方法，以及公式的推导过程，将求解圆参数的方程组转化为求解矩阵来求解。最后以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}$$

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109

#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;
}

最小二乘圆拟合.png

References

【1】最小二乘拟合圆公式推导以及vc实现

本文由芒果浩明发布，转载请注明出处。 本文链接：https://blog.mangoeffect.net/opencv/least-square-circle-fit.html

微信公众号