三维空间点拟合圆原理及C++实现

已知三维空间离散点坐标(xi, yi, zi)，构建一个空间圆使得空间点尽可能靠近拟合的空间圆。首先，所有离散点尽可能在一个平面上，平面方程可表示为(1)写成矩阵形式为，，式中，，(...

GL_logn

8294人浏览 · 2020-09-01 14:27:36

GL_logn · 2020-09-01 14:27:36 发布

已知三维空间离散点坐标(xi, yi, zi)，构建一个空间圆使得空间点尽可能靠近拟合的空间圆。

首先，所有离散点尽可能在一个平面上，平面方程可表示为

$ax+by+cz-1=0$ (1)

写成矩阵形式为，

$MA=L_1$ ，式中

$M=\begin{bmatrix} x1& x2& ...& xn\\ y1& y2& ...& yn\\ z1& z2& ... &zn \end{bmatrix}^{T}$ ， $A=(a,b,c)^{T}$ ， $L_1=(1, 1, ..., 1)^{T}$ (2)

这是一个超定方程求解，根据最小二乘法，可以求出 $A=(M^{T}M)^{-1}M^{T}L_1$ ，即平面的法向向量。

假设所有离散点都在圆上，那么任意两点的连线的中垂线必过圆心。设圆心C（x0，y0，z0），取两个点P1（x1，y1，z1）与P2（x2，y2，z2），则P1和P2连线的向量vector1表示为（x2-x1，y2-y1，z2-z1），P1和P2连线的中点坐标P12为 $(\frac{x1+x2}{2},\frac{y1+y2}{2},\frac{z1+z2}{2},)$

圆心C与P12连线向量vector2为 $(\frac{x1+x2}{2}-x0,\frac{y1+y2}{2}-y0,\frac{z1+z2}{2}-z0,)$ 。要想P1与P2在圆上，则满足vector1*vecotor2=0，即

$(x2-x1,y2-y1,z2-z1)\cdot (\frac{x1+x2}{2}-x0,\frac{y1+y2}{2}-y0,\frac{z1+z2}{2}-z0)=0$

整理一下，有

$\Delta x_{12}\cdot x_{0}+\Delta y_{12}\cdot y_{0}+\Delta z_{12}\cdot z_{0}-l_{1}=0$ ,

式中 $\Delta x_{12}=x_{2}-x_{1},\Delta y_{12}=y_{2}-y_{1},\Delta z_{12}=z_{2}-z_{1}$ , $l_{1}=\frac{x_{2}^{2}+y_{2}^{2}+z_{2}^{2}-x_{1}^{2}-y_{1}^{2}-z_{1}^{2}}{2}$

所有点都在圆上，则有

(3)

写成矩阵形式 $BC=L_2$ ，

式中，，

上述方程亦为超定方程，变换成适定形式为

(4)

由于圆心C必在前述控制的平面内，因此满足 $ax_0+by_0+cz_0-1=0$ , 即

(5)

A为平面的法向量，通过 $A=(M^{T}M)^{-1}M^{T}L_1$ （6）已求出，因而可将式(4)和式(5)合并，写成一个扩展式进行求解

，式中

，

则求解圆心坐标 $C=(D^{T}D)^{-1}D^{T}L_3$

圆的半径可由所有点到圆心的距离的平均值确定：

C++代码实现如下：

vector<float> FitCircle(std::vector<vector<float>> pts)
{
vector<float> circle;

int num = pts.size();
int dim = 3;

Eigen::MatrixXd M(num, dim);

   for (int i = 0; i < num; i++)
   {
       for (int j = 0; j < dim; j++)
       {
           M(i, j) = pts[i][j];
       }
   }

Eigen::MatrixXd L1 = Eigen::MatrixXd::Ones(num, 1);

//式（6）
Eigen::MatrixXd A = (M.transpose()*M).inverse()*M.transpose()*L1;

Eigen::MatrixXd B = Eigen::MatrixXd::Zero(num - 1, 3);

   for (int i = 0; i < num - 1; i++)
   {
       B.row(i) = M.row(i + 1) - M.row(i);
   }

   Eigen::MatrixXd L2 = Eigen::MatrixXd::Zero(num - 1, 1);
   for (int i = 0; i < num - 1; i++)
   {
       L2(i) = (M(i + 1, 0)*M(i + 1, 0) + M(i + 1, 1)*M(i + 1, 1) + M(i + 1, 2)*M(i + 1, 2)
           - (M(i, 0)*M(i, 0) + M(i, 1)*M(i, 1) + M(i, 2)*M(i, 2))) / 2.0;
   }

   Eigen::MatrixXd D;
   //！！！矩阵合并前需要将合并后的矩阵 resize
   D.resize(4, 3);
   D << B.transpose()*B,
       A.transpose();

   Eigen::MatrixXd L3;
   Eigen::MatrixXd One31 = Eigen::MatrixXd::Ones(3, 1);
   L3.resize(4, 1);
   L3 << B.transpose()*L2,
       One31;

   //式（7）
   Eigen::MatrixXd C = (D.transpose()*D).inverse()*D.transpose() * L3;

   //式（8）
   double radius = 0;
   for (int i = 0; i < num; i++)
   {
       Eigen::MatrixXd tmp = M.row(i) - C.transpose();
       radius = radius + sqrt(tmp(0)*tmp(0) + tmp(1)*tmp(1) + tmp(2)*tmp(2));
   }
   radius = radius / num;

   circle.push_back(C(0));
   circle.push_back(C(1));
   circle.push_back(C(2));
   circle.push_back(A(0));
   circle.push_back(A(1));
   circle.push_back(A(2));
   circle.push_back(radius);
   return circle;
}