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

闪电彬彬

已于 2024-01-25 08:15:34 修改

阅读量1.1k

点赞数 20

分类专栏：图形学高阶算法数学文章标签：几何拟合高斯牛顿法

于 2024-01-24 17:10:26 首次发布

本文链接：https://blog.csdn.net/chenbb1989/article/details/135750499

版权

图形学同时被 3 个专栏收录

61 篇文章 39 订阅

订阅专栏

高阶算法

53 篇文章 6 订阅

订阅专栏

数学

37 篇文章 0 订阅

订阅专栏

本文介绍了如何使用最小二乘方法在2D空间中拟合圆，涉及圆的参数化表示、点到圆的距离计算、能量方程以及使用高斯牛顿迭代法求解最优圆心和半径。文章详细阐述了求导和迭代过程，并提供了C++代码实现和测试结果。

摘要由CSDN通过智能技术生成

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

本期话题：最小二乘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$

点到2D圆距离

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

根据点乘得到距离

$d_i =\left \| (p_i-X_0) \right \|-r$

展开一下：

$d_i = r_i -r$

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

最小二乘优化能量方程

能量方程 $r)=\displaystyle \sum _1^n {d_i^2}$

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

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

高斯牛顿迭代法

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

基本原理

$a=(a_0, a_1,...,a_n)是待求解向量，\widehat {a} 是初始给定值，a = \widehat {a} +\Delta a, \Delta 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$

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

$\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}$

$\widehat a) = \begin {bmatrix} d_1 \\ d_2 \\... \\ d_m \end {bmatrix}$

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

算法描述

Ji, di的计算。

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

$\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$

$\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$

$\frac {\partial d_i} {\partial r}=-1$

确定圆初值
根据上述公式构建 $\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}$
求解 $\Delta p$
更新解
$\begin {array}{c} x_0 = x_0+p_{x_0} \\y_0 = y_0+p_{y_0} \\ r=r+p_r \end {array}$
重复2直到收敛

初值确定

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

$F=\displaystyle \sum_{i=1}^{n} f_i^2$

$f_i = r_i^2-r^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都为0。

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

建立方程组

$\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}$

代码实现

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

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