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 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 mesh_transform_numpy(mesh, transformation):
"""Transform a mesh.
Parameters
----------
mesh : compas.datastructures.Mesh
The mesh.
transformation : compas.geometry.Transformation
The transformation.
Notes
-----
The mesh is modified in-place.
Examples
--------
>>> mesh = Mesh.from_obj(compas.get('cube.obj'))
>>> T = matrix_from_axis_and_angle([0, 0, 1], pi / 4)
>>> tmesh = mesh.copy()
>>> mesh_transform(tmesh, T)
"""
vertices = [mesh.vertex_coordinates(key) for key in mesh.vertices()]
xyz = transform_points_numpy(vertices, transformation)
for index, (_, attr) in enumerate(mesh.vertices(True)):
attr['x'] = xyz[index][0]
attr['y'] = xyz[index][1]
attr['z'] = xyz[index][2]
def mesh_transform_numpy(mesh, transformation):
"""Transform a mesh.
Parameters
----------
mesh : compas.datastructures.Mesh
The mesh.
transformation : compas.geometry.Transformation
The transformation.
Notes
-----
The mesh is modified in-place.
Examples
--------
>>> mesh = Mesh.from_obj(compas.get('cube.obj'))
>>> T = matrix_from_axis_and_angle([0, 0, 1], pi / 4)
>>> tmesh = mesh.copy()
>>> mesh_transform(tmesh, T)
"""
vertices = list(mesh.vertices())
xyz = [mesh.vertex_coordinates(vertex) for vertex in vertices]
xyz[:] = transform_points_numpy(xyz, transformation)
for index, vertex in enumerate(vertices):
mesh.vertex_attributes(vertex, 'xyz', xyz[index])
def mesh_transform_numpy(mesh, transformation):
"""Transform a mesh.
Parameters
----------
mesh : :class:`compas.datastructures.Mesh`
The mesh.
transformation : :class:`compas.geometry.Transformation`
The transformation.
Returns
-------
None
The mesh is modified in-place.
Examples
--------
>>> from compas.datastructures import Mesh
>>> from compas.geometry import matrix_from_axis_and_angle
>>> mesh = Mesh.from_polyhedron(6)
>>> T = matrix_from_axis_and_angle([0, 0, 1], math.pi / 4)
>>> tmesh = mesh.copy()
>>> mesh_transform_numpy(tmesh, T)
"""
vertices = list(mesh.vertices())
xyz = [mesh.vertex_coordinates(vertex) for vertex in vertices]
xyz[:] = transform_points_numpy(xyz, transformation)
for index, vertex in enumerate(vertices):
mesh.vertex_attributes(vertex, 'xyz', xyz[index])
def icp_numpy(d1, d2, tol=1e-3):
"""Align two point clouds using the Iterative Closest Point (ICP) method.
Parameters
----------
d1 : list of point
Point cloud 1.
d2 : list of point
Point cloud 2.
tol : float, optional
Tolerance for finding matches.
Default is ``1e-3``.
Returns
-------
Notes
-----
Examples
--------
References
----------
"""
d1 = asarray(d1)
d2 = asarray(d2)
point, axes, spread = pca_numpy(d1)
frame1 = Frame(point, axes[0], axes[1])
point, axes, spread = pca_numpy(d2)
frame2 = Frame(point, axes[0], axes[1])
T = Transformation.from_frame_to_frame(frame1, frame2)
transform_points_numpy(d1, T)
y = cdist(d1, d2, 'eucledian')
closest = argmin(y, axes=1)
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
from compas_plotters import Axes3D
from compas_plotters import Cloud3D
from compas_plotters import Bounds
from compas_plotters import create_axes_3d
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_points_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 transform(self, X):
transform_points_numpy(self._data, X)
import time
import compas
from compas.datastructures import Mesh
from compas.geometry import Rotation
from compas.geometry import transform_points
from compas.geometry import transform_points_numpy
# load mesh
mesh = Mesh.from_ply(compas.get('bunny.ply'))
v, f = mesh.to_vertices_and_faces()
print("The mesh has {} vertices.".format(len(v)))
# create Transformation
T = Rotation.from_axis_and_angle([-0.248, -0.786, -0.566],
2.78,
point=[1.0, 0.0, 0.0])
# transform points with transform_points
t0 = time.time()
transform_points(v, T)
print("transfrom_points takes {:.4f} seconds.".format(time.time() - t0))
# transform points with transform_points_numpy
t0 = time.time()
transform_points_numpy(v, T)
print("transfrom_points_numpy takes {:.4f} seconds.".format(time.time() - t0))
from compas.geometry import transform_points_numpy
from compas.geometry import allclose
points = numpy.random.rand(10000, 3)
bottom = numpy.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0],
[1.0, 1.0, 0.0]])
top = numpy.array([[0.0, 0.0, 1.0], [1.0, 0.0, 1.0], [0.0, 1.0, 1.0],
[1.0, 1.0, 1.0]])
points = numpy.concatenate((points, bottom, top))
points[:, 0] *= 10
points[:, 2] *= 3
bbox = bounding_box(points)
a = length_vector(subtract_vectors(bbox[1], bbox[0]))
b = length_vector(subtract_vectors(bbox[3], bbox[0]))
c = length_vector(subtract_vectors(bbox[4], bbox[0]))
v1 = a * b * c
R = Rotation.from_axis_and_angle([1.0, 1.0, 0.0], 0.5 * 3.14159)
points = transform_points_numpy(points, R.matrix)
bbox = oriented_bounding_box_numpy(points)
a = length_vector(subtract_vectors(bbox[1], bbox[0]))
b = length_vector(subtract_vectors(bbox[3], bbox[0]))
c = length_vector(subtract_vectors(bbox[4], bbox[0]))
v2 = a * b * c
print(v1, v2)
print(allclose([v1], [v2]))
def transform(self, T):
"""Transform the triangle mesh."""
self.vertices[:] = transform_points_numpy(self.vertices, T)