/
bestfit_numpy.py
259 lines (205 loc) · 7.41 KB
/
bestfit_numpy.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
from numpy import asarray
from numpy import sqrt
from numpy import zeros
from numpy.linalg import lstsq
from scipy.optimize import leastsq
from compas.geometry import pca_numpy
from compas.geometry import world_to_local_coordinates_numpy
from compas.geometry import local_to_world_coordinates_numpy
def bestfit_line_numpy(points):
"""Fit a line through a set of points.
Parameters
----------
points : array_like[point]
XYZ coordinates of the points.
Returns
-------
[float, float, float]
XYZ coordinates of a point on the line.
[float, float, float]
The direction vector of the line.
Raises
------
ValueError
If the number of points is smaller than the dimensionality of the points.
At least two points are needed for two-dimensional data.
At least three points are needed for three-dimensional data.
See Also
--------
:func:`compas.geometry.bestfit_plane_numpy`, :func:`compas.geometry.bestfit_frame_numpy`
:func:`compas.geometry.bestfit_circle_numpy`, :func:`compas.geometry.bestfit_sphere_numpy`
"""
o, uvw, _ = pca_numpy(points)
return o, uvw[0]
def bestfit_plane_numpy(points):
"""Fit a plane through more than three (non-coplanar) points.
Parameters
----------
points : array_like[point]
XYZ coordinates of the points.
Returns
-------
[float, float, float]
A point on the plane.
[float, float, float]
The normal vector.
Raises
------
ValueError
If the number of points is smaller than the dimensionality of the points.
At least two points are needed for two-dimensional data.
At least three points are needed for three-dimensional data.
See Also
--------
:func:`compas.geometry.bestfit_line_numpy`, :func:`compas.geometry.bestfit_frame_numpy`
:func:`compas.geometry.bestfit_circle_numpy`, :func:`compas.geometry.bestfit_sphere_numpy`
:func:`compas.geometry.bestfit_plane`
"""
o, uvw, _ = pca_numpy(points)
return o, uvw[2]
def bestfit_frame_numpy(points):
"""Fit a frame to a set of points.
Parameters
----------
points : array_like[point]
XYZ coordinates of the points.
Returns
-------
[float, float, float]
The frame origin.
[float, float, float]
The local X axis.
[float, float, float]
The local Y axis.
Raises
------
ValueError
If the number of points is smaller than the dimensionality of the points.
At least two points are needed for two-dimensional data.
At least three points are needed for three-dimensional data.
See Also
--------
:func:`compas.geometry.bestfit_line_numpy`, :func:`compas.geometry.bestfit_plane_numpy`
:func:`compas.geometry.bestfit_circle_numpy`, :func:`compas.geometry.bestfit_sphere_numpy`
"""
o, uvw, _ = pca_numpy(points)
return o, uvw[0], uvw[1]
def bestfit_circle_numpy(points):
"""Fit a circle through a set of points.
Parameters
----------
points : array_like[point]
XYZ coordinates of the points.
Returns
-------
[float, float, float]
XYZ coordinates of the center of the circle.
[float, float, float]
The normal vector of the local frame.
float
The radius of the circle.
Raises
------
ValueError
If the number of points is smaller than the dimensionality of the points.
At least two points are needed for two-dimensional data.
At least three points are needed for three-dimensional data.
See Also
--------
:func:`compas.geometry.bestfit_line_numpy`, :func:`compas.geometry.bestfit_plane_numpy`, :func:`compas.geometry.bestfit_frame_numpy`
:func:`compas.geometry.bestfit_sphere_numpy`
Notes
-----
The point of this function is to find the bestfit frame through the given points
and transform the points to make the problem 2D.
Once in 2D, the problem simplifies to finding the center point that minimises
the difference between the resulting circles for all given points, i.e.
minimise in the least squares sense the deviation between the individual
radii and the average radius.
For more information see [1]_.
References
----------
.. [1] Scipy. *Least squares circle*.
Available at: http://scipy-cookbook.readthedocs.io/items/Least_Squares_Circle.html.
"""
o, uvw, _ = pca_numpy(points)
frame = [o, uvw[0], uvw[1]]
rst = world_to_local_coordinates_numpy(frame, points)
x = rst[:, 0]
y = rst[:, 1]
def dist(xc, yc):
# compute the radius of the circle through each of the given points
# for the current centre point
return sqrt((x - xc) ** 2 + (y - yc) ** 2)
def f(c):
# compute the deviation of the radius of each sample point
# from the average radius
# => minimize this deviation
Ri = dist(*c)
return Ri - Ri.mean()
# The mean of x and y are very nearly 0 (1.0e-15), which reveals a numerical
# instability of the problem. So, we choose our initial guess
# to be an epsilon bigger than that.
xm = ym = 0.00001
c0 = xm, ym
c, error = leastsq(f, c0)
# compute the radius of the circle through each sample point for the
# computed center point.
Ri = dist(*c)
# compute the radius of the bestfit circle as the average of the individual
# radii.
R = Ri.mean()
# residu = sum((Ri - R) ** 2)
# print(residu)
# convert the location of the center point back to global coordinates.
xyz = local_to_world_coordinates_numpy(frame, [[c[0], c[1], 0.0]])[0]
return xyz, uvw[2], R
def bestfit_sphere_numpy(points):
"""Returns the sphere's center and radius that fits best through a set of points.
Parameters
----------
points: array_like[point]
XYZ coordinates of the points.
Returns
-------
[float, float, float]
Sphere center (XYZ coordinates).
float
Sphere radius.
See Also
--------
:func:`compas.geometry.bestfit_line_numpy`, :func:`compas.geometry.bestfit_plane_numpy`, :func:`compas.geometry.bestfit_frame_numpy`
:func:`compas.geometry.bestfit_circle_numpy`
Notes
-----
For more information see [1]_.
Examples
--------
>>> from compas.geometry import bestfit_sphere_numpy
>>> points = [(291.580, -199.041, 120.194), (293.003, -52.379, 33.599),\
(514.217, 26.345, 29.143), (683.253, 26.510, -6.194),\
(683.247, -327.154, 179.113), (231.606, -430.659, 115.458),\
(87.278, -419.178, -18.863), (24.731, -340.222, -127.158)]
>>> center, radius = bestfit_sphere_numpy(points)
References
----------
.. [1] Least Squares Sphere Fit.
Available at: https://jekel.me/2015/Least-Squares-Sphere-Fit/.
"""
# Assemble the A matrix
spX = asarray([p[0] for p in points])
spY = asarray([p[1] for p in points])
spZ = asarray([p[2] for p in points])
A = zeros((len(spX), 4))
A[:, 0] = spX * 2
A[:, 1] = spY * 2
A[:, 2] = spZ * 2
A[:, 3] = 1
# Assemble the f matrix
f = zeros((len(spX), 1))
f[:, 0] = (spX * spX) + (spY * spY) + (spZ * spZ)
C, residules, rank, singval = lstsq(A, f)
# solve for the radius
t = (C[0] * C[0]) + (C[1] * C[1]) + (C[2] * C[2]) + C[3]
radius = sqrt(t)
return [float(C[0][0]), float(C[1][0]), float(C[2][0])], radius