def icp_numpy(source, target, tol=1e-3):
"""Align two point clouds using the Iterative Closest Point (ICP) method.
Parameters
----------
source : list of point
The source data.
target : list of point
The target data.
tol : float, optional
Tolerance for finding matches.
Default is ``1e-3``.
Returns
-------
The transformed points
Notes
-----
First we align the source with the target cloud using the frames resulting
from a PCA of each of the clouds, simply by calculating a frame-to-frame transformation.
This initial alignment is used to establish an initial correspondence between
the points of the two clouds.
Then we iteratively improve the alignment by computing successive "best-fit"
transformations using SVD of the cross-covariance matrix of the two data sets.
During this iterative process, we continuously update the correspondence
between the point clouds by finding the closest point in the target to each
of the source points.
The algorithm terminates when the alignment error is below a specified tolerance.
Examples
--------
>>>
"""
A = asarray(source)
B = asarray(target)
origin, axes, _ = pca_numpy(A)
A_frame = Frame(origin, axes[0], axes[1])
origin, axes, _ = pca_numpy(B)
B_frame = Frame(origin, axes[0], axes[1])
X = Transformation.from_frame_to_frame(A_frame, B_frame)
A = transform_points_numpy(A, X)
for i in range(20):
D = cdist(A, B, 'euclidean')
closest = argmin(D, axis=1)
if norm(normrow(A - B[closest])) < tol:
break
X = bestfit_transform(A, B[closest])
A = transform_points_numpy(A, X)
return A, X
def bestfit_plane_numpy(points):
"""Fit a plane through more than three (non-coplanar) points.
Parameters
----------
points : list
XYZ coordinates of the points.
Returns
-------
tuple
A point on the plane, and the normal vector.
Examples
--------
>>>
"""
# xyz = asarray(points).reshape((-1, 3))
# n = xyz.shape[0]
# c = (sum(xyz, axis=0) / n).reshape((-1, 3))
# Yt = xyz - c
# C = Yt.T.dot(Yt) / (n - 1)
# U, S, Vt = svd(C)
# w = Vt[2, :]
# return c, w
o, uvw, _ = pca_numpy(points)
return o, uvw[2]
def bestfit_circle_numpy(points):
"""Fit a circle through a set of points.
Parameters
----------
points : list
XYZ coordinates of the points.
Returns
-------
tuple
XYZ coordinates of the center of the circle, the normal vector of the
local frame, and the radius of the circle.
Notes
-----
For more information see [1]_.
References
----------
.. [1] Scipy. *Least squares circle*.
Available at: http://scipy-cookbook.readthedocs.io/items/Least_Squares_Circle.html.
Examples
--------
.. code-block:: python
#
"""
o, uvw, _ = pca_numpy(points)
rst = local_coords_numpy(o, uvw, points)
x = rst[:, 0]
y = rst[:, 1]
def dist(xc, yc):
return sqrt((x - xc) ** 2 + (y - yc) ** 2)
def f(c):
Ri = dist(*c)
return Ri - Ri.mean()
xm = mean(x)
ym = mean(y)
c0 = xm, ym
c, ier = leastsq(f, c0)
Ri = dist(*c)
R = Ri.mean()
residu = sum((Ri - R) ** 2)
print(residu)
xyz = global_coords_numpy(o, uvw, [[c[0], c[1], 0.0]])[0]
o = xyz.tolist()
u, v, w = uvw.tolist()
return o, w, R
def oabb_numpy(points):
origin, (xaxis, yaxis, zaxis), values = pca_numpy(points)
frame = Frame(origin, xaxis, yaxis)
world = Frame.worldXY()
X = Transformation.from_frame_to_frame(frame, world)
points = transform_points_numpy(points, X)
bbox = bounding_box(points)
bbox = transform_points_numpy(bbox, X.inverse())
return bbox
def bestfit_frame_numpy(points):
"""Fit a frame to a set of points.
Parameters
----------
points : list
XYZ coordinates of the points.
Returns
-------
3-tuple
The frame origin, and the local X and Y axes.
Examples
--------
>>>
"""
o, uvw, _ = pca_numpy(points)
return o, uvw[0], uvw[1]
def bestfit_plane_numpy(points):
"""Fit a plane through more than three (non-coplanar) points.
Parameters
----------
points : list
XYZ coordinates of the points.
Returns
-------
tuple
A point on the plane, and the normal vector.
Examples
--------
>>>
"""
o, uvw, _ = pca_numpy(points)
return o, uvw[2]
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.
Examples
--------
>>>
"""
o, uvw, _ = pca_numpy(points)
return o, uvw[2]
def oabb_numpy(points):
"""Oriented bounding box of a set of points.
Parameters
----------
points : array_like[point]
XYZ coordinates of the points.
Returns
-------
list[[float, float, float]]
XYZ coordinates of 8 points defining a box.
"""
origin, (xaxis, yaxis, zaxis), values = pca_numpy(points)
frame = Frame(origin, xaxis, yaxis)
world = Frame.worldXY()
X = Transformation.from_frame_to_frame(frame, world)
points = transform_points_numpy(points, X)
bbox = bounding_box(points)
bbox = transform_points_numpy(bbox, X.inverse())
return bbox
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.
Examples
--------
>>>
"""
o, uvw, _ = pca_numpy(points)
return o, uvw[0], uvw[1]
from compas.plotters import Axes3D from compas.plotters import Cloud3D from compas.plotters import Bounds from compas.plotters import create_axes_3d from compas.numerical import pca_numpy data = random.rand(300, 3) data[:, 0] *= 10.0 data[:, 1] *= 1.0 data[:, 2] *= 4.0 a = 3.14159 * 30.0 / 180 Ry = matrix_from_axis_and_angle([0, 1.0, 0.0], a, rtype='array') a = -3.14159 * 45.0 / 180 Rz = matrix_from_axis_and_angle([0, 0, 1.0], a, rtype='array') R = Rz.dot(Ry) data = transform_numpy(data, R) average, vectors, values = pca_numpy(data) axes = create_axes_3d() Bounds(data).plot(axes) Cloud3D(data).plot(axes) Axes3D(average, vectors).plot(axes) plt.show()
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.
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.
Examples
--------
>>>
"""
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_plane_numpy4(points):
from compas.numerical import pca_numpy
c, (_, _, w), _ = pca_numpy(points)
return c, w
