欢迎关注更多精彩
关注我，学习常用算法与数据结构，一题多解，降维打击。
本期话题：最小二乘2D圆拟合
相关背景资料
点击前往
2D圆拟合输入和输出要求
输入
- 8到50个点，全部采样自圆上，z轴坐标都为0。
- 每个点3个坐标，坐标精确到小数点后面20位。
- 坐标单位是mm, 范围[-500mm, 500mm]。
tips
输入虽然是严格来自2D圆，但是由于浮点表示，记录的值和真实值始终是有微小的误差，点云到拟合圆的距离不可能完全为0。
输出
- 1点X0表示 圆心，用三个坐标表示。
- 圆半径r。
精度要求
- X0点到标准圆心距离不能超过0.0001mm。
- r与标准半径的差不能超过0.0001mm。
2D圆优化标函数
根据论文，圆拟合转化成数学表示如下：
圆参数化表示
- 圆心为1点X0 = (x0, y0)。
- 半径r。
圆方程 ( x − x 0 ) 2 + ( y − y 0 ) 2 = r 2 圆方程 (x-x_0)^2+(y-y_0)^2=r^2 圆方程(x−x0)2+(y−y0)2=r2
点到2D圆距离
第i个点 pi(xi, yi, zi)。
根据点乘得到距离
d i = ∥ ( p i − X 0 ) ∥ − r d_i =\left \| (p_i-X_0) \right \|-r di=∥(pi−X0)∥−r
展开一下：
d i = r i − r d_i = r_i -r di=ri−r
r i = ( x i − x 0 ) 2 + ( y i − y 0 ) 2 r_i = \sqrt{(x_i-x_0)^2 + (y_i-y_0)^2} ri=(xi−x0)2+(yi−y0)2
最小二乘优化能量方程
能量方程 E = f ( X 0 , r ) = ∑ 1 n d i 2 E=f(X0, r)=\displaystyle \sum _1^n {d_i^2} E=f(X0,r)=1∑ndi2
上式是一个4元二次函数中，X0, r是未知量，拟合2D圆的过程也可以理解为优化X0, r使得方程E最小。
可以对上述方程求导，使得导数等于0取得最值。但是求导后会变成一个比较复杂的方程组，不好解。可以使用高斯牛顿迭代法来求解。
高斯牛顿迭代法
用于解非线性最小二乘问题。同时，高斯牛顿法需要比较可靠的初值，所以寻找目标函数的初值也是一个比较关键的步骤。
基本原理
设 a = ( a 0 , a 1 , . . . , a n ) 是待求解向量， a ^ 是初始给定值， a = a ^ + Δ a , Δ a 是我们每次迭代后移动的量 设 a=(a_0, a_1,...,a_n)是待求解向量，\widehat {a} 是初始给定值，a = \widehat {a} +\Delta a, \Delta a 是我们每次迭代后移动的量 设a=(a0,a1,...,an)是待求解向量，a 是初始给定值，a=a +Δa,Δa是我们每次迭代后移动的量
定义距离函数为 F ( x , a ) , d i = F ( x i , a ) , 进行泰勒 1 阶展开， F ( x , a ) ＝ F ( x , a ^ ) + ∂ F ∂ a ^ Δ a = F ( x , a ^ ) + J Δ a 定义距离函数为 F(x, a), d_i = F(x_i, a), 进行泰勒1阶展开， F(x, a) ＝ F(x, \widehat a) + \frac {\partial F}{\partial \widehat a}\Delta a = F(x, \widehat a) + J\Delta a 定义距离函数为F(x,a),di=F(xi,a),进行泰勒1阶展开，F(x,a)＝F(x,a )+∂a ∂FΔa=F(x,a )+JΔa
每次迭代，其实就是希望通过调整 Δ a 使得 J Δ a ＝ − F ( x , a ^ ) 每次迭代，其实就是希望通过调整\Delta a 使得 J\Delta a ＝ -F(x, \widehat a) 每次迭代，其实就是希望通过调整Δa使得JΔa＝−F(x,a )
J = [ ∂ F ( x 0 , a ) ∂ a 0 ∂ F ( x 0 , a ) ∂ a 1 . . . ∂ F ( x 0 , a ) ∂ a n ∂ F ( x 1 , a ) ∂ a 0 ∂ F ( x 1 , a ) ∂ a 1 . . . ∂ F ( x 1 , a ) ∂ a n . . . . . . . . . . . . ∂ F ( x n , a ) ∂ a 0 ∂ F ( x n , a ) ∂ a 1 . . . ∂ F ( x n , a ) ∂ a n ] J = \begin {bmatrix} \frac {\partial F(x_0, a)} {\partial a_0} & \frac {\partial F(x_0, a)} {\partial a_1} & ...& \frac {\partial F(x_0, a)} {\partial a_n} \\ \\ \frac {\partial F(x_1, a)} {\partial a_0} & \frac {\partial F(x_1, a)} {\partial a_1} & ...& \frac {\partial F(x_1, a)} {\partial a_n} \\\\ ... & ... & ...& ... \\ \\ \frac {\partial F(x_n, a)} {\partial a_0} & \frac {\partial F(x_n, a)} {\partial a_1} & ...& \frac {\partial F(x_n, a)} {\partial a_n} \end {bmatrix} J= ∂a0∂F(x0,a)∂a0∂F(x1,a)...∂a0∂F(xn,a)∂a1∂F(x0,a)∂a1∂F(x1,a)...∂a1∂F(xn,a)............∂an∂F(x0,a)∂an∂F(x1,a)...∂an∂F(xn,a)
F ( x , a ^ ) = [ d 1 d 2 . . . d m ] F(x, \widehat a) = \begin {bmatrix} d_1 \\ d_2 \\... \\ d_m \end {bmatrix} F(x,a )= d1d2...dm
J Δ a = − F ( x , a ^ ) , 解出 Δ a , 更新 a = a ^ + Δ a , 持续迭代直到 Δ a 足够小 J\Delta a = -F(x,\widehat a), 解出\Delta a ,更新 a = \widehat {a} +\Delta a, 持续迭代直到\Delta a足够小 JΔa=−F(x,a ),解出Δa,更新a=a +Δa,持续迭代直到Δa足够小
算法描述
Ji, di的计算。
对3个未知数求导结果如下：
∂ d i ∂ x 0 = 1 2 ( x i − x 0 ) 2 + ( y i − y 0 ) 2 ⋅ ( x i − x 0 ) ⋅ − 1 = − ( x i − x 0 ) / r i \frac {\partial d_i} {\partial x_0}=\frac {1} {2 \sqrt{(x_i-x_0)^2 + (y_i-y_0)^2}}\cdot(x_i-x_0)\cdot -1 = -(x_i-x_0)/r_i ∂x0∂di=2(xi−x0)2+(yi−y0)21⋅(xi−x0)⋅−1=−(xi−x0)/ri
∂ d i ∂ y 0 = 1 2 ( x i − x 0 ) 2 + ( y i − y 0 ) 2 ⋅ ( y i − y 0 ) ⋅ − 1 = − ( y i − y 0 ) / r i \frac {\partial d_i} {\partial y_0}=\frac {1} {2 \sqrt{(x_i-x_0)^2 + (y_i-y_0)^2}}\cdot(y_i-y_0)\cdot -1 = -(y_i-y_0)/r_i ∂y0∂di=2(xi−x0)2+(yi−y0)21⋅(yi−y0)⋅−1=−(yi−y0)/ri
∂ d i ∂ r = − 1 \frac {\partial d_i} {\partial r}=-1 ∂r∂di=−1
-
确定圆初值
-
根据上述公式构建 J ⋅ ( [ p x 0 p y 0 p r ] ) = − D = − [ d 1 d 2 . . . d n ] J \cdot \left(\begin {bmatrix}p_{x_0} \\ p_{y_0}\\p_{r} \end {bmatrix} \right)=-D=-\begin {bmatrix}d_1 \\ d_2\\...\\d_n \end {bmatrix} J⋅ px0py0pr =−D=− d1d2...dn
-
求解 Δ p \Delta p Δp
-
更新解
x 0 = x 0 + p x 0 y 0 = y 0 + p y 0 r = r + p r \begin {array}{c} x_0 = x_0+p_{x_0} \\y_0 = y_0+p_{y_0} \\ r=r+p_r \end {array} x0=x0+px0y0=y0+py0r=r+pr -
重复2直到收敛
初值确定
先将圆建立线性的能量方程。
可以作一个近似转换，把半径先平方再相减。
F = ∑ i = 1 n f i 2 F=\displaystyle \sum_{i=1}^{n} f_i^2 F=i=1∑nfi2
f i = r i 2 − r 2 f_i = r_i^2-r^2 fi=ri2−r2
展开后
f i = ( x i − x 0 ) 2 + ( y i − y 0 ) 2 − r 2 = − 2 x i x 0 − 2 y i y 0 + ( x 0 2 + y 0 2 − r 2 ) + ( x i 2 + y i 2 ) f_i = (x_i-x_0)^2 + (y_i-y_0)^2-r^2 \\ = -2x_ix_0-2y_iy_0 + (x_0^2+y_0^2-r^2)+(x_i^2+y_i^2) fi=(xi−x0)2+(yi−y0)2−r2=−2xix0−2yiy0+(x02+y02−r2)+(xi2+yi2)
我们希望fi都为0。
可以令 ρ = x 0 2 + y 0 2 − r 2 可以令\rho=x_0^2+y_0^2-r^2 可以令ρ=x02+y02−r2
建立方程组
A [ x 0 y 0 ρ ] = B A i = ( 2 x i , 2 y i , − 1 ) , b i = x i 2 + y i 2 解得上述方程后 r = x 0 2 + y 0 2 − ρ A \left [\begin {array}{c} x_0\\y_0\\\rho \end{array}\right ]=B \\A_i=(2x_i, 2y_i, -1), b_i=x_i^2+y_i^2 \\解得上述方程后 r=\sqrt{x_0^2+y_0^2-\rho} A x0y0ρ =BAi=(2xi,2yi,−1),bi=xi2+yi2解得上述方程后r=x02+y02−ρ
代码实现
代码链接：https://gitcode.com/chenbb1989/3DAlgorithm/tree/master/CBB3DAlgorithm/Fitting
#include "FittingCircle2D.h"
#include <Eigen/Dense>
namespace Gauss {
double F(Fitting::Circle2D circle, const Point& p)
{
double ri = Eigen::Vector2d(p.x() - circle.center.x(), p.y() - circle.center.y()).norm();
return ri-circle.r;
}
double GetError(Fitting::Circle2D circle, const std::vector<Eigen::Vector3d>& points)
{
double err = 0;
for (auto& p : points) {
double d = F(circle, p);
err += d * d;
}
err /= points.size();
return err;
}
Fitting::Matrix FittingCircle2D::Jacobi(const std::vector<Eigen::Vector3d>& points)
{
Fitting::Matrix J(points.size(), 3);
for (int i = 0; i < points.size(); ++i) {
auto& p = points[i];
double ri = (Eigen::Vector2d(p.x(), p.y()) - circle.center).norm();
J(i, 0) = -(p.x()-circle.center.x())/ri;
J(i, 1) = -(p.y() - circle.center.y()) / ri;
J(i, 2) = -1;
}
return J;
}
void FittingCircle2D::afterHook(const Eigen::VectorXd& xp)
{
circle.center += Eigen::Vector2d(xp(0), xp(1));
circle.r = xp(2);
}
Eigen::VectorXd FittingCircle2D::getDArray(const std::vector<Eigen::Vector3d>& points)
{
Eigen::VectorXd D(points.size());
for (int i = 0; i < points.size(); ++i)D(i) =F(points[i]);
return D;
}
bool FittingCircle2D::GetInitFit(const std::vector<Eigen::Vector3d>& points)
{
if (points.size() < 3)return false;
Fitting::Matrix A(points.size(), 3);
Eigen::VectorXd B(points.size());
for (int i = 0; i < points.size(); ++i) {
A(i, 0) = 2 * points[i].x();
A(i, 1) = 2 * points[i].y();
A(i, 2) = -1;
B(i) = Eigen::Vector2d(points[i].x(), points[i].y()).squaredNorm();
}
Eigen::VectorXd xp;
// 求解 Axp = B https://blog.csdn.net/ABC_ORANGE/article/details/104489257/
xp = A.colPivHouseholderQr().solve(B);
circle.center.x() = xp(0);
circle.center.y() = xp(1);
circle.r = sqrt(xp(0) * xp(0) + xp(1) * xp(1) - xp(2));
return true;
}
double FittingCircle2D::F(const Eigen::Vector3d& p)
{
return Gauss::F(circle, p);
}
double FittingCircle2D::GetError(const std::vector<Eigen::Vector3d>& points)
{
return Gauss::GetError(circle, points);
}
void FittingCircle2D::Copy(void* ele)
{
memcpy(ele, &circle, sizeof(Fitting::Circle2D));
}
}
测试结果
PS D:\selfad\3DAlgorithm_build\CBB3DAlgorithm\FittingTest\bin> .\FittingRun.exe FittingCircle2DTest
2.7131524091739308875e-25
1.2353000000002221093
22.332233299999675324
12222.234300000000076
circle2d test pass
本人码农，希望通过自己的分享，让大家更容易学懂计算机知识。创作不易，帮忙点击公众号的链接。
