Coverage for trimesh/registration.py: 93%

403 statements  

« prev     ^ index     » next       coverage.py v7.17.0, created at 2026-11-12 17:18 +0000

1""" 

2registration.py 

3--------------- 

4 

5Functions for registering (aligning) point clouds with meshes. 

6""" 

7 

8import numpy as np 

9 

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 

16 

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 

24 

25 cKDTree = exceptions.ExceptionWrapper(E) 

26 sparse = exceptions.ExceptionWrapper(E) 

27 

28 

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) 

46 

47 

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. 

63 

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` 

85 

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 """ 

93 

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) 

104 

105 if not util.is_instance_named(mesh, "Trimesh"): 

106 raise ValueError("mesh must be Trimesh object!") 

107 

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) 

122 

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 

127 

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!") 

134 

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 

141 

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 

152 

153 # expand the diagonals into (n, 4, 4) transforms 

154 cubes = np.eye(4) * diagonals[:, None, :] 

155 

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 

160 

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 ) 

168 

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 ) 

179 

180 # save transform and costs from ICP 

181 transforms[i] = matrix 

182 costs[i] = cost 

183 

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 ) 

194 

195 # convert to per- point distance average 

196 cost /= len(points) 

197 

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 

205 

206 return mesh_to_other, cost 

207 

208 

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. 

223 

224 Finds the transformation T mapping a to b which minimizes the 

225 square sum distances between Ta and b, also called the cost. 

226 

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. 

229 

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 

249 

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 """ 

259 

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)) 

273 

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] 

281 

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]) 

290 

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 

300 

301 # Use SVD to find optimal orthogonal matrix R 

302 # constrained to det(R) = 1 if necessary. 

303 

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 ) 

308 

309 u, _s, vh = np.linalg.svd(target) 

310 

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) 

316 

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))) 

323 

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 

331 

332 return matrix 

333 

334 

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). 

342 

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 

357 

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 """ 

367 

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)!") 

371 

372 if initial is None: 

373 initial = np.eye(4) 

374 

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) 

381 

382 # transform a under initial_transformation 

383 a = transform_points(a, initial) 

384 total_matrix = initial 

385 

386 # start with infinite cost 

387 old_cost = np.inf 

388 

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] 

397 

398 # align a with closest points 

399 matrix, transformed, cost = procrustes(a=a, b=closest, **kwargs) 

400 

401 # update a with our new transformed points 

402 a = transformed 

403 total_matrix = np.dot(matrix, total_matrix) 

404 

405 if old_cost - cost < threshold: 

406 break 

407 else: 

408 old_cost = cost 

409 

410 return total_matrix, transformed, cost 

411 

412 

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 

428 

429 

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 

445 

446 

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 

463 

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. 

469 

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 

479 

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. 

517 

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 """ 

526 

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 

543 

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)) 

558 

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)) 

566 

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)) 

571 

572 def _create_Dl_Ul(D, source_mesh, source_landmarks, target_positions): 

573 # Create landmark terms (Eq. 11) 

574 Dl, Ul = None, None 

575 

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 

579 

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 

612 

613 target_geometry, target_positions, centroid, scale = _normalize_by_source( 

614 source_mesh, target_geometry, target_positions 

615 ) 

616 

617 # Number of edges and vertices in source mesh 

618 nE = len(source_mesh.edges) 

619 nV = len(source_mesh.vertices) 

620 

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) 

637 

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] 

649 

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 

656 

657 last_error = np.finfo(np.float32).max 

658 error = np.finfo(np.float16).max 

659 cpt_iter = 0 

660 

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 ) 

671 

672 # Data weighting 

673 vertices_weight = np.ones(nV) 

674 vertices_weight[qres["distances"] > distance_threshold] = 0 

675 

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 

686 

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 

698 

699 if return_records: 

700 result = records 

701 else: 

702 result = transformed_vertices 

703 

704 result = _denormalize_by_source( 

705 source_mesh, target_geometry, target_positions, result, centroid, scale 

706 ) 

707 return result 

708 

709 

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. 

722 

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)) 

755 

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 

769 

770 qres["nearest"], qres["distances"], qres["tids"] = closest_point(mesh, input_points) 

771 

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 ) 

778 

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 

787 

788 

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. 

800 

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) 

815 

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) 

832 

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"], :] 

844 

845 return qres 

846 

847 

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 

863 

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. 

867 

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 

877 

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. 

914 

915 

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 """ 

924 

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) 

937 

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 

953 

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 

963 

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 

976 

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) 

986 

987 rows = np.repeat(np.arange(nL), 3) 

988 cols = source_landmarks_vids.flat 

989 data = source_landmarks_barys.flat 

990 

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 

1007 

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 

1014 

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 

1030 

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 

1037 

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 ] 

1046 

1047 source_vtet, source_tet, V = _build_tetrahedrons(source_mesh) 

1048 Vinv = np.linalg.inv(V) 

1049 

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 

1055 

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 ) 

1065 

1066 transformed_vertices = source_vtet.copy() 

1067 if return_records: 

1068 records = [transformed_vertices[:nV]] 

1069 

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] 

1074 

1075 if use_landmarks: 

1076 Astack.append(AEl * wl) 

1077 Bstack.append(target_positions * wl) 

1078 

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 ) 

1089 

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 

1108 

1109 # Account for vertices' weight 

1110 AEc.data *= vertices_weight[non_markers_mask][AEc.indices] 

1111 Bc *= vertices_weight[non_markers_mask, None] 

1112 

1113 Astack.append(AEc * wc) 

1114 Bstack.append(Bc * wc) 

1115 

1116 # Now solve Eq. 14 ... 

1117 A = sparse.vstack(Astack, format="csc") 

1118 A.eliminate_zeros() 

1119 b = np.concatenate(Bstack) 

1120 

1121 LU = sparse.linalg.splu((A.T * A).tocsc()) 

1122 transformed_vertices = LU.solve(A.T * b) 

1123 # done ! 

1124 

1125 if return_records: 

1126 records.append(transformed_vertices[:nV]) 

1127 

1128 if return_records: 

1129 result = records 

1130 else: 

1131 result = transformed_vertices[:nV] 

1132 

1133 result = _denormalize_by_source( 

1134 source_mesh, target_geometry, target_positions, result, centroid, scale 

1135 ) 

1136 return result