最小二乘平面拟合(高斯牛顿法)

闪电彬彬

已于 2024-01-21 18:49:56 修改

阅读量946

点赞数 25

分类专栏：图形学高阶算法 c++ 文章标签：平面最小二乘法高斯牛顿迭代

于 2024-01-21 18:42:10 首次发布

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

版权

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

61 篇文章 35 订阅

订阅专栏

高阶算法

50 篇文章 3 订阅

订阅专栏

c++

21 篇文章 0 订阅

订阅专栏

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

本期话题：最小二乘平面拟合

背景

ptb认证

ptb是对几何体拟合算法的认证。

主要涉及3D直线，3D圆，平面，球，圆柱，锥。

官方会给出点云信息，由用户将拟合结果上传到官方服务器进行对比答案，返回结果。

拟合有很多种度量标准，不同的标准出来的答案不可能完全精确。所以，要通过认证必须用官方给定的度量方法，具体可以参考论文。

认证精度要求

在这里插入图片描述
对于位置类型，比如圆心，直线的点等，误差不能超过0.0001mm。

对于角度，比如圆锥的开角，误差不能超过0.0000001rad。

对于方向，与标准值夹角不能直过0.0000001rad。

对于半径，误差不能超过0.0001mm。

学习资料

论文资料
Forbes, Alistair B.: Least-squares best fit geometric elements, NPL Report DITC 140/89 , ISSN
0262-5369, 40 pages, revised edition, 1991

由NPL发表。里面介绍了所有ptb认证的拟合算法。

平面拟合输入和输出要求

输入

8到50个点，全部采样自平面上。
每个点3个坐标，坐标精确到小数点后面20位。
坐标单位是mm, 范围[-500mm, 500mm]。

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

输出

平面上1点p，用三个坐标表示。
平面法向k，用三个坐标表示，需要单位化。

精度要求

p点到标准平面距离不能超过0.0001mm。
k与标准法向的夹角不能超过0.0000001rad。

平面优化标函数

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

平面参数化表示

平面上1点X0 = (x0, y0, z0)。
方向单位向量A=(a,b,c)。

点到平面距离

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

根据点乘得到距离

$d_i = \frac { \left \| (p_i-X_0)\cdot A \right \|}{\left \| A \right \|}$

展开一下：

$d_i = \frac {a(x_i-x_0) + b(y_i-y_0) + c(z_i-z_0)} {\sqrt{a^2+b^2+c^2}}$

最小二乘优化能量方程

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

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

拟合&测试基类设计

基类主要放一些公共方法和规定具体实现类要实现的算法。

具体在调用时，都用基类为载体，减少代码量。

拟合基类

#include <Eigen/Core>
#include <vector>
#include "GeometryTypes.h"


namespace Fitting {
	using Matrix = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
	/* getCenter
		获取点云中心
	 */
	Eigen::Vector3d getCenter(const std::vector<Eigen::Vector3d>& points);

	/* moveCenter
	* 将中心移至0点
	*/
	void moveCenter(const Eigen::Vector3d center, std::vector<Eigen::Vector3d>& points);

	/* getRotationByOrient(Eigen::Vector3d orient);
	* 返回一个旋转矩阵使得向量与Z轴平行
	*/ 
	Eigen::Matrix3d getRotationByOrient(Eigen::Vector3d orient);
	class FittingBase {
	protected:
		Eigen::Matrix3d U;
		std::vector<Eigen::Vector3d> transPoints;
		/* Jacobi
		* 构造jacobi矩阵
		*/
		virtual Matrix Jacobi(const std::vector<Eigen::Vector3d> &points)=0;
		
		/* findNext
		*	迭代1次得到解delta
		*/
		Eigen::VectorXd findNext(const std::vector<Eigen::Vector3d>& points);

		/* beforHook
		* 每次迭代前的准备工作
		*/
		virtual void beforHook(Eigen::VectorXd& a0, const std::vector<Eigen::Vector3d>& points)=0;
		
		/* afterHook
		* 迭代后更新答案
		*/
		virtual void afterHook(const Eigen::VectorXd& xp)=0;

		
		/* 获取 d数组
		*/
		virtual Eigen::VectorXd getDArray(const std::vector<Eigen::Vector3d>& points)=0;
		 
		// GetInitFit 返回是否拟合成功
		virtual bool GetInitFit(const std::vector<Eigen::Vector3d>& points)=0;
		// iteration
		double iteration(const std::vector<Eigen::Vector3d>& points);

		/* F
		* 距离函数
		*/
		virtual double F(const Eigen::Vector3d& p)=0;

		/* 获取 最小二乘残差
		*/
		virtual double  GetError(const std::vector<Eigen::Vector3d>& points) = 0;
		/* 获取 结果
		*/
		virtual void Copy(void* ele) = 0;

	public:
		// Fitting
		double Fitting(const std::vector<Eigen::Vector3d>& points, void* ele);
	};
}

测试基类


namespace Fitting {
	const double POSITION_MAX_DST = 0.0001, ORIENTATION_MAX_DST = 0.0000001, ANGLE_MAX_DST = 0.0000001, RADIUS_MAX_DST = 0.0001;

	void writePoint(FILE *fp, const Point &p);
	void writeNumber(FILE *fp, double n);

	// 比较平面点是否一致
	bool planeCmp(const Eigen::Vector3d& vec1, const Point& p1, const Point& p2);

	// 比较直线点是否一致
	bool lineCmp(const Eigen::Vector3d &vec1, const Point &p1, const Point &p2);
	bool lineCmp2D(const Eigen::Vector3d &vec1, const Point &p1, const Point &p2);
	// 比较半径
	bool radiusCmp(double r1, double r2);
	// 比较法向是否一致
	bool orientationCmp(const Eigen::Vector3d &vec1, const Eigen::Vector3d& vec2);
	bool orientationCmp2D(const Eigen::Vector3d &vec1, const Eigen::Vector3d& vec2);
	// 比较角度
	bool angleCmp(double angle1, double angle2);
	// 比较点位置
	bool positionCmp(const Point &p1, const Point &p2);

	/*
	测试基类
	*/
	class TestBase {
	protected:
		// 测试用例路径和文件名
		string suffixName, fileName;
		vector<Point> points;
	public:
		TestBase() {}
		TestBase(string _suffixName, string _fileName): suffixName(_suffixName), fileName(_fileName) {}
		void SetFile(string _suffixName, string _fileName);

		void readPoints();
		virtual double Fitting()=0;
		virtual bool JudgeAnswer(FILE* fp) = 0;
		virtual void ReadAnswer() = 0;
		virtual void SaveAnswer(FILE* fp) = 0;
	};

}

PCA主成分分析法

学习资料：
https://www.bilibili.com/video/BV1E5411E71z/?spm_id_from=333.337.search-card.all.click&vd_source=fb27f95f25902a2cc94d4d8e49f5f777

算法过程

计算出所有点平均值 $centerid(\overline x,\overline y,\overline z)$ , 作为平面上1点。
构建矩阵 $A=\begin {bmatrix} x_1-\overline x& y_1-\overline y & z_1-\overline z \\ x_2-\overline x& y_2-\overline y & z_2-\overline z\\ ... & ... &... \\x_n-\overline x& y_n-\overline y & z_n-\overline z \end {bmatrix}$
对矩阵A进行SVD分解，取右特征值矩阵最小特征值所对应列作为平面的法向。

代码实现

代码链接：https://gitcode.com/chenbb1989/3DAlgorithm/tree/master/CBB3DAlgorithm/Fitting

#include "TestPlane.h"
#include "FittingPlanePCA.h"
#include <iostream>

namespace Gauss {

	double TestPlanePCA::Fitting() {
		return FittingPlane(points, fitResult);
	}
	bool TestPlanePCA::JudgeAnswer(FILE* fp) {
		ReadAnswer();
		if (!planeCmp(ans.Orient, ans.BasePoint, fitResult.BasePoint))return false;
		if (!orientationCmp(ans.Orient, ans.Orient))return false;
		return true;
	}
	void TestPlanePCA::ReadAnswer() {
		vector<double> nums;
		if (PointCloud::readNums((suffixName + "/answer/" + fileName + ".txt"), nums)) {
			for (int i = 0; i < 3; ++i) ans.BasePoint[i] = nums[i];
			for (int i = 0; i < 3; ++i) ans.Orient[i] = nums[3 + i];
		}
		else {
			std::cout << "read answer error" << std::endl;
		}
	}
	void TestPlanePCA::SaveAnswer(FILE* fp) {
		writePoint(fp, fitResult.BasePoint);
		writePoint(fp, fitResult.Orient);
	}
}

测试结果

https://gitcode.com/chenbb1989/3DAlgorithm/blob/master/CBB3DAlgorithm/Fitting/gauss/fitting_result/result.txt

b05 : PLANE : pass
b06 : PLANE : pass
b07 : PLANE : pass
b08 : PLANE : pass

高斯牛顿迭代法

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

基本原理

$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足够小$

如何3个参数表示平面

如果直接拿6个参数去做迭代，1是比较麻烦，会出现比较难解的方向，2是平面上的点有很多个，结果不唯一。

当平面法向与Z轴偏差比较小的时候可以使用3个参数来表示。

在这里插入图片描述

如上图，绿线为Z轴，橙色线为XOY平面。

由于法向与Z轴比较相近，可以设法向为(a, b, 1), a,b 是比较小的量。

现在表示平面还需要一个点，规定点必须在以(a,b,1)为法向过0点的直线上。

就有直线公式 x0/a=y0/b=z0/c

那么只要知道z0就可知 x0=az0, y0=bz0.

综上，可以使用a, b, z0 表示一个法向与Z轴相近的平面。

算法描述

Ji, di的计算。
由于算法是要将法向旋转致Z轴再进行迭代。

即a=b=z0=x0=y0=0
c=1

对3个未知数求导结果如下：
求导后a=b=z0=x0=y0=0，c=1要代入

$\frac {\partial d_i} {\partial z0}=\frac {\partial \frac {a(x_i-x_0) + b(y_i-y_0) + c(z_i-z_0)}{\sqrt{a^2+b^2+c^2}}} {\partial z0}_{|z0=0} = -1$

$\frac {\partial d_i} {\partial a}=\frac {\partial \frac {a(x_i-x_0) + b(y_i-y_0) + c(z_i-z_0)}{\sqrt{a^2+b^2+c^2}}} {\partial a}_{|a=0} = x_i$

$\frac {\partial d_i} {\partial b}=\frac {\partial \frac {a(x_i-x_0) + b(y_i-y_0) + c(z_i-z_0)}{\sqrt{a^2+b^2+c^2}}} {\partial b}_{|b=0} = y_i$

$d_i =z_i$

确定平面初值
将直线通过刚体变换U至Z轴，U的构建可以参考代码
$\begin {bmatrix}x_i \\ y_i \\ z_i \end {bmatrix} = U \cdot \left (\begin {bmatrix}x_i \\ y_i \\ z_i \end {bmatrix}- \begin{bmatrix}x_0 \\ y_0 \\ z_0 \end {bmatrix}\right )$
根据上述公式构建 $\cdot \left(\begin {bmatrix}p_{z_0} \\ p_{a}\\p_{b} \end {bmatrix} \right)=-D=-\begin {bmatrix}d_0 \\ d_1\\...\\d_n \end {bmatrix}$
求解 $\Delta p$
更新解
$\begin {bmatrix}x_0 \\ y_0 \\ z_0 \end {bmatrix} = \begin {bmatrix}x_0 \\ y_0 \\ z_0 \end {bmatrix} + U^T \cdot \begin{bmatrix}p_a*p_{z_0} \\ p_b*p_{z_0} \\ p_{z_0} \end {bmatrix}$

$\begin {bmatrix}a \\ b \\ c \end {bmatrix} = U^T \cdot \begin{bmatrix}p_a \\ p_b \\ 1 \end {bmatrix}.normalize()$

重复2直到收敛

初值确定

由于点数比较少，可以枚举3个点生成平面。

代码实现

代码链接：https://gitcode.com/chenbb1989/3DAlgorithm/tree/master/CBB3DAlgorithm/Fitting

#include "FittingPlane.h"
#include "FittingPlanePCA.h"
#include <Eigen/Dense>


namespace Gauss {
	Fitting::Matrix FittingPlane::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];
			J(i, 0) = -1;
			J(i, 1) = p.x();
			J(i, 2) = p.y();
		}
		return J;
	}
	void FittingPlane::beforHook(Eigen::VectorXd& a0, const std::vector<Eigen::Vector3d>& points)
	{
		a0.resize(3);
		a0 << 0, 0, 0;
		U = Fitting::getRotationByOrient(plane.Orient);
		for (int i = 0; i < points.size(); ++i) {
			transPoints[i] = U * (points[i] - plane.BasePoint);
		}
	}
	void FittingPlane::afterHook(const Eigen::VectorXd& xp)
	{
		plane.BasePoint += U.transpose() * Eigen::Vector3d(xp(0)*xp(1), xp(0)*xp(2), xp(0));
		plane.Orient =U.transpose()* Eigen::Vector3d(xp(1), xp(2), 1).normalized();
	}
	Eigen::VectorXd FittingPlane::getDArray(const std::vector<Eigen::Vector3d>& points)
	{
		Eigen::VectorXd D(points.size());
		for (int i = 0; i < points.size(); ++i)D(i) =points[i].z();
		return D;
	}
	bool FittingPlane::GetInitFit(const std::vector<Eigen::Vector3d>& points)
	{
		if (points.size() < 3)return false;
		plane.BasePoint = points[0];
		Point s = points[1];
		Point s1 = points[2];

		plane.Orient = (s-plane.BasePoint).cross(s1-plane.BasePoint);
		plane.Orient.normalize();

		return true;
	}
	double FittingPlane::F(const Eigen::Vector3d& p)
	{
		return Gauss::F(plane, p);
	}
	double FittingPlane::GetError(const std::vector<Eigen::Vector3d>& points)
	{
		return Gauss::GetError(plane, points);
	}
	void FittingPlane::Copy(void* ele)
	{
		memcpy(ele, &plane, sizeof(Fitting::Plane));
	}
}

测试结果

https://gitcode.com/chenbb1989/3DAlgorithm/blob/master/CBB3DAlgorithm/Fitting/gauss/fitting_result/result.txt

b05 : PLANE : pass
b06 : PLANE : pass
b07 : PLANE : pass
b08 : PLANE : pass

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