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

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

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

背景

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认证的拟合算法。

平面拟合输入和输出要求

输入

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

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

输出

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

精度要求

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

平面优化标函数

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

平面参数化表示

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

点到平面距离

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

根据点乘得到距离

$d_i=\frac{\Vert (p_i?X_0)?A\Vert }{\Vert A\Vert }$

展开一下：

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

最小二乘优化能量方程

能量方程 $E=f(X0,A)=\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

算法过程

1. 计算出所有点平均值 $centerid(\overline{x},\overline{y},\overline{z})$ , 作为平面上1点。
2. 构建矩阵$A=\left[\begin{array}{ccc}{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{array}\right]$
3. 对矩阵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)是待求解向量，\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足够小$

如何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{?d_i}{?z0}={\frac{?\frac{a(x_i?x_0)+b(y_i?y_0)+c(z_i?z_0)}{\sqrt{a^2+b^2+c^2}}}{?z0}}_{|z0=0}=?1$

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

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

$d_i=z_i$

1. 确定平面初值

2. 将直线通过刚体变换U至Z轴，U的构建可以参考代码
   $\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]=U?\left(\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]?\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]\right)$

3. 根据上述公式构建$J?\left(\left[\begin{array}{c}{p}_{z_0}\\ p_a\\ p_b\end{array}\right]\right)=?D=?\left[\begin{array}{c}{d}_0\\ d_1\\ ...\\ d_n\end{array}\right]$

4. 求解$\Delta p$

5. 更新解
   $\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]=\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]+U^T?\left[\begin{array}{c}{p}_a?p_{z_0}\\ p_b?p_{z_0}\\ p_{z_0}\end{array}\right]$

$\left[\begin{array}{c}a\\ b\\ c\end{array}\right]=U^T?\left[\begin{array}{c}{p}_a\\ p_b\\ 1\end{array}\right].normalize()$

6. 重复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

---

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