Say I have an infinite cylinder that can be defined by its axis $\pmb v$, radius $r$ and a center point $\pmb c$ indicating its position.
Now, say I have many points in 3D and I want to find the model $(\pmb v, \pmb c, r)$ that best fits these points. Does anyone have a simple explanation or straight up implementation of such a method? Also, what is the best method to do this if my points are noisy?
On
Given a set of $n$ points $\{r_i\}$, where $r_i = (x_i, y_i, z_i)$ , you want to find the best fit right circular cylinder to these points.
The equation of a cylinder is
$ \sqrt{ ( r - C)^T (I - {a a}^T ) (r - C) } = R $
where $r = (x, y, z)$, $C$ is a point on the axis of the cylinder, $a$ is a unit vector along the axis, and $R$ is the radius.
From this equation we can define the error function of the estimate of the cylinder parameters as follows
$ E = \displaystyle \sum_{i=1}^n \varepsilon_i^2 $
where
$ \varepsilon_i = \sqrt{ ( r - C)^T (I - {a a}^T ) (r - C) } - R $
Now to specify the variables involved: The unit vector $a$ is specified using spherical coordinates as follows:
$a = (\sin \theta \cos \phi , \sin \theta \sin \phi, \cos \theta ) $
Since $C$ can be any point on the axis, we will create two unit vectors make with $a$ an orthonormal basis that is a function only of $\theta$ and $\phi$. The basis is $[u_1, u_2, a] $ where
$ u_1 = (\cos \theta \cos \phi, \cos \theta \sin \phi, - \sin \theta) $
$ u_2 = ( - \sin \phi, \cos \phi , 0 ) $
Now the point $C$ can be uniquely specified as
$ C = \alpha u_1 + \beta u_2 $
And finally we have $R$ the radius. Therefore,
$\varepsilon_i = E(r_i,\theta, \phi, \alpha, \beta, R) $
We want to minimize the function $E$, and for that we can use any optimization method. I used the simple steepest descent method (with some modification to determine the step size).
The only step needed is to derive the gradient vector of $E$. We have
$ \dfrac{\partial E}{\partial \theta} = \displaystyle \sum_{i=1}^n 2 \varepsilon_i \dfrac{\partial \varepsilon_i}{\partial \theta} $
Now, define
$ Q_i = \sqrt{(r_i - C)^T (I - {a a }^T ) (r_i - C)} $
Then, using basic vector calculus,
$\dfrac{\partial \varepsilon_i}{\partial \theta} = \dfrac{1}{Q_i} \left( - (r_i - C)^T (I - {aa}^T) \dfrac{\partial C}{\partial \theta} - (r_i - C)^T a \left( \dfrac{ \partial a}{\partial \theta} \right)^T (r_i - C) \right) $
Similar expressions can be derived for the partial derivatives of $E$ with respect to the rest of the variables.
I've implemented this with a simple program. First I created a data set that lies exactly on a cylinder. The steepest descent method converged to the correct parameters in about 100 iterations.
Then I added noise to the points, by adding a variable increment/decrement of the radius by $ \pm 5 \%$ and then by $\pm 10 \%$. The program still converged to a solution with parameters very close to their true values (less than $1 \%$, most of the time).
Here's the complete listing of the program written in (Microsoft Excel Visual Basic for Applications script).
Public Sub best_fit_right_circular_cylinder()
Dim x(5), y(5), jac(5, 6) As Double
Dim points(100, 3) As Double
Dim p0(3), p1(3) As Double
Dim a(3), u1(3), u2(3) As Double
Dim x_(5), y_(5), dx(5), dy(5) As Double
' Create the poitns
r = 7
dr = 0.1 * r
Call initialize_rd
Call rd_xyz_1(1, 30, 25, 50)
Call rd_xyz_2(1, 3, 5, 0)
Call calculate_xyz_(1, p0, p1)
For i = 1 To 3
ActiveSheet.Cells(i, 10) = p1(i)
ActiveSheet.Cells(i, 9) = ra(1, i, 3)
Next i
n = 30
For ipoint = 1 To n
' use cylinderical coordinates
phi = WorksheetFunction.RandBetween(0, 360) * p / 180
z = WorksheetFunction.RandBetween(-20, 20)
fraction = 1 / 10 * WorksheetFunction.RandBetween(-10, 10)
p0(1) = (r + fraction * dr) * Cos(phi)
p0(2) = (r + fraction * dr) * Sin(phi)
p0(3) = z
Call calculate_xyz_(1, p0, p1)
For i = 1 To 3
points(ipoint, i) = p1(i)
Next i
Next ipoint
' X = [th, phi, u1, u2, R ]
x(1) = 0
x(2) = 0.5
x(3) = 0
x(4) = 0
x(5) = 1
success = 0
For ic = 1 To 5000
Call find_f_best_fit_cylinder(n, points, x, y)
If norm(y, 5) < 0.000001 Then
MsgBox ("Converged in " + Str(ic - 1))
success = 1
Exit For
End If
If ic = 1 Then
gamma = 0.01
Else
For i = 1 To 5
dx(i) = x(i) - x_(i)
dy(i) = y(i) - y_(i)
Next i
If norm(dx, 5) < 0.000000001 Then
success = 1
MsgBox ("converged in " + Str(ic - 1))
Exit For
End If
dynormsq = norm(dy, 5) ^ 2
gamma = Abs(dotn(dx, dy, 5)) / dynormsq
End If
For i = 1 To 5
x_(i) = x(i)
x(i) = x(i) - gamma * y(i)
y_(i) = y(i)
Next i
Next ic
If success = 0 Then
MsgBox ("Steepest descent did not converge")
Exit Sub
End If
For i = 1 To 5
ActiveSheet.Cells(i, 1) = x(i)
Next i
th = x(1)
phi = x(2)
a(1) = Sin(th) * Cos(phi)
a(2) = Sin(th) * Sin(phi)
a(3) = Cos(th)
u1(1) = Cos(th) * Cos(phi)
u1(2) = Cos(th) * Sin(phi)
u1(3) = -Sin(th)
u2(1) = -Sin(phi)
u2(2) = Cos(phi)
u2(3) = 0
For i = 1 To 3
p1(i) = u1(i) * x(3) + u2(i) * x(4)
ActiveSheet.Cells(i, 2) = a(i)
ActiveSheet.Cells(i, 3) = p1(i)
Next i
ActiveSheet.Cells(1, 4) = x(5)
End Sub
Public Sub find_f_best_fit_cylinder(n, points, x, y)
Dim e1 As Double, e As Double
Dim r As Double, a(3) As Double, q(3, 3), u1(3), u2(3) As Double
Dim c(3), u(3), v(3) As Double
Dim f As Double
Dim q1(3, 3), v1(3), v2(3) As Double
Dim ath(3), aphi(3), u1th(3), u1phi(3), u2th(3), u2phi(3), cth(3), cphi(3) As Double
th = x(1)
phi = x(2)
a(1) = Sin(th) * Cos(phi)
a(2) = Sin(th) * Sin(phi)
a(3) = Cos(th)
u1(1) = Cos(th) * Cos(phi)
u1(2) = Cos(th) * Sin(phi)
u1(3) = -Sin(th)
u2(1) = -Sin(phi)
u2(2) = Cos(phi)
u2(3) = 0
For i = 1 To 3
ath(i) = u1(i)
aphi(i) = Sin(th) * u2(i)
u1th(i) = -a(i)
u1phi(i) = Cos(th) * u2(i)
u2th(i) = 0
Next i
u2phi(1) = -Cos(phi)
u2phi(2) = -Sin(phi)
u2phi(3) = 0
For i = 1 To 3
For j = 1 To 3
If i = j Then q(i, j) = 1 - a(i) * a(j) Else q(i, j) = -a(i) * a(j)
Next j
Next i
For i = 1 To 5
y(i) = 0
Next i
For ipoint = 1 To n
For i = 1 To 3
c(i) = x(3) * u1(i) + x(4) * u2(i)
Next i
For i = 1 To 3
u(i) = points(ipoint, i) - c(i)
Next i
f = Sqr(xtqy(3, q, u, u))
e1 = f - x(5)
' d/dth ( (ri - C)^T (I - a a^T) (ri - C) = 2 [(I - a a^T) (ri - C)]^T [ - dC/dth ] + (ri - C)^T ( - 2 a (ath)^T ) (ri - C)
' similarly for d/dphi
' d/dx3 =2 [Q (r - C)]^T (- u1 ) , d/dx4 = 2 [Q (r - C)]^T (- u2)
Call multiplyv(3, 3, q, u, v)
For i = 1 To 3
cth(i) = x(3) * u1th(i) + x(4) * u2th(i)
cphi(i) = x(3) * u1phi(i) + x(4) * u2phi(i)
Next i
y(1) = y(1) - dot(v, cth) * e1 / f
For i = 1 To 3
For j = 1 To 3
q1(i, j) = a(i) * ath(j)
Next j
Next i
y(1) = y(1) - 2 * xtqy(3, q1, u, u) * e1 / f
y(2) = y(2) - dot(v, cphi) * e1 / f
For i = 1 To 3
For j = 1 To 3
q1(i, j) = a(i) * aphi(j)
Next j
Next i
y(2) = y(2) - 2 * xtqy(3, q1, u, u) * e1 / f
y(3) = y(3) - dot(v, u1) * e1 / f
y(4) = y(4) - dot(v, u2) * e1 / f
y(5) = y(5) - 2 * e1
Next ipoint
End Sub
Public Sub multiplyv(n, M, a, b, c)
Dim i, j, k As Integer
Dim c1(200) As Double
For i = 1 To n
c1(i) = 0
For k = 1 To M
c1(i) = c1(i) + a(i, k) * b(k)
Next k
Next i
For i = 1 To n
c(i) = c1(i)
Next i
End Sub
Public Function dot(x, y) As Double
dot = x(1) * y(1) + x(2) * y(2) + x(3) * y(3)
End Function
Public Function dotn(x, y, n) As Double
Dim s As Double
s = 0
For i = 1 To n
s = s + x(i) * y(i)
Next i
dotn = s
End Function
Public Function xtqy(n, q, x, y) As Double
Dim temp(10) As Double
Dim a As Double
Call multiplyrv(n, n, x, q, temp)
a = 0
For i = 1 To n
a = a + temp(i) * y(i)
Next i
xtqy = a
End Function
Public Sub multiplyrv(n, M, a, b, c)
Dim i, j, k As Integer
Dim c1(100) As Double
For i = 1 To M
c1(i) = 0
For k = 1 To n
c1(i) = c1(i) + a(k) * b(k, i)
Next k
Next i
For i = 1 To M
c(i) = c1(i)
Next i
End Sub
Public Sub initialize_rd()
Dim i, j, k As Integer
Dim r(3, 3), d(3) As Double
For i = 1 to 3
d(i) = 0
For j = 1 to 3
If i = j Then r(i, j) = 1 Else r(i, j) = 0
Next j
Next i
For k = 0 To 200
For i = 1 To 3
For j = 1 To 3
ra(k, i, j) = r(i, j)
Next j
da(k, i) = d(i)
Next i
da(k, 0) = 1
Next k
End Sub
Public Sub rd_xyz_1(ByVal index As Integer, ByVal theta As Double, ByVal phi As Double, ByVal beta As Double)
Dim r1(3, 3) As Double
Dim rproduct(3, 3) As Double
Dim rtheta, rphi, rbeta As Double
Dim k As Integer
p1 = WorksheetFunction.Pi()
rtheta = p1 * theta / 180
rphi = p1 * phi / 180
rbeta = p1 * beta / 180
c1 = Cos(rtheta)
c2 = Cos(rphi)
c3 = Cos(rbeta)
s1 = Sin(rtheta)
s2 = Sin(rphi)
s3 = Sin(rbeta)
r1(1, 1) = c3 * c2 * c1 - s3 * s2
r1(2, 1) = c3 * s2 * c1 + s3 * c2
r1(3, 1) = -c3 * s1
r1(1, 2) = -s3 * c2 * c1 - c3 * s2
r1(2, 2) = -s3 * s2 * c1 + c3 * c2
r1(3, 2) = s3 * s1
r1(1, 3) = s1 * c2
r1(2, 3) = s1 * s2
r1(3, 3) = c1
For i = 1 To 3
For j = 1 To 3
rproduct(i, j) = 0
For k = 1 To 3
rproduct(i, j) = rproduct(i, j) + ra(index, i, k) * r1(k, j)
Next k
Next j
Next i
For i = 1 To 3
For j = 1 To 3
ra(index, i, j) = rproduct(i, j)
Next j
Next i
End Sub
Public Sub rd_xyz_2(ByVal index As Integer, ByVal dx As Double, ByVal dy As Double, ByVal dz As Double)
Dim d1(3), dproduct(3) As Double
Dim k As Integer
d1(1) = dx
d1(2) = dy
d1(3) = dz
For i = 1 To 3
dproduct(i) = 0
For k = 1 To 3
dproduct(i) = dproduct(i) + ra(index, i, k) * d1(k)
Next k
Next i
For i = 1 To 3
da(index, i) = da(index, i) + dproduct(i)
Next i
End Sub
Public Sub calculate_xyz_(ByVal ndx As Integer, xi, xo)
For i = 1 To 3
xo(i) = da(ndx, i)
For j = 1 To 3
xo(i) = xo(i) + ra(ndx, i, j) * xi(j)
Next j
Next i
End Sub
The distance between a point $\mathbf{x}$ and the cylinder is (assuming $\lvert\mathbf{v}\rvert=1$): $$ \left\lvert\sqrt{[\mathbf{x}-\mathbf{c}-(\mathbf{x}- \mathbf{c})\times \mathbf{v}]^2}-r\right\rvert. $$ So we get the naive least square: $$ L(\mathbf{v},\mathbf{c},r;\lambda)=\sum_i (\sqrt{[\mathbf{x}_i-\mathbf{c}-(\mathbf{x}_i-\mathbf{c})\times \mathbf{v}]^2}-r)^2-\lambda_1(\mathbf{v}^2-1)-\lambda_2\mathbf{c}\cdot\mathbf{v} $$ But you need to square root etc. We could try instead $$ L(\mathbf{v},\mathbf{c},r;\lambda)=\sum_i ([\mathbf{x}_i-\mathbf{c}-(\mathbf{x}_i-\mathbf{c})\times \mathbf{v}]^2-r^2)^2-\lambda_1(\mathbf{v}^2-1)-\lambda_2\mathbf{c}\cdot\mathbf{v} $$ at the expense of making it quartic in $\mathbf{x},\mathbf{c},\mathbf{v}$. Neither versions have analytic solution so you need some numerical methods.
Anyway, symmetry suggests we should expect the best-fit axis to pass through $\bar{\mathbf{x}}$. Diagonalise the second central moment tensor (with equal weights $\frac1n$ on the points) $$ \frac1n\sum_i (\mathbf{x}_i-\bar{\mathbf{x}})\otimes(\mathbf{x}_i-\bar{\mathbf{x}})=P\begin{pmatrix}\sigma_1\\&\sigma_2\\&&\sigma_3\end{pmatrix}P^{-1} $$ If we have sampled enough points uniformly on a long cylinder (length $L\gg r$), then $\mathbf{v}$ should correspond to the eigendirection of largest eigenvalue (say $\sigma_1$) and the other two eigenvalues $\sigma_2,\sigma_3$ should be approximately equal (you expect the eigenvalues to be around $\frac1{12}L^2,\frac12r^2,\frac12r^2$). Hence we get $r^2$ as, e.g., $\sigma_2+\sigma_3$ or $2\sqrt{\sigma_2\sigma_3}$. You could use this as the starting point for numerical methods.