Coverage for trimesh/registration.py: 93%
403 statements
« prev ^ index » next coverage.py v7.17.0, created at 2026-11-12 17:18 +0000
« prev ^ index » next coverage.py v7.17.0, created at 2026-11-12 17:18 +0000
1"""
2registration.py
3---------------
5Functions for registering (aligning) point clouds with meshes.
6"""
8import numpy as np
10from . import bounds, transformations, util
11from .geometry import weighted_vertex_normals
12from .points import PointCloud, plane_fit
13from .transformations import transform_points
14from .triangles import angles, cross, normals
15from .typed import ArrayLike, Integer, Seed
17try:
18 import scipy.sparse as sparse
19 from scipy.spatial import cKDTree
20except BaseException as E:
21 # wrapping just ImportError fails in some cases
22 # will raise the error when someone tries to use KDtree
23 from . import exceptions
25 cKDTree = exceptions.ExceptionWrapper(E)
26 sparse = exceptions.ExceptionWrapper(E)
29# permutations of cube rotations
30# the principal inertia transform has arbitrary sign
31# along the 3 major axis so try all combinations of
32# 180 degree rotations with a quick first ICP pass
33_cube_diagonals = np.array(
34 [
35 [1, 1, 1, 1],
36 [1, 1, -1, 1],
37 [1, -1, 1, 1],
38 [-1, 1, 1, 1],
39 [-1, -1, 1, 1],
40 [-1, 1, -1, 1],
41 [1, -1, -1, 1],
42 [-1, -1, -1, 1],
43 ],
44 dtype=np.float64,
45)
48def mesh_other(
49 mesh,
50 other,
51 samples: Integer = 500,
52 scale: bool = False,
53 icp_first: Integer = 10,
54 icp_final: Integer = 50,
55 reflection: bool = True,
56 seed: Seed = None,
57 **kwargs,
58):
59 """
60 Align a mesh with another mesh or a PointCloud using
61 the principal axes of inertia as a starting point which
62 is refined by iterative closest point.
64 Parameters
65 ------------
66 mesh : trimesh.Trimesh object
67 Mesh to align with other
68 other : trimesh.Trimesh or (n, 3) float
69 Mesh or points in space
70 samples : int
71 Number of samples from mesh surface to align
72 scale : bool
73 Allow scaling in transform
74 icp_first : int
75 How many ICP iterations for the 9 possible
76 combinations of sign flippage
77 icp_final : int
78 How many ICP iterations for the closest
79 candidate from the wider search
80 seed : None or int
81 Seed the surface sampling this uses to pick key points:
82 pass an integer for deterministic results.
83 kwargs : dict
84 Passed through to `icp`, which passes through to `procrustes`
86 Returns
87 -----------
88 mesh_to_other : (4, 4) float
89 Transform to align mesh to the other object
90 cost : float
91 Average squared distance per point
92 """
94 def key_points(m, count):
95 """
96 Return a combination of mesh vertices and surface samples
97 with vertices chosen by likelihood to be important
98 to registration.
99 """
100 if len(m.vertices) < (count / 2):
101 return np.vstack((m.vertices, m.sample(count - len(m.vertices), seed=seed)))
102 else:
103 return m.sample(count, seed=seed)
105 if not util.is_instance_named(mesh, "Trimesh"):
106 raise ValueError("mesh must be Trimesh object!")
108 inverse = True
109 search = mesh
110 # if both are meshes use the smaller one for searching
111 if util.is_instance_named(other, "Trimesh"):
112 if len(mesh.vertices) > len(other.vertices):
113 # do the expensive tree construction on the
114 # smaller mesh and query the others points
115 search = other
116 inverse = False
117 points = key_points(m=mesh, count=samples)
118 points_mesh = mesh
119 else:
120 points_mesh = other
121 points = key_points(m=other, count=samples)
123 if points_mesh.is_volume:
124 points_PIT = points_mesh.principal_inertia_transform
125 else:
126 points_PIT = points_mesh.bounding_box_oriented.principal_inertia_transform
128 elif util.is_shape(other, (-1, 3)):
129 # case where other is just points
130 points = other
131 points_PIT = bounds.oriented_bounds(points)[0]
132 else:
133 raise ValueError("other must be mesh or (n, 3) points!")
135 # get the transform that aligns the search mesh principal
136 # axes of inertia with the XYZ axis at the origin
137 if search.is_volume:
138 search_PIT = search.principal_inertia_transform
139 else:
140 search_PIT = search.bounding_box_oriented.principal_inertia_transform
142 # transform that moves the principal axes of inertia
143 # of the search mesh to be aligned with the best- guess
144 # principal axes of the points
145 search_to_points = np.dot(np.linalg.inv(points_PIT), search_PIT)
146 if not reflection:
147 # drop the negative-determinant seeds — icp can only refine
148 # proper rotations so a reflected seed stays reflected, #2482
149 diagonals = _cube_diagonals[np.prod(_cube_diagonals, axis=1) > 0.0]
150 else:
151 diagonals = _cube_diagonals
153 # expand the diagonals into (n, 4, 4) transforms
154 cubes = np.eye(4) * diagonals[:, None, :]
156 # loop through permutations and run iterative closest point
157 costs = np.ones(len(cubes)) * np.inf
158 transforms = [None] * len(cubes)
159 centroid = search.centroid
161 for i, flip in enumerate(cubes):
162 # transform from points to search mesh
163 # flipped around the centroid of search
164 a_to_b = np.dot(
165 transformations.transform_around(flip, centroid),
166 np.linalg.inv(search_to_points),
167 )
169 # run first pass ICP
170 matrix, _junk, cost = icp(
171 a=points,
172 b=search,
173 initial=a_to_b,
174 max_iterations=int(icp_first),
175 scale=scale,
176 reflection=reflection,
177 **kwargs,
178 )
180 # save transform and costs from ICP
181 transforms[i] = matrix
182 costs[i] = cost
184 # run a final ICP refinement step
185 matrix, _junk, cost = icp(
186 a=points,
187 b=search,
188 initial=transforms[np.argmin(costs)],
189 max_iterations=int(icp_final),
190 scale=scale,
191 reflection=reflection,
192 **kwargs,
193 )
195 # convert to per- point distance average
196 cost /= len(points)
198 # we picked the smaller mesh to construct the tree
199 # on so we may have calculated a transform backwards
200 # to save computation, so just invert matrix here
201 if inverse:
202 mesh_to_other = np.linalg.inv(matrix)
203 else:
204 mesh_to_other = matrix
206 return mesh_to_other, cost
209def procrustes(
210 a: ArrayLike,
211 b: ArrayLike,
212 weights: ArrayLike | None = None,
213 reflection: bool = True,
214 translation: bool = True,
215 scale: bool = True,
216 return_cost: bool = True,
217):
218 """
219 Perform Procrustes' analysis to quickly align two corresponding
220 point clouds subject to constraints. This is much cheaper than
221 any other registration method but only applies if the two inputs
222 correspond in order.
224 Finds the transformation T mapping a to b which minimizes the
225 square sum distances between Ta and b, also called the cost.
227 Optionally filter the points in a and b via a binary weights array.
228 Non-uniform weights are supported and determine the optimal rotation.
230 Parameters
231 ----------
232 a : (n,3) float
233 List of points in space
234 b : (n,3) float
235 List of points in space
236 weights : (n,) float
237 List of floats representing how much weight is assigned to each point.
238 Binary entries can be used to filter the arrays; normalization is not required.
239 Translation and scaling are adjusted according to the weighting.
240 The rotation is also optimized according to the weighting.
241 reflection : bool
242 If the transformation is allowed reflections
243 translation : bool
244 If the transformation is allowed translation and rotation.
245 scale : bool
246 If the transformation is allowed scaling
247 return_cost : bool
248 Whether to return the cost and transformed a as well
250 Returns
251 ----------
252 matrix : (4,4) float
253 The transformation matrix sending a to b
254 transformed : (n,3) float
255 The image of a under the transformation
256 cost : float
257 The cost of the transformation
258 """
260 a_original = np.asanyarray(a, dtype=np.float64)
261 b_original = np.asanyarray(b, dtype=np.float64)
262 if not util.is_shape(a_original, (-1, 3)) or not util.is_shape(b_original, (-1, 3)):
263 raise ValueError("points must be (n,3)!")
264 if len(a_original) != len(b_original):
265 raise ValueError("a and b must contain same number of points!")
266 # weights are set to uniform if not provided.
267 if weights is None:
268 weights = np.ones(len(a_original))
269 w = np.maximum(np.asanyarray(weights, dtype=np.float64), 0)
270 if len(w) != len(a):
271 raise ValueError("weights must have same length as a and b!")
272 w_norm = (w / w.sum()).reshape((-1, 1))
274 # All zero entries are removed from further computations.
275 # If weights is a binary array, the optimal solution can still be found by
276 # simply removing the zero entries.
277 nonzero_weights = w_norm[:, 0] > 0.0
278 a_nonzero = a_original[nonzero_weights]
279 b_nonzero = b_original[nonzero_weights]
280 w_norm = w_norm[nonzero_weights]
282 # Remove translation component
283 if translation:
284 # centers are (weighted) averages of the individual points.
285 acenter = (a_nonzero * w_norm).sum(axis=0)
286 bcenter = (b_nonzero * w_norm).sum(axis=0)
287 else:
288 acenter = np.zeros(a_nonzero.shape[1])
289 bcenter = np.zeros(b_nonzero.shape[1])
291 # Remove scale component
292 if scale:
293 # scale is the square root of the (weighted) average of the
294 # squared difference between each point and the center.
295 ascale = np.sqrt((((a_nonzero - acenter) ** 2) * w_norm).sum())
296 bscale = np.sqrt((((b_nonzero - bcenter) ** 2) * w_norm).sum())
297 else:
298 ascale = 1
299 bscale = 1
301 # Use SVD to find optimal orthogonal matrix R
302 # constrained to det(R) = 1 if necessary.
304 # weight the covariance so the rotation is optimal for the weights
305 target = np.dot(
306 ((b_nonzero - bcenter) / bscale).T, ((a_nonzero - acenter) / ascale) * w_norm
307 )
309 u, _s, vh = np.linalg.svd(target)
311 if reflection:
312 R = np.dot(u, vh)
313 else:
314 # no reflection allowed, so determinant must be 1.0
315 R = np.dot(np.dot(u, np.diag([1, 1, np.linalg.det(np.dot(u, vh))])), vh)
317 # Compute our 4D transformation matrix encoding
318 # a -> (R @ (a - acenter)/ascale) * bscale + bcenter
319 # = (bscale/ascale)R @ a + (bcenter - (bscale/ascale)R @ acenter)
320 translation = bcenter - (bscale / ascale) * np.dot(R, acenter)
321 matrix = np.hstack((bscale / ascale * R, translation.reshape(-1, 1)))
322 matrix = np.vstack((matrix, np.array([0.0] * (a.shape[1]) + [1.0]).reshape(1, -1)))
324 if return_cost:
325 # Transform the original input array, including zero-weighted points
326 transformed = transform_points(a_original, matrix)
327 # The cost is the (weighted) sum of the euclidean distances between
328 # the transformed source points and the target points.
329 cost = (((b_nonzero - transformed[nonzero_weights]) ** 2) * w_norm).sum()
330 return matrix, transformed, cost
332 return matrix
335def icp(a, b, initial=None, threshold=1e-5, max_iterations=20, **kwargs):
336 """
337 Apply the iterative closest point algorithm to align a point cloud with
338 another point cloud or mesh. Will only produce reasonable results if the
339 initial transformation is roughly correct. Initial transformation can be
340 found by applying Procrustes' analysis to a suitable set of landmark
341 points (often picked manually).
343 Parameters
344 ----------
345 a : (n,3) float
346 List of points in space.
347 b : (m,3) float or Trimesh
348 List of points in space or mesh.
349 initial : (4,4) float
350 Initial transformation.
351 threshold : float
352 Stop when change in cost is less than threshold
353 max_iterations : int
354 Maximum number of iterations
355 kwargs : dict
356 Args to pass to procrustes
358 Returns
359 ----------
360 matrix : (4,4) float
361 The transformation matrix sending a to b
362 transformed : (n,3) float
363 The image of a under the transformation
364 cost : float
365 The cost of the transformation
366 """
368 a = np.asanyarray(a, dtype=np.float64)
369 if not util.is_shape(a, (-1, 3)):
370 raise ValueError("points must be (n,3)!")
372 if initial is None:
373 initial = np.eye(4)
375 is_mesh = util.is_instance_named(b, "Trimesh")
376 if not is_mesh:
377 b = np.asanyarray(b, dtype=np.float64)
378 if not util.is_shape(b, (-1, 3)):
379 raise ValueError("points must be (n,3)!")
380 btree = cKDTree(b)
382 # transform a under initial_transformation
383 a = transform_points(a, initial)
384 total_matrix = initial
386 # start with infinite cost
387 old_cost = np.inf
389 # avoid looping forever by capping iterations
390 for _ in range(max_iterations):
391 # Closest point in b to each point in a
392 if is_mesh:
393 closest, _distance, _faces = b.nearest.on_surface(a)
394 else:
395 _distances, ix = btree.query(a, 1)
396 closest = b[ix]
398 # align a with closest points
399 matrix, transformed, cost = procrustes(a=a, b=closest, **kwargs)
401 # update a with our new transformed points
402 a = transformed
403 total_matrix = np.dot(matrix, total_matrix)
405 if old_cost - cost < threshold:
406 break
407 else:
408 old_cost = cost
410 return total_matrix, transformed, cost
413def _normalize_by_source(source_mesh, target_geometry, target_positions):
414 # Utility function to put the source mesh in [-1, 1]^3 and transform
415 # target geometry accordingly
416 if not util.is_instance_named(target_geometry, "Trimesh") and not isinstance(
417 target_geometry, PointCloud
418 ):
419 vertices = np.asanyarray(target_geometry)
420 target_geometry = PointCloud(vertices)
421 centroid, scale = source_mesh.centroid, source_mesh.scale
422 source_mesh.vertices = (source_mesh.vertices - centroid[None, :]) / scale
423 # Dont forget to also transform the target positions
424 target_geometry.vertices = (target_geometry.vertices - centroid[None, :]) / scale
425 if target_positions is not None:
426 target_positions = (target_positions - centroid[None, :]) / scale
427 return target_geometry, target_positions, centroid, scale
430def _denormalize_by_source(
431 source_mesh, target_geometry, target_positions, result, centroid, scale
432):
433 # Utility function to transform source mesh from
434 # [-1, 1]^3 to its original transform
435 # and transform target geometry accordingly
436 source_mesh.vertices = scale * source_mesh.vertices + centroid[None, :]
437 target_geometry.vertices = scale * target_geometry.vertices + centroid[None, :]
438 if target_positions is not None:
439 target_positions = scale * target_positions + centroid[None, :]
440 if isinstance(result, list):
441 result = [scale * x + centroid[None, :] for x in result]
442 else:
443 result = scale * result + centroid[None, :]
444 return result
447def nricp_amberg(
448 source_mesh,
449 target_geometry,
450 source_landmarks=None,
451 target_positions=None,
452 steps=None,
453 eps=0.0001,
454 gamma=1,
455 distance_threshold=0.1,
456 return_records=False,
457 use_faces=True,
458 use_vertex_normals=True,
459 neighbors_count=8,
460):
461 """
462 Non Rigid Iterative Closest Points
464 Implementation of "Amberg et al. 2007: Optimal Step
465 Nonrigid ICP Algorithms for Surface Registration."
466 Allows to register non-rigidly a mesh on another or
467 on a point cloud. The core algorithm is explained
468 at the end of page 3 of the paper.
470 Comparison between nricp_amberg and nricp_sumner:
471 * nricp_amberg fits to the target mesh in less steps
472 * nricp_amberg can generate sharp edges
473 * only vertices and their neighbors are considered
474 * nricp_sumner tend to preserve more the original shape
475 * nricp_sumner parameters are easier to tune
476 * nricp_sumner solves for triangle positions whereas
477 nricp_amberg solves for vertex transforms
478 * nricp_sumner is less optimized when wn > 0
480 Parameters
481 ----------
482 source_mesh : Trimesh
483 Source mesh containing both vertices and faces.
484 target_geometry : Trimesh or PointCloud or (n, 3) float
485 Target geometry. It can contain no faces or be a PointCloud.
486 source_landmarks : (n,) int or ((n,) int, (n, 3) float)
487 n landmarks on the the source mesh.
488 Represented as vertex indices (n,) int.
489 It can also be represented as a tuple of triangle
490 indices and barycentric coordinates ((n,) int, (n, 3) float,).
491 target_positions : (n, 3) float
492 Target positions assigned to source landmarks
493 steps : Core parameters of the algorithm
494 Iterable of iterables (ws, wl, wn, max_iter,).
495 ws is smoothness term, wl weights landmark importance, wn normal importance
496 and max_iter is the maximum number of iterations per step.
497 eps : float
498 If the error decrease if inferior to this value, the current step ends.
499 gamma : float
500 Weight the translation part against the rotational/skew part.
501 Recommended value : 1.
502 distance_threshold : float
503 Distance threshold to account for a vertex match or not.
504 return_records : bool
505 If True, also returns all the intermediate results. It can help debugging
506 and tune the parameters to match a specific case.
507 use_faces : bool
508 If True and if target geometry has faces, use proximity.closest_point to find
509 matching points. Else use scipy's cKDTree object.
510 use_vertex_normals : bool
511 If True and if target geometry has faces, interpolate the normals of the target
512 geometry matching points.
513 Else use face normals or estimated normals if target geometry has no faces.
514 neighbors_count : int
515 number of neighbors used for normal estimation. Only used if target geometry has
516 no faces or if use_faces is False.
518 Returns
519 ----------
520 result : (n, 3) float or list[(n, 3) float]
521 The vertices positions of source_mesh such that it is registered non-rigidly
522 onto the target geometry.
523 If return_records is True, it returns the list of the vertex positions at each
524 iteration.
525 """
527 def _solve_system(M_kron_G, D, vertices_weight, nearest, ws, nE, nV, Dl, Ul, wl):
528 # Solve for Eq. 12
529 U = nearest * vertices_weight[:, None]
530 use_landmarks = Dl is not None and Ul is not None
531 A_stack = [ws * M_kron_G, D.multiply(vertices_weight[:, None])]
532 B_shape = (4 * nE + nV, 3)
533 if use_landmarks:
534 A_stack.append(wl * Dl)
535 B_shape = (4 * nE + nV + Ul.shape[0], 3)
536 A = sparse.csr_matrix(sparse.vstack(A_stack))
537 B = sparse.lil_matrix(B_shape, dtype=np.float32)
538 B[4 * nE : (4 * nE + nV), :] = U
539 if use_landmarks:
540 B[4 * nE + nV : (4 * nE + nV + Ul.shape[0]), :] = Ul * wl
541 X = sparse.linalg.spsolve(A.T * A, A.T * B).toarray()
542 return X
544 def _node_arc_incidence(mesh, do_weight):
545 # Computes node-arc incidence matrix of mesh (Eq.10)
546 nV = mesh.edges.max() + 1
547 nE = len(mesh.edges)
548 rows = np.repeat(np.arange(nE), 2)
549 cols = mesh.edges.flatten()
550 data = np.ones(2 * nE, np.float32)
551 data[1::2] = -1
552 if do_weight:
553 edge_lengths = np.linalg.norm(
554 mesh.vertices[mesh.edges[:, 0]] - mesh.vertices[mesh.edges[:, 1]], axis=-1
555 )
556 data *= np.repeat(1 / edge_lengths, 2)
557 return sparse.coo_matrix((data, (rows, cols)), shape=(nE, nV))
559 def _create_D(vertex_3d_data):
560 # Create Data matrix (Eq. 8)
561 nV = len(vertex_3d_data)
562 rows = np.repeat(np.arange(nV), 4)
563 cols = np.arange(4 * nV)
564 data = np.concatenate((vertex_3d_data, np.ones((nV, 1))), axis=-1).flatten()
565 return sparse.csr_matrix((data, (rows, cols)), shape=(nV, 4 * nV))
567 def _create_X(nV):
568 # Create Unknowns Matrix (Eq. 1)
569 X_ = np.concatenate((np.eye(3), np.array([[0, 0, 0]])), axis=0)
570 return np.tile(X_, (nV, 1))
572 def _create_Dl_Ul(D, source_mesh, source_landmarks, target_positions):
573 # Create landmark terms (Eq. 11)
574 Dl, Ul = None, None
576 if source_landmarks is None or target_positions is None:
577 # If no landmarks are provided, return None for both
578 return Dl, Ul
580 if isinstance(source_landmarks, tuple):
581 source_tids, source_barys = source_landmarks
582 source_tri_vids = source_mesh.faces[source_tids]
583 # u * x1, v * x2 and w * x3 combined
584 Dl = D[source_tri_vids.flatten(), :]
585 Dl.data *= source_barys.flatten().repeat(np.diff(Dl.indptr))
586 x0 = source_mesh.vertices[source_tri_vids[:, 0]]
587 x1 = source_mesh.vertices[source_tri_vids[:, 1]]
588 x2 = source_mesh.vertices[source_tri_vids[:, 2]]
589 Ul0 = (
590 target_positions
591 - x1 * source_barys[:, 1, None]
592 - x2 * source_barys[:, 2, None]
593 )
594 Ul1 = (
595 target_positions
596 - x0 * source_barys[:, 0, None]
597 - x2 * source_barys[:, 2, None]
598 )
599 Ul2 = (
600 target_positions
601 - x0 * source_barys[:, 0, None]
602 - x1 * source_barys[:, 1, None]
603 )
604 Ul = np.zeros((Ul0.shape[0] * 3, 3))
605 Ul[0::3] = Ul0 # y - v * x2 + w * x3
606 Ul[1::3] = Ul1 # y - u * x1 + w * x3
607 Ul[2::3] = Ul2 # y - u * x1 + v * x2
608 else:
609 Dl = D[source_landmarks, :]
610 Ul = target_positions
611 return Dl, Ul
613 target_geometry, target_positions, centroid, scale = _normalize_by_source(
614 source_mesh, target_geometry, target_positions
615 )
617 # Number of edges and vertices in source mesh
618 nE = len(source_mesh.edges)
619 nV = len(source_mesh.vertices)
621 # Initialize transformed vertices
622 transformed_vertices = source_mesh.vertices.copy()
623 # Node-arc incidence (M in Eq. 10)
624 M = _node_arc_incidence(source_mesh, True)
625 # G (Eq. 10)
626 G = np.diag([1, 1, 1, gamma])
627 # M kronecker G (Eq. 10)
628 M_kron_G = sparse.kron(M, G)
629 # D (Eq. 8)
630 D = _create_D(source_mesh.vertices)
631 # D but for normal computation from the transformations X
632 DN = _create_D(source_mesh.vertex_normals)
633 # Unknowns 4x3 transformations X (Eq. 1)
634 X = _create_X(nV)
635 # Landmark related terms (Eq. 11)
636 Dl, Ul = _create_Dl_Ul(D, source_mesh, source_landmarks, target_positions)
638 # Parameters of the algorithm (Eq. 6)
639 # order : Alpha, Beta, normal weighting, and max iteration for step
640 if steps is None:
641 steps = [
642 [0.01, 10, 0.5, 10],
643 [0.02, 5, 0.5, 10],
644 [0.03, 2.5, 0.5, 10],
645 [0.01, 0, 0.0, 10],
646 ]
647 if return_records:
648 records = [transformed_vertices]
650 # Main loop
651 for ws, wl, wn, max_iter in steps:
652 # If normals are estimated from points and if there are less
653 # than 3 points per query, avoid normal estimation
654 if not use_faces and neighbors_count < 3:
655 wn = 0
657 last_error = np.finfo(np.float32).max
658 error = np.finfo(np.float16).max
659 cpt_iter = 0
661 # Current step iterations loop
662 while last_error - error > eps and (max_iter is None or cpt_iter < max_iter):
663 qres = _from_mesh(
664 target_geometry,
665 transformed_vertices,
666 from_vertices_only=not use_faces,
667 return_normals=wn > 0,
668 return_interpolated_normals=wn > 0 and use_vertex_normals,
669 neighbors_count=neighbors_count,
670 )
672 # Data weighting
673 vertices_weight = np.ones(nV)
674 vertices_weight[qres["distances"] > distance_threshold] = 0
676 if wn > 0 and "normals" in qres:
677 target_normals = qres["normals"]
678 if use_vertex_normals and "interpolated_normals" in qres:
679 target_normals = qres["interpolated_normals"]
680 # Normal weighting = multiplying weights by cosines^wn
681 source_normals = DN * X
682 dot = util.diagonal_dot(source_normals, target_normals)
683 # Normal orientation is only known for meshes as target
684 dot = np.clip(dot, 0, 1) if use_faces else np.abs(dot)
685 vertices_weight = vertices_weight * dot**wn
687 # Actual system solve
688 X = _solve_system(
689 M_kron_G, D, vertices_weight, qres["nearest"], ws, nE, nV, Dl, Ul, wl
690 )
691 transformed_vertices = D * X
692 last_error = error
693 error_vec = np.linalg.norm(qres["nearest"] - transformed_vertices, axis=-1)
694 error = (error_vec * vertices_weight).mean()
695 if return_records:
696 records.append(transformed_vertices)
697 cpt_iter += 1
699 if return_records:
700 result = records
701 else:
702 result = transformed_vertices
704 result = _denormalize_by_source(
705 source_mesh, target_geometry, target_positions, result, centroid, scale
706 )
707 return result
710def _from_mesh(
711 mesh,
712 input_points,
713 from_vertices_only=False,
714 return_barycentric_coordinates=False,
715 return_normals=False,
716 return_interpolated_normals=False,
717 neighbors_count=10,
718 **kwargs,
719):
720 """
721 Find the the closest points and associated attributes from a Trimesh.
723 Parameters
724 -----------
725 mesh : Trimesh
726 Trimesh from which the query is performed
727 input_points : (m, 3) float
728 Input query points
729 from_vertices_only : bool
730 If True, consider only the vertices and not the faces
731 return_barycentric_coordinates : bool
732 If True, return the barycentric coordinates
733 return_normals : bool
734 If True, compute the normals at each closest point
735 return_interpolated_normals : bool
736 If True, return the interpolated normal at each closest point
737 neighbors_count : int
738 The number of closest neighbors to query
739 kwargs : dict
740 Dict to accept other key word arguments (not used)
741 Returns
742 ----------
743 qres : dict
744 Dictionary containing :
745 - nearest points (m, 3) with key 'nearest'
746 - distances to nearest point (m,) with key 'distances'
747 - support triangle indices of the nearest points (m,) with key 'tids'
748 - [optional] normals at nearest points (m,3) with key 'normals'
749 - [optional] barycentric coordinates in support triangles (m,3) with key
750 'barycentric_coordinates'
751 - [optional] interpolated normals (m,3) with key 'interpolated_normals'
752 """
753 input_points = np.asanyarray(input_points)
754 neighbors_count = min(neighbors_count, len(mesh.vertices))
756 if from_vertices_only or len(mesh.faces) == 0:
757 # Consider only the vertices
758 return _from_points(
759 mesh.vertices,
760 input_points,
761 mesh.kdtree,
762 return_normals=return_normals,
763 neighbors_count=neighbors_count,
764 )
765 # Else if we consider faces, use proximity.closest_point
766 qres = {}
767 from .proximity import closest_point
768 from .triangles import points_to_barycentric
770 qres["nearest"], qres["distances"], qres["tids"] = closest_point(mesh, input_points)
772 if return_normals:
773 qres["normals"] = mesh.face_normals[qres["tids"]]
774 if return_barycentric_coordinates or return_interpolated_normals:
775 qres["barycentric_coordinates"] = points_to_barycentric(
776 mesh.vertices[mesh.faces[qres["tids"]]], qres["nearest"]
777 )
779 if return_interpolated_normals:
780 # Interpolation from barycentric coordinates
781 qres["interpolated_normals"] = np.einsum(
782 "ij,ijk->ik",
783 qres["barycentric_coordinates"],
784 mesh.vertex_normals[mesh.faces[qres["tids"]]],
785 )
786 return qres
789def _from_points(
790 target_points,
791 input_points,
792 kdtree=None,
793 return_normals=False,
794 neighbors_count=10,
795 **kwargs,
796):
797 """
798 Find the the closest points and associated attributes
799 from a set of 3D points.
801 Parameters
802 -----------
803 target_points : (n, 3) float
804 Points from which the query is performed
805 input_points : (m, 3) float
806 Input query points
807 kdtree : scipy.cKDTree
808 KDTree used for query. Computed if not provided
809 return_normals : bool
810 If True, compute the normals at each nearest point
811 neighbors_count : int
812 The number of closest neighbors to query
813 kwargs : dict
814 Dict to accept other key word arguments (not used)
816 Returns
817 ----------
818 qres : dict
819 Dictionary containing :
820 - nearest points (m, 3) with key 'nearest'
821 - distances to nearest point (m,) with key 'distances'
822 - vertex indices of the nearest points (m,) with key 'vertex_indices'
823 - [optional] normals at nearest points (m,3) with key 'normals'
824 """
825 # Empty result
826 target_points = np.asanyarray(target_points)
827 input_points = np.asanyarray(input_points)
828 neighbors_count = min(neighbors_count, len(target_points))
829 qres = {}
830 if kdtree is None:
831 kdtree = cKDTree(target_points)
833 if return_normals:
834 assert neighbors_count >= 3
835 distances, indices = kdtree.query(input_points, k=neighbors_count)
836 nearest = target_points[indices, :]
837 qres["normals"] = plane_fit(nearest)[1]
838 qres["nearest"] = nearest[:, 0]
839 qres["distances"] = distances[:, 0]
840 qres["vertex_indices"] = indices[:, 0]
841 else:
842 qres["distances"], qres["vertex_indices"] = kdtree.query(input_points)
843 qres["nearest"] = target_points[qres["vertex_indices"], :]
845 return qres
848def nricp_sumner(
849 source_mesh,
850 target_geometry,
851 source_landmarks=None,
852 target_positions=None,
853 steps=None,
854 distance_threshold=0.1,
855 return_records=False,
856 use_faces=True,
857 use_vertex_normals=True,
858 neighbors_count=8,
859 face_pairs_type="vertex",
860):
861 """
862 Non Rigid Iterative Closest Points
864 Implementation of the correspondence computation part of
865 "Sumner and Popovic 2004: Deformation Transfer for Triangle Meshes"
866 Allows to register non-rigidly a mesh on another geometry.
868 Comparison between nricp_amberg and nricp_sumner:
869 * nricp_amberg fits to the target mesh in less steps
870 * nricp_amberg can generate sharp edges
871 * only vertices and their neighbors are considered
872 * nricp_sumner tend to preserve more the original shape
873 * nricp_sumner parameters are easier to tune
874 * nricp_sumner solves for triangle positions whereas
875 nricp_amberg solves for vertex transforms
876 * nricp_sumner is less optimized when wn > 0
878 Parameters
879 ----------
880 source_mesh : Trimesh
881 Source mesh containing both vertices and faces.
882 target_geometry : Trimesh or PointCloud or (n, 3) float
883 Target geometry. It can contain no faces or be a PointCloud.
884 source_landmarks : (n,) int or ((n,) int, (n, 3) float)
885 n landmarks on the the source mesh.
886 Represented as vertex indices (n,) int.
887 It can also be represented as a tuple of triangle indices and barycentric
888 coordinates ((n,) int, (n, 3) float,).
889 target_positions : (n, 3) float
890 Target positions assigned to source landmarks
891 steps : Core parameters of the algorithm
892 Iterable of iterables (wc, wi, ws, wl, wn).
893 wc is the correspondence term (strength of fitting), wi is the identity term
894 (recommended value : 0.001), ws is smoothness term, wl weights the landmark
895 importance and wn the normal importance.
896 distance_threshold : float
897 Distance threshold to account for a vertex match or not.
898 return_records : bool
899 If True, also returns all the intermediate results. It can help debugging
900 and tune the parameters to match a specific case.
901 use_faces : bool
902 If True and if target geometry has faces, use proximity.closest_point to find
903 matching points. Else use scipy's cKDTree object.
904 use_vertex_normals : bool
905 If True and if target geometry has faces, interpolate the normals of the target
906 geometry matching points.
907 Else use face normals or estimated normals if target geometry has no faces.
908 neighbors_count : int
909 number of neighbors used for normal estimation. Only used if target geometry has
910 no faces or if use_faces is False.
911 face_pairs_type : str 'vertex' or 'edge'
912 Method to determine face pairs used in the smoothness cost. 'vertex' yields
913 smoother results.
916 Returns
917 ----------
918 result : (n, 3) float or list[(n, 3) float]
919 The vertices positions of source_mesh such that it is registered non-rigidly
920 onto the target geometry.
921 If return_records is True, it returns the list of the vertex positions at each
922 iteration.
923 """
925 def _construct_transform_matrix(faces, Vinv, size):
926 # Utility function for constructing the per-frame transforms
927 _construct_transform_matrix._row = np.array([0, 1, 2] * 4)
928 nV = len(Vinv)
929 rows = np.tile(_construct_transform_matrix._row, nV) + 3 * np.repeat(
930 np.arange(nV), 12
931 )
932 cols = np.repeat(faces.flat, 3)
933 minus_inv_sum = -Vinv.sum(axis=1)
934 Vinv_flat = Vinv.reshape(nV, 9)
935 data = np.concatenate((minus_inv_sum, Vinv_flat), axis=-1).flatten()
936 return sparse.coo_matrix((data, (rows, cols)), shape=(3 * nV, size), dtype=float)
938 def _build_tetrahedrons(mesh):
939 # UUtility function for constructing the frames
940 v4_vec = mesh.face_normals
941 v1 = mesh.triangles[:, 0]
942 v2 = mesh.triangles[:, 1]
943 v3 = mesh.triangles[:, 2]
944 v4 = v1 + v4_vec
945 vertices = np.concatenate((mesh.vertices, v4))
946 nV, nT = len(mesh.vertices), len(mesh.faces)
947 v4_indices = np.arange(nV, nV + nT)[:, None]
948 tetrahedrons = np.concatenate((mesh.faces, v4_indices), axis=-1)
949 frames = np.concatenate(
950 ((v2 - v1)[..., None], (v3 - v1)[..., None], v4_vec[..., None]), axis=-1
951 )
952 return vertices, tetrahedrons, frames
954 def _construct_identity_cost(vtet, tet, Vinv):
955 # Utility function for constructing the identity cost
956 AEi = _construct_transform_matrix(
957 tet,
958 Vinv,
959 len(vtet),
960 ).tocsr()
961 Bi = np.tile(np.identity(3, dtype=float), (len(tet), 1))
962 return AEi, Bi
964 def _construct_smoothness_cost(vtet, tet, Vinv, face_pairs):
965 # Utility function for constructing the smoothness (stiffness) cost
966 AEs_r = _construct_transform_matrix(
967 tet[face_pairs[:, 0]], Vinv[face_pairs[:, 0]], len(vtet)
968 ).tocsr()
969 AEs_l = _construct_transform_matrix(
970 tet[face_pairs[:, 1]], Vinv[face_pairs[:, 1]], len(vtet)
971 ).tocsr()
972 AEs = (AEs_r - AEs_l).tocsc()
973 AEs.eliminate_zeros()
974 Bs = np.zeros((len(face_pairs) * 3, 3))
975 return AEs, Bs
977 def _construct_landmark_cost(vtet, source_mesh, source_landmarks):
978 # Utility function for constructing the landmark cost
979 if source_landmarks is None:
980 return None, np.ones(len(source_mesh.vertices), dtype=bool)
981 if isinstance(source_landmarks, tuple):
982 # If the input source landmarks are in barycentric form
983 source_landmarks_tids, source_landmarks_barys = source_landmarks
984 source_landmarks_vids = source_mesh.faces[source_landmarks_tids]
985 nL, nVT = len(source_landmarks_tids), len(vtet)
987 rows = np.repeat(np.arange(nL), 3)
988 cols = source_landmarks_vids.flat
989 data = source_landmarks_barys.flat
991 AEl = sparse.coo_matrix((data, (rows, cols)), shape=(nL, nVT))
992 marker_vids = source_landmarks_vids[
993 source_landmarks_barys > np.finfo(np.float16).eps
994 ]
995 non_markers_mask = np.ones(len(source_mesh.vertices), dtype=bool)
996 non_markers_mask[marker_vids] = False
997 else:
998 # Else if they are in vertex index form
999 nL, nVT = len(source_landmarks), len(vtet)
1000 rows = np.arange(nL)
1001 cols = source_landmarks.flat
1002 data = np.ones(nL)
1003 AEl = sparse.coo_matrix((data, (rows, cols)), shape=(nL, nVT))
1004 non_markers_mask = np.ones(len(source_mesh.vertices), dtype=bool)
1005 non_markers_mask[source_landmarks] = False
1006 return AEl, non_markers_mask
1008 def _construct_correspondence_cost(points, non_markers_mask, size):
1009 # Utility function for constructing the correspondence cost
1010 AEc = sparse.identity(size, dtype=float, format="csc")[: len(non_markers_mask)]
1011 AEc = AEc[non_markers_mask]
1012 Bc = points[non_markers_mask]
1013 return AEc, Bc
1015 def _compute_vertex_normals(vertices, faces):
1016 # Utility function for computing source vertex normals
1017 mesh_triangles = vertices[faces]
1018 mesh_triangles_cross = cross(mesh_triangles)
1019 mesh_face_normals = normals(
1020 triangles=mesh_triangles, crosses=mesh_triangles_cross
1021 )[0]
1022 mesh_face_angles = angles(mesh_triangles)
1023 mesh_normals = weighted_vertex_normals(
1024 vertex_count=nV,
1025 faces=faces,
1026 face_normals=mesh_face_normals,
1027 face_angles=mesh_face_angles,
1028 )
1029 return mesh_normals
1031 # First, normalize the source and target to [-1, 1]^3
1032 (target_geometry, target_positions, centroid, scale) = _normalize_by_source(
1033 source_mesh, target_geometry, target_positions
1034 )
1035 nV = len(source_mesh.vertices)
1036 use_landmarks = source_landmarks is not None and target_positions is not None
1038 if steps is None:
1039 steps = [
1040 # [wc, wi, ws, wl, wn],
1041 [1, 0.001, 1.0, 1000, 0],
1042 [1, 0.001, 1.0, 1000, 0],
1043 [10, 0.001, 1.0, 1000, 0],
1044 [100, 0.001, 1.0, 1000, 0],
1045 ]
1047 source_vtet, source_tet, V = _build_tetrahedrons(source_mesh)
1048 Vinv = np.linalg.inv(V)
1050 # List of (n, 2) faces index which share a vertex
1051 if face_pairs_type == "vertex":
1052 face_pairs = source_mesh.face_neighborhood
1053 else:
1054 face_pairs = source_mesh.face_adjacency
1056 # Construct the cost matrices
1057 # Identity cost (Eq. 12)
1058 AEi, Bi = _construct_identity_cost(source_vtet, source_tet, Vinv)
1059 # Smoothness cost (Eq. 11)
1060 AEs, Bs = _construct_smoothness_cost(source_vtet, source_tet, Vinv, face_pairs)
1061 # Landmark cost (Eq. 13)
1062 AEl, non_markers_mask = _construct_landmark_cost(
1063 source_vtet, source_mesh, source_landmarks
1064 )
1066 transformed_vertices = source_vtet.copy()
1067 if return_records:
1068 records = [transformed_vertices[:nV]]
1070 # Main loop
1071 for i, (wc, wi, ws, wl, wn) in enumerate(steps):
1072 Astack = [AEi * wi, AEs * ws]
1073 Bstack = [Bi * wi, Bs * ws]
1075 if use_landmarks:
1076 Astack.append(AEl * wl)
1077 Bstack.append(target_positions * wl)
1079 if (i > 0 or not use_landmarks) and wc > 0:
1080 # Query the nearest points
1081 qres = _from_mesh(
1082 target_geometry,
1083 transformed_vertices[:nV],
1084 from_vertices_only=not use_faces,
1085 return_normals=wn > 0,
1086 return_interpolated_normals=(use_vertex_normals and wn > 0),
1087 neighbors_count=neighbors_count,
1088 )
1090 # Correspondence cost (Eq. 13)
1091 AEc, Bc = _construct_correspondence_cost(
1092 qres["nearest"], non_markers_mask, len(source_vtet)
1093 )
1094 vertices_weight = np.ones(nV)
1095 vertices_weight[qres["distances"] > distance_threshold] = 0
1096 if wn > 0 or "normals" in qres:
1097 target_normals = qres["normals"]
1098 if use_vertex_normals and "interpolated_normals" in qres:
1099 target_normals = qres["interpolated_normals"]
1100 # Normal weighting : multiplying weights by cosines^wn
1101 source_normals = _compute_vertex_normals(
1102 transformed_vertices, source_mesh.faces
1103 )
1104 dot = util.diagonal_dot(source_normals, target_normals)
1105 # Normal orientation is only known for meshes as target
1106 dot = np.clip(dot, 0, 1) if use_faces else np.abs(dot)
1107 vertices_weight = vertices_weight * dot**wn
1109 # Account for vertices' weight
1110 AEc.data *= vertices_weight[non_markers_mask][AEc.indices]
1111 Bc *= vertices_weight[non_markers_mask, None]
1113 Astack.append(AEc * wc)
1114 Bstack.append(Bc * wc)
1116 # Now solve Eq. 14 ...
1117 A = sparse.vstack(Astack, format="csc")
1118 A.eliminate_zeros()
1119 b = np.concatenate(Bstack)
1121 LU = sparse.linalg.splu((A.T * A).tocsc())
1122 transformed_vertices = LU.solve(A.T * b)
1123 # done !
1125 if return_records:
1126 records.append(transformed_vertices[:nV])
1128 if return_records:
1129 result = records
1130 else:
1131 result = transformed_vertices[:nV]
1133 result = _denormalize_by_source(
1134 source_mesh, target_geometry, target_positions, result, centroid, scale
1135 )
1136 return result