欢迎关注更多精彩
关注我，学习常用算法与数据结构，一题多解，降维打击。
本期话题：最小二乘平面拟合
背景
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 = ∥ ( p i − X 0 ) ⋅ A ∥ ∥ A ∥ d_i = \frac { \left \| (p_i-X_0)\cdot A \right \|}{\left \| A \right \|} di=∥A∥∥(pi−X0)⋅A∥
展开一下：
d i = a ( x i − x 0 ) + b ( y i − y 0 ) + c ( z i − z 0 ) a 2 + b 2 + c 2 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}} di=a2+b2+c2a(xi−x0)+b(yi−y0)+c(zi−z0)
最小二乘优化能量方程
能量方程 E = f ( X 0 , A ) = ∑ 1 n d i 2 E=f(X0, A)=\displaystyle \sum _1^n {d_i^2} E=f(X0,A)=1∑ndi2
上式是一个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
算法过程
- 计算出所有点平均值 c e n t e r i d ( x ‾ , y ‾ , z ‾ ) centerid(\overline x,\overline y,\overline z) centerid(x,y,z) , 作为平面上1点。
- 构建矩阵 A = [ x 1 − x ‾ y 1 − y ‾ z 1 − z ‾ x 2 − x ‾ y 2 − y ‾ z 2 − z ‾ . . . . . . . . . x n − x ‾ y n − y ‾ z n − z ‾ ] 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= x1−xx2−x...xn−xy1−yy2−y...yn−yz1−zz2−z...zn−z
- 对矩阵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 ) 是待求解向量， 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足够小
如何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要代入
∂ d i ∂ z 0 = ∂ a ( x i − x 0 ) + b ( y i − y 0 ) + c ( z i − z 0 ) a 2 + b 2 + c 2 ∂ z 0 ∣ z 0 = 0 = − 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 ∂z0∂di=∂z0∂a2+b2+c2a(xi−x0)+b(yi−y0)+c(zi−z0)∣z0=0=−1
∂ d i ∂ a = ∂ a ( x i − x 0 ) + b ( y i − y 0 ) + c ( z i − z 0 ) a 2 + b 2 + c 2 ∂ a ∣ a = 0 = x i \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 ∂a∂di=∂a∂a2+b2+c2a(xi−x0)+b(yi−y0)+c(zi−z0)∣a=0=xi
∂ d i ∂ b = ∂ a ( x i − x 0 ) + b ( y i − y 0 ) + c ( z i − z 0 ) a 2 + b 2 + c 2 ∂ b ∣ b = 0 = y 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 ∂b∂di=∂b∂a2+b2+c2a(xi−x0)+b(yi−y0)+c(zi−z0)∣b=0=yi
d i = z i d_i =z_i di=zi
-
确定平面初值
-
将直线通过刚体变换U至Z轴，U的构建可以参考代码
[ x i y i z i ] = U ⋅ ( [ x i y i z i ] − [ x 0 y 0 z 0 ] ) \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 ) xiyizi =U⋅ xiyizi − x0y0z0 -
根据上述公式构建 J ⋅ ( [ p z 0 p a p b ] ) = − D = − [ d 0 d 1 . . . d n ] J \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} J⋅ pz0papb =−D=− d0d1...dn
-
求解 Δ p \Delta p Δp
-
更新解
[ x 0 y 0 z 0 ] = [ x 0 y 0 z 0 ] + U T ⋅ [ p a ∗ p z 0 p b ∗ p z 0 p z 0 ] \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} x0y0z0 = x0y0z0 +UT⋅ pa∗pz0pb∗pz0pz0
[ a b c ] = U T ⋅ [ p a p b 1 ] . n o r m a l i z e ( ) \begin {bmatrix}a \\ b \\ c \end {bmatrix} = U^T \cdot \begin{bmatrix}p_a \\ p_b \\ 1 \end {bmatrix}.normalize() abc =UT⋅ papb1 .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
本人码农，希望通过自己的分享，让大家更容易学懂计算机知识。创作不易，帮忙点击公众号的链接。
