Least Squares Fit Cylindrical

This article was originally created by CSDN Dianyunxia, ​​and the original text link . If you didn't see this article in Dianyunxia's blog, then this is a shameless crawler.

1. Algorithm principle

  From the geometric properties of the cylindrical surface, the distance from the point on the cylindrical surface to its axis is always equal to the radius $r_0$,As shown in Figure 1.

  Among them, $P$ is any point on the cylindrical surface,$P_0(x_0，y_0，z_0)$ is a point on the cylinder axis,$(a,b,c)$ is the cylinder axis vector
,$r_0$is the radius of the base circle of the cylinder. The distance from any point on the cylindrical surface to its axis is the radius $r_0$，即:
$$(x-x_0{)}^2+(y-y_0{)}^2+(z-z_0{)}^2-[a(x-x_0)+b(y-y_0)+c(z-z_0){]}^2=r_0^2\text{\hspace{1em}(1)}$$

  Equation (1) is the cylindrical surface equation. According to the coordinates $(x,y,z)$ , find a point on the cylinder axis$(x_0，y_0，z_0)$ , cylinder axis vector$(a,b,c)$ , the radius of the cylinder bottom circle$r_0$These seven parameters can uniquely determine a cylinder.

The detailed steps of the cylinder fitting method are as follows:
(1) Determine the initial values ​​of the cylinder model parameters. The estimation principle of the initial value of cylinder fitting is as follows:

  Search for several adjacent points of any point on the cylindrical surface, fit these points into a plane, and normalize the obtained plane normal vector, which is the unit normal vector of the point. Process each point on the cylindrical surface to obtain the unit normal vector of each point. Treat the unit normal vector of each point as a point, and fit these points into a plane to obtain the plane normal vector, that is, the initial value of the cylinder axis vector $a_0、b_0、c_0$, this step uses principal component analysis to solve.
  First perform coordinate transformation on the cylinder so that the cylinder axis vector $(a_0，b_0，c_0)$ into parallel$The\; vector\; of\; the\; Z$ axis is$(0，0，\sqrt{(a_0{)}^2+(b_0{)}^2+(c_0{)}^2})$ . Let the rotation matrix be${Ｒ}_{3\times 3}$, to make:


  the rotation matrix with the coordinates ( x , y , z ) of a point on the cylinder $(x,y,z)$ to get the new coordinates$(x^{\prime },y^{\prime },z^{\prime })$x ′ , y ′ x',y'in the new coordinates$x^{\prime },y^{’}$ Get the coordinates of the bottom surface of the cylinder in the new coordinate system, according to$(x^{\prime },y^{’})$can fit a circle.
  The equation of a circle is$(x-x_1^{0^{\prime }}{)}^2+(y-y_1^{0^{\prime }}{)}^2=(r_0{)}^{2If}$ you know the coordinates of the center of the circle$x_1^{0^{\prime }},y_1^{0^{\prime }}$, radius $r_0$These three parameters can uniquely determine a circular surface.
  First, the circular coordinates $(x^{\prime },y^{\prime })$is ready; secondly, find the error equation, expand the equation of the circle to get$x^2+(x_1^{0^{\prime }}{)}^2+y^2+(y_1^{0^{\prime }}{)}^2-2y{y}_1^{0^{\prime }}-(r_0{)}^2=0$，即$2x(x_1^{0^{\prime }})+2y\left(y_1^{0^{\prime }}\right)+(r_0{)}^2-(x_1^{0^{\prime }}{)}^2-(y_1^{0^{\prime }}{)}^2-(x^2+y^2)=0$。可以设$x_1^{0^{\prime }}、y_1^{0^{\prime }}、(r_0{)}^2-(x_1^{0^{\prime }}{)}^2-(y_1^{0^{\prime }}{)}^2$ is the parameter to be requested. Then the error equation is:$V=2x(x_1^{0^{\prime }})+2y\left(y_1^{0^{\prime }}\right)+(r_0{)}^2-(x_1^{0^{\prime }}{)}^2-(y_1^{0^{\prime }}{)}^2-(x^2+y^2)$, written in the form of matrix:

  Assume that the obtained points of the circle are of equal precision, that is, of equal weight. According to the principle of linear least squares, that is, $V^{T}V=min$ , get the position parameter$X$：
$$X=(B^{T}B{)}^{-1}{B}^{T}L\text{\hspace{1em}( 5 )}$$ Calculate the initial value x 1 0 ′, y 1 0 ′ x_1^{0'},y_1^{0'}
  of a point on the axis of the cylinder in the new coordinate system$x_1^{0^{\prime }},y_1^{0^{\prime }}$And the initial value of the radius of the bottom circle $r_0$, and then according to the formula: x 1 0 ′ , y 1 0 ′ x_1^{0'},y_1^{0'}

  in the new coordinate system$x_1^{0^{\prime }},y_1^{0^{\prime }}$Convert to coordinates x 1 0 , y 1 0 x_1^{0},y_1^{0} in the old coordinate system$x_1^0,y_1^0$. Whether in the new coordinate system or in the old coordinate system, the initial value of the radius of the bottom circle $r_0$No change. In order to minimize the error, the initial value of a point on the cylinder axis $z_1^0$The value of is set to the cylinder point zz in the original coordinate systemHalf of the sum of the maximum and minimum values ​​of$z\; .$Then calculate the initial values ​​of the four parameters$x_1^0,y_1^0,z_1^0,r_0$.
  (2) Establish an improved error equation to solve the parameter values. The equation of the cylindrical surface is:
$$(x-x_1{)}^2+(y-y_1{)}^2+(z-z_1{)}^2-[a(x-x_1)+b(y-y_1)+c(z-z_1){]}^2=r^2\text{\hspace{1em}(7)}$$
传统方法，令
$$f=(x-x_1{)}^2+(y-y_1{)}^2+(z-z_1{)}^2-[a(x-x_1)+b(y-y_1)+c(z-z_1){]}^2-r^2\text{\hspace{1em}( 8 )}$$
to$f$ is linearized

  like this, the error equation can be listed as:

where




$$V_{n\times 1}=B_{n\times 7}{X}_{7\times 1}-L_{n\times 1}\text{\hspace{1em}(23)}$$

  Assume that the obtained points on the cylindrical surface are of equal precision, that is, the weights are equal. According to the principle of least squares, the parameter
$$X=(B^{T}B{)}^{-1}{B}^{T}L\text{\hspace{1em}( 24 )}$$
  This is a cyclic iterative process, and the initial value substituted in each iterative calculation process is equal to the previous initial value plus the obtained$The\; correction\; value\; of\; X$ , when$The\; iteration\; is\; launched\; when\; the\; value\; of\; X$ is small enough to meet the required accuracy.

2. Code implementation