欢迎关注更多精彩
关注我，学习常用算法与数据结构，一题多解，降维打击。
本期话题：最小二乘3D圆拟合
相关背景资料
点击前往
3D圆拟合输入和输出要求
输入
- 8到50个点，全部采样自3D圆上。
- 每个点3个坐标，坐标精确到小数点后面20位。
- 坐标单位是mm, 范围[-500mm, 500mm]。
tips
输入虽然是严格来自3D圆，但是由于浮点表示，记录的值和真实值始终是有微小的误差，点云到拟合圆的距离不可能完全为0。
输出
- 1点X0表示 圆心，用三个坐标表示。
- 法向A代表圆所在平面法向。
- 圆半径r。
精度要求
- X0点到标准圆心距离不能超过0.0001mm。
- 法向A与圆法向夹角不能超过0.0000001rad。
- r与标准半径的差不能超过0.0001mm。
3D圆优化标函数
根据论文，圆拟合转化成数学表示如下：
圆参数化表示
- 圆心为1点X0 = (x0, y0, z0)。
- 法向A=(a,b,c)。
- 半径r。
圆方程 ( x − x 0 ) 2 + ( y − y 0 ) 2 + ( z − z 0 ) 2 = r 2 a ( x − x 0 ) + b ( y − y 0 ) + c ( z − z 0 ) = 0 圆方程\begin {array}{c}(x-x_0)^2+(y-y_0)^2+(z-z_0)^2=r^2 \\ a(x-x_0)+b(y-y_0)+c(z-z_0)=0 \end {array} 圆方程(x−x0)2+(y−y0)2+(z−z0)2=r2a(x−x0)+b(y−y0)+c(z−z0)=0
能量方程
第i个点 pi(xi, yi, zi)。
定义距离如图
根据勾股定理，黄色线为距离。
d i = e i 2 + f i 2 d_i = \sqrt{e_i^2+f_i^2} di=ei2+fi2
根据定义得到能量方程
E = ∑ 1 n d i 2 = ∑ 1 n ( e i 2 + f i 2 ) E=\displaystyle \sum _1^n {d_i^2}=\displaystyle \sum _1^n {(e_i^2+f_i^2)} E=1∑ndi2=1∑n(ei2+fi2)
e i = a ( x i − x 0 ) + b ( y i − y 0 ) + c ( z i − z 0 ) f i = u i 2 + v i 2 + w i 2 − r \begin {array}{l} e_i =a(x_i-x_0)+b(y_i-y_0)+c(z_i-z_0)\\ f_i=\sqrt{u_i^2+v_i^2+w_i^2}-r\end {array} ei=a(xi−x0)+b(yi−y0)+c(zi−z0)fi=ui2+vi2+wi2−r
u i = c ( y i − y 0 ) − b ( z i − z 0 ) v i = a ( z i − z 0 ) − c ( x i − x 0 ) w i = b ( x i − x 0 ) − a ( y i − y 0 ) \begin {array}{l}u_i =c(y_i-y_0)-b(z_i-z_0)\\ v_i=a(z_i-z_0)-c(x_i-x_0)\\ w_i=b(x_i-x_0)-a(y_i-y_0)\end {array} ui=c(yi−y0)−b(zi−z0)vi=a(zi−z0)−c(xi−x0)wi=b(xi−x0)−a(yi−y0)
最小二乘优化能量方程
能量方程 E = f ( X 0 , A , r ) = ∑ 1 n d i 2 E=f(X0, A, r)=\displaystyle \sum _1^n {d_i^2} E=f(X0,A,r)=1∑ndi2
上式是一个7元二次函数中，X0, A, r是未知量，拟合3D圆的过程也可以理解为优化X0, A, r使得方程E最小。
上述方程直接求导不好解，可以使用高斯牛顿迭代法。
高斯牛顿迭代法
用于解非线性最小二乘问题。同时，高斯牛顿法需要比较可靠的初值，所以寻找目标函数的初值也是一个比较关键的步骤。
基本原理
设 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足够小
用2个数表示法向
如果直接拿3个参数表示法向去做迭代，1是比较麻烦，会出现比较难解的方向，2是法向长度不固定，结果不唯一。
当直线与Z轴偏差比较小的时候可以使用2个参数来表示。
如上图，绿线为Z轴，橙色线为XOY平面。
由于法向与Z轴比较相近，可以设法向为(a, b, 1), a,b 是比较小的量。
模型简化
为了使用上述方法，当得到一个拟合初值后，需要先将中心线旋转致Z轴，把圆心移致0点。
此时，
x0=y0=z0=0.
a=b=0, c=1.
e i = z i f i = ( x i 2 + y i 2 ) − r \begin {array}{l} e_i =z_i\\ f_i=\sqrt{(x_i^2+y_i^2)}-r\end {array} ei=zifi=(xi2+yi2)−r
算法描述
E = ∑ 1 n ( e i 2 + f i 2 ) = ∑ 1 n e i 2 + ∑ 1 n f i 2 E=\displaystyle \sum _1^n {(e_i^2+f_i^2)}=\displaystyle \sum _1^n {e_i^2}+\displaystyle \sum _1^n{f_i^2} E=1∑n(ei2+fi2)=1∑nei2+1∑nfi2
我们希望ei,fi 都等于0。
可以对ei, fi 分别求偏导
J, D的计算。
J = ∂ e 1 ∂ x 0 ∂ e 1 ∂ y 0 ∂ e 1 ∂ a ∂ e 1 ∂ b ∂ e 1 ∂ r ∂ e 2 ∂ x 0 ∂ e 2 ∂ y 0 ∂ e 2 ∂ a ∂ e 2 ∂ b ∂ e 2 ∂ r . . . . . . . . . . . . . . . ∂ e n ∂ x 0 ∂ e n ∂ y 0 ∂ e n ∂ a ∂ e n ∂ b ∂ e n ∂ r ∂ f 1 ∂ x 0 ∂ f 1 ∂ y 0 ∂ f 1 ∂ a ∂ f 1 ∂ b ∂ f 1 ∂ r ∂ f 2 ∂ x 0 ∂ f 2 ∂ y 0 ∂ f 2 ∂ a ∂ f 2 ∂ b ∂ f 2 ∂ r . . . . . . . . . . . . . . . ∂ f n ∂ x 0 ∂ f n ∂ y 0 ∂ f n ∂ a ∂ f n ∂ b ∂ f n ∂ r , D = e 1 e 2 . . . e n f 1 f 2 . . . f n J= \begin{array}{l} \frac {\partial e_1}{\partial x_0}& \frac {\partial e_1}{\partial y_0}& \frac {\partial e_1}{\partial a}& \frac {\partial e_1}{\partial b}& \frac {\partial e_1}{\partial r} \\ \frac {\partial e_2}{\partial x_0}& \frac {\partial e_2}{\partial y_0}& \frac {\partial e_2}{\partial a}& \frac {\partial e_2}{\partial b}& \frac {\partial e_2}{\partial r}\\...&...&...&...&...\\\frac {\partial e_n}{\partial x_0}& \frac {\partial e_n}{\partial y_0}& \frac {\partial e_n}{\partial a}& \frac {\partial e_n}{\partial b}& \frac {\partial e_n}{\partial r}\\ \frac {\partial f_1}{\partial x_0}& \frac {\partial f_1}{\partial y_0}& \frac {\partial f_1}{\partial a}& \frac {\partial f_1}{\partial b}& \frac {\partial f_1}{\partial r} \\ \frac {\partial f_2}{\partial x_0}& \frac {\partial f_2}{\partial y_0}& \frac {\partial f_2}{\partial a}& \frac {\partial f_2}{\partial b}& \frac {\partial f_2}{\partial r}\\...&...&...&...&...\\\frac {\partial f_n}{\partial x_0}& \frac {\partial f_n}{\partial y_0}& \frac {\partial f_n}{\partial a}& \frac {\partial f_n}{\partial b}& \frac {\partial f_n}{\partial r} \end {array}, \ D= \begin{array}{l} e_1\\e_2\\...\\e_n\\ f_1\\f_2\\...\\f_n \end {array} J=∂x0∂e1∂x0∂e2...∂x0∂en∂x0∂f1∂x0∂f2...∂x0∂fn∂y0∂e1∂y0∂e2...∂y0∂en∂y0∂f1∂y0∂f2...∂y0∂fn∂a∂e1∂a∂e2...∂a∂en∂a∂f1∂a∂f2...∂a∂fn∂b∂e1∂b∂e2...∂b∂en∂b∂f1∂b∂f2...∂b∂fn∂r∂e1∂r∂e2...∂r∂en∂r∂f1∂r∂f2...∂r∂fn, D=e1e2...enf1f2...fn
6个未知分别对e_i, f_i求导结果如下：
∂ e i ∂ x 0 = a ( x i − x 0 ) + b ( y i − y 0 ) + c ( z i − z 0 ) ∂ x 0 = − a = 0 \frac {\partial e_i} {\partial x_0}=\frac {a(x_i-x_0)+b(y_i-y_0)+c(z_i-z_0)} {\partial x_0} = -a=0 ∂x0∂ei=∂x0a(xi−x0)+b(yi−y0)+c(zi−z0)=−a=0
∂ e i ∂ y 0 = a ( x i − x 0 ) + b ( y i − y 0 ) + c ( z i − z 0 ) ∂ y 0 = − b = 0 \frac {\partial e_i} {\partial y_0}=\frac {a(x_i-x_0)+b(y_i-y_0)+c(z_i-z_0)} {\partial y_0} = -b=0 ∂y0∂ei=∂y0a(xi−x0)+b(yi−y0)+c(zi−z0)=−b=0
∂ e i ∂ z 0 = a ( x i − x 0 ) + b ( y i − y 0 ) + c ( z i − z 0 ) ∂ z 0 = − c = − 1 \frac {\partial e_i} {\partial z_0}=\frac {a(x_i-x_0)+b(y_i-y_0)+c(z_i-z_0)} {\partial z_0} = -c=-1 ∂z0∂ei=∂z0a(xi−x0)+b(yi−y0)+c(zi−z0)=−c=−1
∂ e i ∂ a = a ( x i − x 0 ) + b ( y i − y 0 ) + c ( z i − z 0 ) ∂ a = x i − x 0 = x i \frac {\partial e_i} {\partial a}=\frac {a(x_i-x_0)+b(y_i-y_0)+c(z_i-z_0)} {\partial a} = x_i-x_0=x_i ∂a∂ei=∂aa(xi−x0)+b(yi−y0)+c(zi−z0)=xi−x0=xi
∂ e i ∂ b = a ( x i − x 0 ) + b ( y i − y 0 ) + c ( z i − z 0 ) ∂ b = y i − y 0 = y i \frac {\partial e_i} {\partial b}=\frac {a(x_i-x_0)+b(y_i-y_0)+c(z_i-z_0)} {\partial b} = y_i-y_0=y_i ∂b∂ei=∂ba(xi−x0)+b(yi−y0)+c(zi−z0)=yi−y0=yi
∂ e i ∂ r = a ( x i − x 0 ) + b ( y i − y 0 ) + c ( z i − z 0 ) ∂ r = 0 \frac {\partial e_i} {\partial r}=\frac {a(x_i-x_0)+b(y_i-y_0)+c(z_i-z_0)} {\partial r} = 0 ∂r∂ei=∂ra(xi−x0)+b(yi−y0)+c(zi−z0)=0
回顾一下
u i = c ( y i − y 0 ) − b ( z i − z 0 ) v i = a ( z i − z 0 ) − c ( x i − x 0 ) w i = b ( x i − x 0 ) − a ( y i − y 0 ) \begin {array}{l}u_i =c(y_i-y_0)-b(z_i-z_0)\\ v_i=a(z_i-z_0)-c(x_i-x_0)\\ w_i=b(x_i-x_0)-a(y_i-y_0)\end {array} ui=c(yi−y0)−b(zi−z0)vi=a(zi−z0)−c(xi−x0)wi=b(xi−x0)−a(yi−y0)
∂ f i ∂ x 0 = u i 2 + v i 2 + w i 2 − r ∂ x 0 = ( x i − x 0 ) 2 ∂ x 0 2 u i 2 + v i 2 + w i 2 = − x i x i 2 + y i 2 \frac {\partial f_i} {\partial x_0}=\frac {\sqrt{u_i^2+v_i^2+w_i^2}-r} {\partial x_0} \\ =\frac {\frac {(x_i-x_0)^2}{\partial x_0}}{2\sqrt{u_i^2+v_i^2+w_i^2}}\\ =\frac {-x_i}{\sqrt{x_i^2+y_i^2}} ∂x0∂fi=∂x0ui2+vi2+wi2−r=2ui2+vi2+wi2∂x0(xi−x0)2=xi2+yi2−xi
∂ f i ∂ y 0 = u i 2 + v i 2 + w i 2 − r ∂ y 0 = ( y i − y 0 ) 2 ∂ y 0 2 u i 2 + v i 2 + w i 2 = − y i x i 2 + y i 2 \frac {\partial f_i} {\partial y_0}=\frac {\sqrt{u_i^2+v_i^2+w_i^2}-r} {\partial y_0} \\ =\frac {\frac {(y_i-y_0)^2}{\partial y_0}}{2\sqrt{u_i^2+v_i^2+w_i^2}}\\ =\frac {-y_i}{\sqrt{x_i^2+y_i^2}} ∂y0∂fi=∂y0ui2+vi2+wi2−r=2ui2+vi2+wi2∂y0(yi−y0)2=xi2+yi2−yi
∂ f i ∂ z 0 = u i 2 + v i 2 + w i 2 − r ∂ z 0 = 0 \frac {\partial f_i} {\partial z_0}=\frac {\sqrt{u_i^2+v_i^2+w_i^2}-r} {\partial z_0}=0 ∂z0∂fi=∂z0ui2+vi2+wi2−r=0
∂ f i ∂ a = u i 2 + v i 2 + w i 2 − r ∂ a = [ a ( z i − z 0 ) − c ( x i − x 0 ) ] 2 ∂ a 2 u i 2 + v i 2 + w i 2 = − x i z i x i 2 + y i 2 \frac {\partial f_i} {\partial a}=\frac {\sqrt{u_i^2+v_i^2+w_i^2}-r} {\partial a}\\ =\frac {\frac {[a(z_i-z_0)-c(x_i-x_0)]^2}{\partial a}}{2\sqrt{u_i^2+v_i^2+w_i^2}}\\ =\frac {-x_iz_i}{\sqrt{x_i^2+y_i^2}} ∂a∂fi=∂aui2+vi2+wi2−r=2ui2+vi2+wi2∂a[a(zi−z0)−c(xi−x0)]2=xi2+yi2−xizi
∂ f i ∂ b = u i 2 + v i 2 + w i 2 − r ∂ b = [ c ( y i − y 0 ) − b ( z i − z 0 ) ] 2 ∂ b 2 u i 2 + v i 2 + w i 2 = − y i z i x i 2 + y i 2 \frac {\partial f_i} {\partial b}=\frac {\sqrt{u_i^2+v_i^2+w_i^2}-r} {\partial b}\\ =\frac {\frac {[c(y_i-y_0)-b(z_i-z_0)]^2}{\partial b}}{2\sqrt{u_i^2+v_i^2+w_i^2}}\\ =\frac {-y_iz_i}{\sqrt{x_i^2+y_i^2}} ∂b∂fi=∂bui2+vi2+wi2−r=2ui2+vi2+wi2∂b[c(yi−y0)−b(zi−z0)]2=xi2+yi2−yizi
∂ f i ∂ r = u i 2 + v i 2 + w i 2 − r ∂ r = − 1 \frac {\partial f_i} {\partial r}=\frac {\sqrt{u_i^2+v_i^2+w_i^2}-r} {\partial r} = -1 ∂r∂fi=∂rui2+vi2+wi2−r=−1
-
确定圆初值
-
将中轴通过刚体变换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 x 0 p y 0 p z 0 p a p b p r ] ) = − D J \cdot \left(\begin {bmatrix}p_{x_0} \\ p_{y_0}\\ p_{z_0}\\p_a\\p_b\\p_{r} \end {bmatrix} \right)=-D J⋅ px0py0pz0papbpr =−D
-
求解 Δ p \Delta p Δp
-
更新解
[ x 0 y 0 z 0 ] = [ x 0 y 0 z 0 ] + U T ⋅ [ p x 0 p y 0 p z 0 ] [ a b c ] = U T ⋅ [ p a p b 1 ] . n o r m a l i z e ( ) r = r + p r \begin {array}{l}\\ \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_{x_0} \\ p_{y_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() \\\\ r=r+p_r \end {array} x0y0z0 = x0y0z0 +UT⋅ px0py0pz0 abc =UT⋅ papb1 .normalize()r=r+pr -
重复2直到收敛
初值确定
先对点进行拟合平面点击前往，在平面上拟合出2D圆点击前往。
代码实现
代码链接：https://gitcode.com/chenbb1989/3DAlgorithm/tree/master/CBB3DAlgorithm/Fitting
#include "CircleFitter.h"
#include "FittingPlane.h"
#include "FittingCircle2D.h"
#include <Eigen/Dense>
namespace Gauss {
double F(Fitting::Circle circle, const Point& p)
{
double ei = (p - circle.center).dot(circle.orient);
double fi = (p - circle.center).cross(circle.orient).norm() - circle.r;
return sqrt(ei * ei + fi * fi);
}
double GetError(Fitting::Circle 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 CircleFitter::Jacobi(const std::vector<Eigen::Vector3d>& points)
{
int n = points.size();
Fitting::Matrix J(2 * n, 6);
for (int i = 0; i < n; ++i) {
auto& p = points[i];
//ei求导
double ri = p.norm();
J(i, 0) = 0;
J(i, 1) = 0;
J(i, 2) = -1;
J(i, 3) = p.x();
J(i, 4) = p.y();
J(i, 5) = 0;
//fi求导
J(i + n, 0) = -p.x() / ri;
J(i + n, 1) = -p.y() / ri;
J(i + n, 2) = 0;
J(i + n, 3) = -p.x() * p.z() / ri;
J(i + n, 4) = -p.y() * p.z() / ri;
J(i + n, 5) = -1;
}
return J;
}
void CircleFitter::beforHook(const std::vector<Eigen::Vector3d>& points)
{
U = Fitting::getRotationByOrient(circle.orient);
for (int i = 0; i < points.size(); ++i) {
transPoints[i] = U * (points[i] - circle.center);
}
}
void CircleFitter::afterHook(const Eigen::VectorXd& xp)
{
circle.center += U.transpose() * Eigen::Vector3d(xp(0), xp(1), xp(2));
circle.orient = U.transpose() * Eigen::Vector3d(xp(3), xp(4), 1).normalized();
circle.r += xp(5);
}
Eigen::VectorXd CircleFitter::getDArray(const std::vector<Eigen::Vector3d>& points)
{
int n = points.size();
Eigen::VectorXd D(2*points.size());
for (int i = 0; i < points.size(); ++i) {
auto& p = points[i];
// ei
D(i) = p.z();
//fi
D(i + n) = p.norm() - circle.r;
}
return D;
}
bool CircleFitter::GetInitFit(const std::vector<Eigen::Vector3d>& points)
{
if (points.size() < 3)return false;
// 拟合平面
Fitting::Plane plane;
Fitting::FittingBase *fb = new FittingPlane();
fb->Fitting(points, &plane);
delete fb;
// 投影到平面，并旋转致xoy平面
U = Fitting::getRotationByOrient(plane.Orient);
transPoints.resize(points.size());
for (int i = 0; i < points.size(); ++i) {
auto p = points[i];
double d = (plane.BasePoint - p).dot(plane.Orient);
p += d * plane.Orient;
transPoints[i] = U * (p - plane.BasePoint);
}
// 拟合圆
Fitting::Circle2D circle2d;
fb = new FittingCircle2D();
fb->Fitting(transPoints, &circle2d);
delete fb;
// 旋转回来
circle.center = U.transpose() * Eigen::Vector3d(circle2d.center.x(), circle2d.center.y(), 0) + plane.BasePoint;
circle.orient = plane.Orient;
circle.r = circle2d.r;
return true;
}
double CircleFitter::F(const Eigen::Vector3d& p)
{
return Gauss::F(circle, p);
}
double CircleFitter::GetError(const std::vector<Eigen::Vector3d>& points)
{
return Gauss::GetError(circle, points);
}
void CircleFitter::Copy(void* ele)
{
memcpy(ele, &circle, sizeof(Fitting::Circle));
}
}
测试代码
#include "TestCircle.h"
#include "CircleFitter.h"
#include <iostream>
namespace Gauss {
double TestCircle::Fitting() {
Fitting::FittingBase* fb = new Gauss::CircleFitter();
auto err = fb->Fitting(points, &fitResult);
return err;
}
bool TestCircle::JudgeAnswer(FILE* fp) {
ReadAnswer();
if (!positionCmp(ans.center, fitResult.center))return false;
if (!orientationCmp(ans.orient, fitResult.orient))return false;
if (!radiusCmp(ans.r, fitResult.r))return false;
return true;
}
void TestCircle::ReadAnswer() {
vector<double> nums;
if (PointCloud::readNums((suffixName + "/answer/" + fileName + ".txt"), nums)) {
for (int i = 0; i < 3; ++i) ans.center[i] = nums[i];
for (int i = 0; i < 3; ++i) ans.orient[i] = nums[i+3];
ans.r = nums[6];
}
else {
std::cout << "read answer error" << std::endl;
}
}
void TestCircle::SaveAnswer(FILE* fp) {
writePoint(fp, fitResult.center);
writePoint(fp, fitResult.orient);
writeNumber(fp, fitResult.r);
}
}
测试结果
https://gitcode.com/chenbb1989/3DAlgorithm/blob/master/CBB3DAlgorithm/Fitting/gauss/fitting_result/result.txt
b09 : CIRCLE : pass
b10 : CIRCLE : pass
b11 : CIRCLE : pass
b12 : CIRCLE : pass
b13 : CIRCLE : pass
b14 : CIRCLE : pass
b15 : CIRCLE : pass
本人码农，希望通过自己的分享，让大家更容易学懂计算机知识。创作不易，帮忙点击公众号的链接。
