最小二乘2D圆拟合(高斯牛顿法)

欢迎关注更多精彩
关注我，学习常用算法与数据结构，一题多解，降维打击。

本期话题：最小二乘2D圆拟合

相关背景资料
点击前往

2D圆拟合输入和输出要求

输入

1. 8到50个点，全部采样自圆上，z轴坐标都为0。
2. 每个点3个坐标，坐标精确到小数点后面20位。
3. 坐标单位是mm, 范围[-500mm, 500mm]。

tips
输入虽然是严格来自2D圆，但是由于浮点表示，记录的值和真实值始终是有微小的误差，点云到拟合圆的距离不可能完全为0。

输出

1. 1点X0表示 圆心，用三个坐标表示。
2. 圆半径r。

精度要求

1. X0点到标准圆心距离不能超过0.0001mm。
2. r与标准半径的差不能超过0.0001mm。

2D圆优化标函数

根据论文，圆拟合转化成数学表示如下：

圆参数化表示

1. 圆心为1点X0 = (x0, y0)。
2. 半径r。

$圆方程(x?x_0{)}^2+(y?y_0{)}^2=r^2$

点到2D圆距离

第i个点 pi(xi, yi, zi)。

根据点乘得到距离

$d_i=\Vert (p_i?X_0)\Vert ?r$

展开一下：

$d_i=r_i?r$

$r_i=\sqrt{(x_i?x_0{)}^2+(y_i?y_0{)}^2}$

最小二乘优化能量方程

能量方程 $E=f(X0,r)=\sum _1^n{d}_i^2$

上式是一个4元二次函数中，X0, r是未知量，拟合2D圆的过程也可以理解为优化X0, r使得方程E最小。

可以对上述方程求导，使得导数等于0取得最值。但是求导后会变成一个比较复杂的方程组，不好解。可以使用高斯牛顿迭代法来求解。

高斯牛顿迭代法

用于解非线性最小二乘问题。同时，高斯牛顿法需要比较可靠的初值，所以寻找目标函数的初值也是一个比较关键的步骤。

基本原理

$设a=(a_0,a_1,...,a_n)是待求解向量，\hat{a}是初始给定值，a=\hat{a}+\Delta a,\Delta a是我们每次迭代后移动的量$

$定义距离函数为F(x,a),d_i=F(x_i,a),进行泰勒1阶展开，F(x,a)＝F(x,\hat{a})+\frac{?F}{?\hat{a}}\Delta a=F(x,\hat{a})+J\Delta a$

$每次迭代，其实就是希望通过调整\Delta a使得J\Delta a＝?F(x,\hat{a})$

$J=\left[\begin{array}{cccc}\frac{?F(x_0,a)}{?a_0}& \frac{?F(x_0,a)}{?a_1}& ...& \frac{?F(x_0,a)}{?a_n}\\ & & & \\ \frac{?F(x_1,a)}{?a_0}& \frac{?F(x_1,a)}{?a_1}& ...& \frac{?F(x_1,a)}{?a_n}\\ & & & \\ ...& ...& ...& ...\\ & & & \\ \frac{?F(x_n,a)}{?a_0}& \frac{?F(x_n,a)}{?a_1}& ...& \frac{?F(x_n,a)}{?a_n}\end{array}\right]$

$F(x,\hat{a})=\left[\begin{array}{c}{d}_1\\ d_2\\ ...\\ d_m\end{array}\right]$

$J\Delta a=?F(x,\hat{a}),解出\Delta a,更新a=\hat{a}+\Delta a,持续迭代直到\Delta a足够小$

算法描述

Ji, di的计算。

对3个未知数求导结果如下：

$\frac{?d_i}{?x_0}=\frac{1}{2\sqrt{(x_i?x_0{)}^2+(y_i?y_0{)}^2}}?(x_i?x_0)??1=?(x_i?x_0)/r_i$

$\frac{?d_i}{?y_0}=\frac{1}{2\sqrt{(x_i?x_0{)}^2+(y_i?y_0{)}^2}}?(y_i?y_0)??1=?(y_i?y_0)/r_i$

$\frac{?d_i}{?r}=?1$

1. 确定圆初值

2. 根据上述公式构建$J?\left(\left[\begin{array}{c}{p}_{x_0}\\ p_{y_0}\\ p_r\end{array}\right]\right)=?D=?\left[\begin{array}{c}{d}_1\\ d_2\\ ...\\ d_n\end{array}\right]$

3. 求解$\Delta p$

4. 更新解
   $\begin{array}{c}{x}_0=x_0+p_{x_0}\\ y_0=y_0+p_{y_0}\\ r=r+p_r\end{array}$

5. 重复2直到收敛

初值确定

先将圆建立线性的能量方程。
可以作一个近似转换，把半径先平方再相减。

$F=\sum _{i=1}^n{f}_i^2$

$f_i=r_i^2?r^2$

展开后

$$\begin{gathered} f_i=(x_i?x_0{)}^2+(y_i?y_0{)}^2?r^2 \\ =?2x_i{x}_0?2y_i{y}_0+(x_0^2+y_0^2?r^2)+(x_i^2+y_i^2) \end{gathered}$$

我们希望fi都为0。

$可以令\rho =x_0^2+y_0^2?r^2$

建立方程组

$$\begin{gathered} 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 } \end{gathered}$$

代码实现

代码链接：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

---

本人码农，希望通过自己的分享，让大家更容易学懂计算机知识。创作不易，帮忙点击公众号的链接。