作为全球某机器视觉头部大厂的资深工程师，深感学校和互联网上主要资料局限于计算机视觉领域，能落地工业领域的算法寥寥无几，且相关算法原理语焉不详、乏善可陈，或示例代码缺少关键实现、谬误百出， 故作【庖丁解牛】系列课程，掰开揉碎了讲解机器视觉领域核心算法，并提供完整的高质量代码，力争为从业者提供严格贴近工业应用场景的、高质量的国际一线算法剖析。
本系列课程围绕工业视觉算法原理及实现，具体场景落地应考虑的工程实践化设计要点及经验，笔者将在其他系列课程中讲解。
庖丁解牛本期公开课的主题是 圆拟合。
什么是圆拟合
顾名思义，圆拟合是从给定的点集数据中拟合出一个圆。
定义
通过一堆离散的点，然后根据这些离散点的特点，拟合出一个圆。
特征
- 一堆离散的点，点数大于等于2；
- 离散点的特征，满足几何基元约束；
- 拟合出一个圆，结果一定是个圆。
主流算法
机器视觉（或是计算机视觉）领域，主流的圆拟合算法有：
- 最小二乘法
- 迭代重加权最小二乘法
- 非线性优化法
- 旋转角度法
- 旋转角度最小二乘法
后面两种算法仅用于小角度圆拟合场景，严格意义上讲不属于典型圆拟合范畴，因此本文不做介绍。
最小二乘法（LS）
原理
圆的标准方程为：
(
r
−
a
)
2
+
(
c
−
b
)
2
=
R
2
(r-a)^2+(c-b)^2=R^2
(r−a)2+(c−b)2=R2
展开可得：
2
r
a
+
2
c
b
−
a
2
−
b
2
+
R
2
=
r
2
+
c
2
2ra+2cb-a^2-b^2+R^2=r^2+c^2
2ra+2cb−a2−b2+R2=r2+c2
令
q
=
a
2
+
b
2
−
R
2
q=a^2+b^2-R^2
q=a2+b2−R2，则有：
[
r
1
^
2
+
c
1
^
2
r
2
^
2
+
c
2
^
2
r
n
^
2
+
c
n
^
2
]
=
[
2
r
1
^
2
c
1
^
1
2
r
2
^
2
c
2
^
1
2
r
n
^
2
c
n
^
1
]
[
a
b
q
]
\begin{equation} \left[ \begin{array}{c} \hat{r_{1}}^2+\hat{c_{1}}^2 \\ \hat{r_{2}}^2+\hat{c_{2}}^2 \\ \hat{r_{n}}^2+\hat{c_{n}}^2 \end{array} \right]= \left[ \begin{array}{ccc} \hat{2r_{1}} & \hat{2c_{1}} & 1 \\ \hat{2r_{2}} & \hat{2c_{2}} & 1 \\ \hat{2r_{n}} & \hat{2c_{n}} & 1 \end{array} \right] \left[ \begin{array}{c} a \\ b \\ q \end{array} \right] \end{equation}
r1^2+c1^2r2^2+c2^2rn^2+cn^2
=
2r1^2r2^2rn^2c1^2c2^2cn^111
abq
式中，
(
a
,
b
)
(a,b)
(a,b)是圆心，
a
2
+
b
2
−
q
\sqrt{a^2+b^2-q}
a2+b2−q是圆半径。
误差函数：
e
2
=
∑
i
=
1
n
[
(
r
i
^
−
a
)
2
+
(
c
i
^
−
b
)
2
−
R
2
]
2
e^2=\sum_{i=1}^n[(\hat{r_{i}}-a)^2+(\hat{c_{i}}-b)^2-R^2]^2
e2=i=1∑n[(ri^−a)2+(ci^−b)2−R2]2
定义矩阵A：
[
2
r
1
^
2
c
1
^
1
2
r
2
^
2
c
2
^
1
2
r
n
^
2
c
n
^
1
]
\begin{equation} \left[ \begin{array}{ccc} \hat{2r_{1}} & \hat{2c_{1}} & 1 \\ \hat{2r_{2}} & \hat{2c_{2}} & 1 \\ \hat{2r_{n}} & \hat{2c_{n}} & 1 \end{array} \right] \end{equation}
2r1^2r2^2rn^2c1^2c2^2cn^111
定义矩阵B：
[
r
1
^
2
+
c
1
^
2
r
2
^
2
+
c
2
^
2
r
n
^
2
+
c
n
^
2
]
\begin{equation} \left[ \begin{array}{c} \hat{r_{1}}^2+\hat{c_{1}}^2 \\ \hat{r_{2}}^2+\hat{c_{2}}^2 \\ \hat{r_{n}}^2+\hat{c_{n}}^2 \end{array} \right] \end{equation}
r1^2+c1^2r2^2+c2^2rn^2+cn^2
则根据最小二乘法有：
[
a
b
q
]
=
(
A
T
A
)
−
1
A
T
B
\begin{equation} \left[ \begin{array}{c} a \\ b \\ q \end{array} \right]=(A^TA)^{-1}A^TB \end{equation}
abq
=(ATA)−1ATB
核心代码
double A_matrix[MAX_POINT_NUM] = { 0.0 };
double B_matrix[MAX_POINT_NUM] = { 0.0 };
double x_matrix[3] = { 0.0 };
for (int i = 0; i < nPointArrayCount; i++)
{
A_matrix[i * 3 + 0] = 2 * fFitPointX[i];
A_matrix[i * 3 + 1] = 2 * fFitPointY[i];
A_matrix[i * 3 + 2] = 1;
B_matrix[i] = pow(fFitPointX[i], 2) + pow(fFitPointY[i], 2);
}
int nRet = LeastSquare(A_matrix, B_matrix, x_matrix, nPointArrayCount);
fCenterX = x_matrix[0];
fCenterY = x_matrix[1];
fRadius = sqrt(pow(x_matrix[0], 2) + pow(x_matrix[1], 2) + x_matrix[2]);
迭代重加权最小二乘法（IRLS）
原理
最小平方圆拟合对于离群值不够鲁棒，因此需要引入权重并使用它来减小离群值的影响。先采用正常的最小二乘法拟合一个圆，然后用各拟合点到此圆的距离来计算在后续的迭代中将使用的各点所对应的权重。
误差函数：
e
2
=
∑
i
=
1
n
w
i
[
(
r
i
^
−
a
)
2
+
(
c
i
^
−
b
)
2
−
R
2
]
2
e^2=\sum_{i=1}^nw_{i}[(\hat{r_{i}}-a)^2+(\hat{c_{i}}-b)^2-R^2]^2
e2=i=1∑nwi[(ri^−a)2+(ci^−b)2−R2]2
式中，
(
a
,
b
)
(a,b)
(a,b)是圆心，
R
R
R是圆半径，
w
i
w_{i}
wi是权重函数。
第一种权重函数为Huber权重函数：
w
(
δ
)
=
{
1
∣
δ
∣
≤
τ
τ
/
∣
δ
∣
∣
δ
∣
>
τ
\begin{equation} w(\delta)=\left\{ \begin{aligned} 1 && |\delta| \leq \tau \\ \tau/|\delta| && |\delta| > \tau \\ \end{aligned} \right. \end{equation}
w(δ)={1τ/∣δ∣∣δ∣≤τ∣δ∣>τ
式中，
δ
\delta
δ为拟合点到圆周的距离，
τ
\tau
τ为削波因数。所有距离
≤
τ
\leq\tau
≤τ的点对应的权重都是1，距离
>
τ
>\tau
>τ的点将分配更小的权重，从而减小离群点对结果的干扰。
第二种权重函数为Tukey权重函数：
w
(
δ
)
=
{
[
1
−
(
δ
/
τ
)
2
]
2
∣
δ
∣
≤
τ
0
∣
δ
∣
>
τ
\begin{equation} w(\delta)=\left\{ \begin{aligned} [1-(\delta/\tau)^2]^2 && |\delta| \leq \tau \\ 0 && |\delta| > \tau \\ \end{aligned} \right. \end{equation}
w(δ)={[1−(δ/τ)2]20∣δ∣≤τ∣δ∣>τ
式中，
δ
\delta
δ为拟合点到圆周的距离，
τ
\tau
τ为削波因数。所有距离
≤
τ
\leq\tau
≤τ的点对应的权重在1~0之间平滑变化，距离
>
τ
>\tau
>τ的点将被完全忽略，不参与拟合，从而减小离群点对结果的干扰。
削波因数
τ
\tau
τ的计算公式为：
σ
δ
=
m
e
d
i
a
n
∣
δ
i
∣
0.6745
\sigma_{\delta}=\frac{median|\delta_{i}|}{0.6745}
σδ=0.6745median∣δi∣
τ
=
2
σ
δ
\tau=2\sigma_{\delta}
τ=2σδ
对于符合正态分布的离群值，鲁棒标准偏差
σ
δ
\sigma_{\delta}
σδ的分子为所有拟合点到圆心距离的中值，分母为基于正态分布计算的标准偏差。
核心代码
// TODO Code 此处代码不做展示，留作读者自行实现
非线性优化法
原理
将所有拟合点到待拟合圆的平方距离进行连加求和，然后使求得的总和最小化。
误差函数：
e
2
=
∑
i
=
1
n
[
(
r
i
^
−
a
)
2
+
(
c
i
^
−
b
)
2
−
R
]
2
e^2=\sum_{i=1}^n[\sqrt{(\hat{r_{i}}-a)^2+(\hat{c_{i}}-b)^2}-R]^2
e2=i=1∑n[(ri^−a)2+(ci^−b)2−R]2
式中，
(
a
,
b
)
(a,b)
(a,b)是圆心，
R
R
R是圆半径。与之前的方法不同，这是一个非线性优化的问题，只能采用非线性优化技术来求解。
常见的非线性优化算法有：
- 梯度下降法
- 牛顿法
- 拟牛顿法
- 高斯牛顿法
- 列文伯格-马夸尔特法
其中，高斯牛顿法（Gauss–Newton algorithm）在牛顿法的基础上做了改进。牛顿法中的H矩阵需要计算目标函数的二阶偏导，计算量巨大，高斯牛顿法采用G矩阵替代H矩阵，大大减小了计算量。
因此，选择高斯牛顿法实现圆拟合的非线性优化。
核心代码
// 开始Gauss-Newton迭代
int iterations = 20; // 迭代次数
double cost = 0, lastCost = 0; // 本次迭代的cost和上一次迭代的cost
// CostFunction = Σ(AXi - Bi)² A=(a,b,c) Xi=(xi,yi,1)^T Bi = -xi² - yi²
for (int iter = 0; iter < iterations; iter++) {
Matrix3d H = Matrix3d::Zero(); // Hessian = J^T W^{-1} J in Gauss-Newton
Vector3d b = Vector3d::Zero(); // bias
cost = 0;
for (int i = 0; i < N; i++) {
double xi = x_data[i], yi = y_data[i]; // 第i个数据点
double error = xi * xi + yi * yi + (ae * xi + be * yi + ce);
Vector3d J; // 雅可比矩阵
J[0] = xi; // de/da
J[1] = yi; // de/db
J[2] = 1; // de/dc
H += J * J.transpose();
b += -error * J;
cost += error * error;
}
// 求解线性方程 Hx=b
Vector3d dx = H.ldlt().solve(b);
if (isnan(dx[0])) {
cout << "result is nan!" << endl;
break;
}
if (iter > 0 && cost >= lastCost) {
cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
break;
}
ae += dx[0];
be += dx[1];
ce += dx[2];
lastCost = cost;
cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
"\t\testimated params: " << ae << "," << be << "," << ce << endl;
}
double centerX = ae / -2;
double centerY = be / -2;
double radius = sqrt(ae * ae / 4 + be * be / 4 - ce);
运行结果对比
采集多组独立的数据，分别采用笔者提出的算法、VisionMaster（国内最领先的算法平台，后面统称为VM）进行圆拟合，从圆拟合结果（含拟合误差）、算法运行耗时两个方面评估算法的准确性和效率性。
VM圆拟合结果：
本文圆拟合结果：
算法运行效率对比：
可以看出，算法准确性方面，笔者提出的算法足以媲美VM；算法效率性方面，在拟合点数为200个点时，笔者提出的算法运行耗时稳定在0.23ms内，亦可媲美VM。
本期课程就到此结束了，下期课程我们不见不散。
如觉得本文内容不错，请记得给小编点赞、收藏、分享一键三连，您的支持是笔者更新的不懈动力。
想要算法源码工程的读者朋友可在评论区留言或私信小编。
2113
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
