From bcf8c2cc623cebccf836f3cf66e3f22f7e60a7b0 Mon Sep 17 00:00:00 2001 From: tomvanmele Date: Thu, 23 May 2024 11:11:58 +0200 Subject: [PATCH] Fix icp_numpy --- CHANGELOG.md | 4 ++++ src/compas/geometry/icp_numpy.py | 13 ++++++++++--- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 75d77a121de..e4467a3f3b2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,8 +9,12 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added +* Added `maxiter` parameter to `compas.geometry.icp_numpy`. + ### Changed +* Fixed bug in `compas.geometry.ic_numpy`, which was caused by returning only the last transformation of the iteration process. + ### Removed diff --git a/src/compas/geometry/icp_numpy.py b/src/compas/geometry/icp_numpy.py index 559b02f739a..ccd905ad351 100644 --- a/src/compas/geometry/icp_numpy.py +++ b/src/compas/geometry/icp_numpy.py @@ -2,6 +2,7 @@ from numpy import argmin from numpy import asarray from numpy.linalg import det +from numpy.linalg import multi_dot from scipy.linalg import norm from scipy.linalg import svd from scipy.spatial.distance import cdist @@ -36,7 +37,7 @@ def bestfit_transform(A, B): return X -def icp_numpy(source, target, tol=None): +def icp_numpy(source, target, tol=None, maxiter=100): """Align two point clouds using the Iterative Closest Point (ICP) method. Parameters @@ -48,6 +49,8 @@ def icp_numpy(source, target, tol=None): tol : float, optional Tolerance for finding matches. Default is :attr:`TOL.approximation`. + maxiter : int, optional + The maximum number of iterations. Returns ------- @@ -90,7 +93,9 @@ def icp_numpy(source, target, tol=None): X = Transformation.from_frame_to_frame(A_frame, B_frame) A = transform_points_numpy(A, X) - for i in range(20): + stack = [X] + + for i in range(maxiter): D = cdist(A, B, "euclidean") closest = argmin(D, axis=1) residual = norm(normrow(A - B[closest])) @@ -101,4 +106,6 @@ def icp_numpy(source, target, tol=None): X = bestfit_transform(A, B[closest]) A = transform_points_numpy(A, X) - return A, X + stack.append(X) + + return A, multi_dot(stack[::-1])