diff --git a/doc/specs/stdlib_spatial.md b/doc/specs/stdlib_spatial.md new file mode 100644 index 000000000..61fe8c219 --- /dev/null +++ b/doc/specs/stdlib_spatial.md @@ -0,0 +1,59 @@ +--- +title: spatial +--- + +# The `stdlib_spatial` module + +This module provides implementations of algorithms for spatial data processing. + +[TOC] + +## `kabsch_umeyama` - Finding optimal rotation matrix + +### Status + +Experimental + +### Description + +Compute the optimal similarity transform (Kabsch–Umeyama): +\[ + P \approx c \, R \, Q + t +\] +where: + +- R is an orthogonal rotation matrix, +- c is an optional scaling factor, +- t is a translation vector. + +The transformation minimizes the root-mean-square deviation (RMSD) between corresponding columns +of P and Q, optionally using weights and with optional scaling. +The implementation is based on the algorithm described here: [Aligning point patterns with Kabsch–Umeyama algorithm](https://zpl.fi/aligning-point-patterns-with-kabsch-umeyama-algorithm) + +### Syntax + +`call ` [[stdlib_spatial(module):kabsch_umeyama(interface)]] `(P, Q, R, t, c, rmsd [, W, scale])` + +### Arguments + +`P`: Shall be a `real` or `complex` rank-2 array. It is an `intent(in)` argument. + +`Q`: Shall be a rank-2 array with same kind as `P`. It is an `intent(in)` argument. + +`R`: Shall be a rank-2 array with same kind as `P`. For `real` kinds, the algorithm returns a proper rotation matrix, meaning `det(R) = 1`. It is an `intent(out)` argument. + +`t`: Shall be a rank-1 array with same kind as `P`. It is an `intent(out)` argument. + +`c`: Scalar value of the same type as `P`. It is an `intent(out)` argument. If `scale` is disabled `c` will be returned with a value of `1`. + +`rmsd`: Scalar value of `real` kind. It is an `intent(out)` argument. + +`W` (optional): Shall be a rank-1 array of `real` kind. It is an `intent(in)` argument. By default, `W` is an array of `1`s. + +`scale` (optional): Shall be a logical type. It is an `intent(in)` argument. By default, `scale = .true.`. + +### Example + +```fortran +{!example/spatial/example_kabsch_umeyama.f90!} +``` \ No newline at end of file diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt index efd1ae171..294904951 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -45,6 +45,7 @@ if (STDLIB_QUADRATURE) endif() add_subdirectory(selection) add_subdirectory(sorting) +add_subdirectory(spatial) add_subdirectory(specialfunctions_gamma) if (STDLIB_SPECIALMATRICES) add_subdirectory(specialmatrices) diff --git a/example/spatial/CMakeLists.txt b/example/spatial/CMakeLists.txt new file mode 100644 index 000000000..867f35f11 --- /dev/null +++ b/example/spatial/CMakeLists.txt @@ -0,0 +1 @@ +ADD_EXAMPLE(kabsch_umeyama) \ No newline at end of file diff --git a/example/spatial/example_kabsch_umeyama.f90 b/example/spatial/example_kabsch_umeyama.f90 new file mode 100644 index 000000000..daa9f5d1f --- /dev/null +++ b/example/spatial/example_kabsch_umeyama.f90 @@ -0,0 +1,36 @@ +program example_kabsch_umeyama + use stdlib_linalg_constants, only: dp + use stdlib_spatial, only: kabsch_umeyama + implicit none + + integer, parameter :: d = 2, N = 7 + real(dp) :: P(d,N), Q(d,N), R(d, d), t(d), c, rmsd + + integer :: i + + ! 2x7 matrices. + P(1,:) = [23.0_dp, 66.0_dp, 88.0_dp, 119.0_dp, 122.0_dp, 170.0_dp, 179.0_dp] + P(2,:) = [178.0_dp, 173.0_dp, 187.0_dp, 202.0_dp, 229.0_dp, 232.0_dp, 199.0_dp] + + Q(1,:) = [232.0_dp, 208.0_dp, 181.0_dp, 155.0_dp, 142.0_dp, 121.0_dp, 139.0_dp] + Q(2,:) = [38.0_dp, 32.0_dp, 31.0_dp, 45.0_dp, 33.0_dp, 59.0_dp, 69.0_dp] + + call kabsch_umeyama(P, Q, R, t, c, rmsd) + + print * + print *, "Recovered rotation R:" + do i = 1, d + print *, R(i,:) + end do + + print * + print *, "Recovered scale c:", c + + print * + print *, "Recovered translation t:" + print *, t + + print * + print *, "RMSD:", rmsd + +end program example_kabsch_umeyama \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index f955ed352..a4f6dde9e 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -44,6 +44,7 @@ ADD_SUBDIR(system) ADD_SUBDIR(stats) add_subdirectory(sparse) +add_subdirectory(spatial) set(fppFiles stdlib_version.fypp @@ -70,5 +71,6 @@ target_link_libraries(${PROJECT_NAME} PUBLIC ${PROJECT_NAME}_strings ${PROJECT_NAME}_blas ${PROJECT_NAME}_lapack ${PROJECT_NAME}_lapack_extended ${PROJECT_NAME}_sparse + ${PROJECT_NAME}_spatial ${OPTIONAL_LIB} ) diff --git a/src/spatial/CMakeLists.txt b/src/spatial/CMakeLists.txt new file mode 100644 index 000000000..1cfd0e111 --- /dev/null +++ b/src/spatial/CMakeLists.txt @@ -0,0 +1,14 @@ +set(spatial_fppFiles + stdlib_spatial.fypp + stdlib_spatial_kabsch_umeyama.fypp + ) + +set(spatial_cppFiles + ) + +set(spatial_f90Files + ) + +configure_stdlib_target(${PROJECT_NAME}_spatial spatial_f90Files spatial_fppFiles spatial_cppFiles) + +target_link_libraries(${PROJECT_NAME}_spatial PUBLIC ${PROJECT_NAME}_core ${PROJECT_NAME}_constants ${PROJECT_NAME}_linalg ${PROJECT_NAME}_intrinsics) \ No newline at end of file diff --git a/src/spatial/stdlib_spatial.fypp b/src/spatial/stdlib_spatial.fypp new file mode 100644 index 000000000..33a15192a --- /dev/null +++ b/src/spatial/stdlib_spatial.fypp @@ -0,0 +1,41 @@ +#:include "common.fypp" +#:set R_KINDS_TYPES = list(zip(REAL_KINDS, REAL_TYPES, REAL_SUFFIX)) +#:set C_KINDS_TYPES = list(zip(CMPLX_KINDS, CMPLX_TYPES, CMPLX_SUFFIX)) +#:set KINDS_TYPES = R_KINDS_TYPES+C_KINDS_TYPES +module stdlib_spatial + use stdlib_kinds, only: sp, dp, xdp, qp + use stdlib_constants + implicit none + private + public :: kabsch_umeyama + + interface kabsch_umeyama + !! ([Specifications](../page/specs/stdlib_spatial.html#kabsch_umeyama)) + !! This interface computes the optimal similarity transform (Kabsch–Umeyama): + !! \[ + !! P \approx c \, R \, Q + t + !! \] + !! The transformation minimizes the RMSD between corresponding columns + !! of P and Q, optionally using weights and with optional scaling. + #:for k, t, s in (KINDS_TYPES) + module subroutine kabsch_umeyama_${s}$(P, Q, R, t, c, rmsd, W, scale) + ${t}$, intent(in) :: P(:, :) + !! Target point set (d × N) + ${t}$, intent(in) :: Q(:, :) + !! Reference point set (d × N) + ${t}$, intent(out) :: R(:, :) + !! Optimal rotation matrix (d × d) + ${t}$, intent(out) :: t(:) + !! Translation vector (d) + ${t}$, intent(out) :: c + !! Scale factor + real(${k}$), intent(out) :: rmsd + !! Root-mean-square deviation + real(${k}$), intent(in), optional :: W(:) + !! Optional weights + logical, intent(in), optional :: scale + !! Enable scaling + end subroutine + #:endfor + end interface +end module stdlib_spatial \ No newline at end of file diff --git a/src/spatial/stdlib_spatial_kabsch_umeyama.fypp b/src/spatial/stdlib_spatial_kabsch_umeyama.fypp new file mode 100644 index 000000000..5de05309a --- /dev/null +++ b/src/spatial/stdlib_spatial_kabsch_umeyama.fypp @@ -0,0 +1,201 @@ +#:include "common.fypp" +#:set R_KINDS_TYPES = list(zip(REAL_KINDS, REAL_TYPES, REAL_SUFFIX)) +#:set C_KINDS_TYPES = list(zip(CMPLX_KINDS, CMPLX_TYPES, CMPLX_SUFFIX)) +#:set KINDS_TYPES = R_KINDS_TYPES+C_KINDS_TYPES +submodule(stdlib_spatial) stdlib_spatial_kabsch_umeyama + use stdlib_linalg, only: svd, det + use stdlib_intrinsics, only: stdlib_sum_kahan, stdlib_dot_product_kahan, kahan_kernel + use stdlib_error, only: error_stop + +contains + #:for k, t, s in (KINDS_TYPES) + module subroutine kabsch_umeyama_${s}$(P, Q, R, t, c, rmsd, W, scale) + ${t}$, intent(in) :: P(:, :) + !! Target point set (d × N) + ${t}$, intent(in) :: Q(:, :) + !! Reference point set (d × N) + ${t}$, intent(out) :: R(:, :) + !! Optimal rotation matrix (d × d) + ${t}$, intent(out) :: t(:) + !! Translation vector (d) + ${t}$, intent(out) :: c + !! Scale factor + real(${k}$), intent(out) :: rmsd + !! Root-mean-square deviation + real(${k}$), intent(in), optional :: W(:) + !! Optional weights + logical, intent(in), optional :: scale + !! Enable scaling + + ! Internal variables. + integer :: i, j, point, d, N + ${t}$, allocatable :: covariance(:,:), U(:,:), Vt(:,:), vec(:), tmp_N(:), tmp_d(:), c_P(:), c_Q(:) + real(${k}$) :: sum_w, variance_p + real(${k}$), allocatable :: S(:) + logical :: scale_ + #:if t.startswith('real') + logical :: reflect_ + #:endif + real(${k}$) :: rmsd_err + + + ! Dimension checks + if(size(P,dim=1)/=size(Q,dim=1) .or. size(P,dim=1)/=size(R,dim=1) .or. size(P,dim=1)/=size(R,dim=2) & + .or. size(P,dim=1)/=size(t)) then + call error_stop("array sizes do not match") + end if + if(size(P,dim=2)/=size(Q,dim=2)) then + call error_stop("array sizes do not match") + end if + if (present(W)) then + if (size(W) /= size(P,dim=2)) then + call error_stop("array sizes do not match") + end if + end if + d = size(P,dim=1) + N = size(P,dim=2) + scale_ = .true. + if(present(scale)) scale_ = scale + + sum_w = one_${k}$ / N + if(present(W)) sum_w = one_${k}$ / stdlib_sum_kahan(W) + if(sum_w H = U * S * Vt + call svd(covariance, S, U, Vt) + + ! Check for reflections in case of real entries. + #:if t.startswith('real') + reflect_ = det(matmul(U,Vt)) < zero_${s}$ + #:endif + + #:if t.startswith('real') + if(reflect_) Vt(d,:) = -Vt(d,:) + #:endif + + ! Optimal rotation matrix. + do i = 1,d + do j = 1,d + #:if t.startswith('complex') + R(i,j) = stdlib_dot_product_kahan(conjg(U(i,:)), Vt(:, j)) + #:else + R(i,j) = stdlib_dot_product_kahan(U(i,:), Vt(:, j)) + #:endif + end do + end do + + ! Scaling factor + if(scale_) then + #:if t.startswith('real') + if(reflect_) then + c = variance_p / (sum(S(1:d-1)) - S(d)) + else + c = variance_p / (sum(S(1:d))) + end if + #:else + c = variance_p / (sum(S(1:d))) + #:endif + else + c = one_${s}$ + end if + + ! Translation vector t = c_P - c*R*c_Q + do i = 1, d + #:if t.startswith('complex') + t(i) = c_P(i) - c * stdlib_dot_product_kahan(conjg(R(i,1:d)), c_Q(1:d)) + #:else + t(i) = c_P(i) - c * stdlib_dot_product_kahan(R(i,1:d), c_Q(1:d)) + #:endif + end do + + ! Compute RMSD + allocate(vec(d), source=zero_${s}$) + rmsd = zero_${k}$ + rmsd_err = zero_${k}$ + do point = 1, N + ! Calculate the k^th difference vector by the formula vec_k = c*R*Q_k + t - P_k + do i = 1, d + #:if t.startswith('complex') + vec(i) = c * stdlib_dot_product_kahan(conjg(R(i,1:d)), Q(1:d,point)) + #:else + vec(i) = c * stdlib_dot_product_kahan(R(i,1:d), Q(1:d,point)) + #:endif + end do + vec(1:d) = vec(1:d) + t(1:d) - P(1:d,point) + call kahan_kernel(real(stdlib_dot_product_kahan(vec,vec), kind=${k}$), rmsd, rmsd_err) + end do + rmsd = sqrt(rmsd * sum_w) + end subroutine + #:endfor +end submodule stdlib_spatial_kabsch_umeyama \ No newline at end of file diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index ea43bc8eb..caa46c947 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -49,6 +49,7 @@ endif() add_subdirectory(optval) add_subdirectory(selection) add_subdirectory(sorting) +add_subdirectory(spatial) add_subdirectory(specialfunctions) if (STDLIB_STATS) add_subdirectory(stats) diff --git a/test/spatial/CMakeLists.txt b/test/spatial/CMakeLists.txt new file mode 100644 index 000000000..081d83afb --- /dev/null +++ b/test/spatial/CMakeLists.txt @@ -0,0 +1,7 @@ +set( + fppFiles + "test_spatial_kabsch_umeyama.fypp" +) +fypp_f90("${fyppFlags}" "${fppFiles}" outFiles) + +ADDTEST(spatial_kabsch_umeyama) \ No newline at end of file diff --git a/test/spatial/test_spatial_kabsch_umeyama.fypp b/test/spatial/test_spatial_kabsch_umeyama.fypp new file mode 100644 index 000000000..7c2e3ec00 --- /dev/null +++ b/test/spatial/test_spatial_kabsch_umeyama.fypp @@ -0,0 +1,366 @@ +#:include "common.fypp" +#:set R_KINDS_TYPES = list(zip(REAL_KINDS, REAL_TYPES, REAL_SUFFIX)) +#:set C_KINDS_TYPES = list(zip(CMPLX_KINDS, CMPLX_TYPES, CMPLX_SUFFIX)) +module test_kabsch_umeyama + use testdrive, only : new_unittest, unittest_type, error_type, check, skip_test + use stdlib_kinds + use stdlib_math, only: all_close, is_close + use stdlib_linalg, only: svd, det, eye + use stdlib_spatial + use stdlib_intrinsics, only: stdlib_dot_product_kahan + use stdlib_constants + implicit none + +contains + + !> Collect all exported unit tests + subroutine collect_suite(testsuite) + !> Collection of tests + type(unittest_type), allocatable, intent(out) :: testsuite(:) + + testsuite = [ & + new_unittest('kabsch_umeyama_real', test_kabsch_umeyama_real), & + new_unittest('kabsch_umeyama_complex', test_kabsch_umeyama_complex) & + ] + end subroutine + + #:for k, t, s in (R_KINDS_TYPES) + subroutine generate_random_set_real_${s}$(P, Q, d, N, scale) + integer, intent(in) :: d, N + ${t}$, intent(out) :: P(d, N), Q(d, N) + logical, intent(in) :: scale + + ! Internal variables. + ${t}$ :: R(d, d) + ${t}$ :: U(d,d), Vt(d,d) + real(${k}$) :: S(d) + ${t}$ :: t(d) + ${t}$ :: c + integer :: i, j + + c = one_${s}$ + ! Random reference points Q + call random_number(Q) + + ! Random proper rotation matrix R constructed via SVD: R = U * V^T + call random_number(R) + call svd(R, S, U, Vt) + do i = 1,d + do j = 1,d + R(i,j) = stdlib_dot_product_kahan(U(i,:), Vt(:, j)) + end do + end do + ! For real cases, only det(R) = +1 can be checked. Because if det(R) = -1, then algorithm will + ! find a rotation matrix without reflection in which rmsd is minimized. + if(det(R) < zero_${s}$) R(:, d) = -R(:, d) + + ! Random scale and translation + if(scale) call random_number(c) + call random_number(t) + + ! Construct P = c*R*Q + t + do j = 1, N + do i = 1, d + P(i,j) = c * & + stdlib_dot_product_kahan(R(i,:), Q(:,j)) + & + t(i) + end do + end do + end subroutine + #:endfor + #:for k, t, s in (C_KINDS_TYPES) + subroutine generate_random_set_complex_${s}$(P, Q, d, N, scale) + integer, intent(in) :: d, N + ${t}$, intent(out) :: P(d, N), Q(d, N) + logical, intent(in) :: scale + + ! Internal variables. + ${t}$ :: R(d,d), t(d) + ${t}$ :: U(d,d), Vt(d,d) + real(${k}$) :: S(d) + ${t}$ :: c + real(${k}$) :: tempdn(d,N,2) + real(${k}$) :: tempdd(d,d,2) + real(${k}$) :: r1, r2 + integer :: i, j + + c = one_${s}$ + ! Random reference points Q + call random_number(tempdn) + Q = cmplx(tempdn(:,:,1), tempdn(:,:,2), kind=${k}$) + + ! Random proper rotation matrix R constructed via SVD: R = U * V^H + call random_number(tempdd) + R = cmplx(tempdd(:,:,1), tempdd(:,:,2), kind=${k}$) + call svd(R, S, U, Vt) + do i = 1,d + do j = 1,d + R(i,j) = stdlib_dot_product_kahan(U(i,:), Vt(:, j)) + end do + end do + + ! Random scale and translation + if(scale) then + call random_number(r1) + call random_number(r2) + c = cmplx(r1, r2, kind=${k}$) + end if + call random_number(tempdd) + t = cmplx(tempdd(:,1,1), tempdd(:,1,2), kind=${k}$) + + ! Construct P = c*R*Q + t + do j = 1, N + do i = 1, d + P(i,j) = c * & + stdlib_dot_product_kahan(R(i,:), Q(:,j)) + & + t(i) + end do + end do + end subroutine + #:endfor + + subroutine test_kabsch_umeyama_real(error) + type(error_type), allocatable, intent(out) :: error + #:for k, t, s in (R_KINDS_TYPES) + block + integer, parameter :: d = 3, N = 8 + ${t}$ :: P_original(d, N), P_recovered(d, N), Q_original(d, N) + ${t}$ :: R_recovered(d, d) + ${t}$ :: t_recovered(d) + ${t}$ :: c_recovered + ${t}$ :: rmsd + ${t}$ :: Identity(d,d) + ${t}$ :: zero_t(d) + ${t}$ :: W(N) + + integer :: i, j + real(${k}$), parameter :: ${k}$tol = 100 * epsilon(one_${k}$) + + R_recovered = zero_${s}$ + t_recovered = zero_${s}$ + c_recovered = zero_${s}$ + rmsd = zero_${k}$ + Identity = eye(d,d) + zero_t = zero_${s}$ + + ! Random set of transformation + call generate_random_set_real_${s}$(P_original, Q_original, d, N, .true.) + ! Call Kabsch–Umeyama + call kabsch_umeyama(P_original, Q_original, R_recovered, t_recovered, c_recovered, rmsd) + ! Apply recovered transform + do j = 1, N + do i = 1, d + P_recovered(i,j) = c_recovered * & + stdlib_dot_product_kahan(R_recovered(i,:), Q_original(:,j)) + & + t_recovered(i) + end do + end do + call check(error, all_close(P_recovered, P_original, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation, P_recovered not equal to P_original") + if(allocated(error)) return + call check(error, is_close(rmsd, zero_${k}$, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation, RMSD not equal to zero") + if(allocated(error)) return + + ! Identity transformation (Q -> Q) + call random_number(Q_original) + ! Call Kabsch–Umeyama + call kabsch_umeyama(Q_original, Q_original, R_recovered, t_recovered, c_recovered, rmsd) + call check(error, all_close(R_recovered, Identity, abs_tol=${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Identity transformation, R_recovered not equal to Identity") + if(allocated(error)) return + call check(error, all_close(t_recovered, zero_t, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Identity transformation, t_recovered not equal to zero") + if(allocated(error)) return + call check(error, is_close(rmsd, zero_${k}$, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Identity transformation, RMSD not equal to zero") + if(allocated(error)) return + + ! Random set of transformation but with scale disabled. + call generate_random_set_real_${s}$(P_original, Q_original, d, N, .false.) + ! Call Kabsch–Umeyama + call kabsch_umeyama(P_original, Q_original, R_recovered, t_recovered, c_recovered, rmsd, scale=.false.) + ! Apply recovered transform + do j = 1, N + do i = 1, d + P_recovered(i,j) = c_recovered * & + stdlib_dot_product_kahan(R_recovered(i,:), Q_original(:,j)) + & + t_recovered(i) + end do + end do + call check(error, all_close(P_recovered, P_original, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation (scale disabled), P_recovered not equal to P_original") + if(allocated(error)) return + call check(error, is_close(c_recovered, one_${k}$, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation (scale disabled), c_recovered not equal to one") + if(allocated(error)) return + call check(error, is_close(rmsd, zero_${k}$, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation (scale disabled), RMSD not equal to zero") + if(allocated(error)) return + + ! Random set of transformation but with weights. Since P = c*R*Q + t, weights should not matter + call random_number(W) + call generate_random_set_real_${s}$(P_original, Q_original, d, N, .true.) + ! Call Kabsch–Umeyama + call kabsch_umeyama(P_original, Q_original, R_recovered, t_recovered, c_recovered, rmsd, W=W) + ! Apply recovered transform + do j = 1, N + do i = 1, d + P_recovered(i,j) = c_recovered * & + stdlib_dot_product_kahan(R_recovered(i,:), Q_original(:,j)) + & + t_recovered(i) + end do + end do + call check(error, is_close(det(R_recovered), one_${s}$, abs_tol=${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation (weighted), det(R_recovered) not equal to one") + if(allocated(error)) return + call check(error, all_close(P_recovered, P_original, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation (weighted), P_recovered not equal to P_original") + if(allocated(error)) return + call check(error, is_close(rmsd, zero_${k}$, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation (weighted), RMSD not equal to zero") + if(allocated(error)) return + end block + #:endfor + end subroutine + + subroutine test_kabsch_umeyama_complex(error) + !> Error handling + type(error_type), allocatable, intent(out) :: error + #:for k, t, s in (C_KINDS_TYPES) + block + integer, parameter :: d = 3, N = 8 + ${t}$ :: P_original(d, N), Q_original(d, N), P_recovered(d, N) + ${t}$ :: R_recovered(d, d) + ${t}$ :: t_recovered(d) + ${t}$ :: c_recovered + real(${k}$) :: rmsd + ${t}$ :: Identity(d,d) + ${t}$ :: zero_t(d) + real(${k}$) :: W(N) + + integer :: i, j + real(${k}$), parameter :: ${k}$tol = 100 * epsilon(one_${k}$) + + R_recovered = zero_${s}$ + t_recovered = zero_${s}$ + c_recovered = zero_${s}$ + rmsd = zero_${k}$ + Identity = eye(d,d) + zero_t = zero_${s}$ + + ! Random set of transformation + call generate_random_set_complex_${s}$(P_original, Q_original, d, N, .true.) + ! Call Kabsch–Umeyama + call kabsch_umeyama(P_original, Q_original, R_recovered, t_recovered, c_recovered, rmsd) + ! Apply recovered transform + do j = 1, N + do i = 1, d + P_recovered(i,j) = c_recovered * & + stdlib_dot_product_kahan(conjg(R_recovered(i,:)), Q_original(:,j)) + & + t_recovered(i) + end do + end do + call check(error, is_close(abs(det(R_recovered)), one_${k}$, abs_tol=${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation, abs(det(R_recovered)) not equal to one") + if(allocated(error)) return + call check(error, all_close(P_recovered, P_original, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation, P_recovered not equal to P_original") + if(allocated(error)) return + call check(error, is_close(rmsd, zero_${k}$, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation, RMSD not equal to zero") + if(allocated(error)) return + + ! Identity transformation (Q -> Q) + call generate_random_set_complex_${s}$(P_original, Q_original, d, N, .true.) + ! Call Kabsch–Umeyama + call kabsch_umeyama(Q_original, Q_original, R_recovered, t_recovered, c_recovered, rmsd) + call check(error, all_close(R_recovered, Identity, abs_tol=${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Identity transformation, R_recovered not equal to Identity") + if(allocated(error)) return + call check(error, all_close(t_recovered, zero_t, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Identity transformation, t_recovered not equal to zero") + if(allocated(error)) return + call check(error, is_close(rmsd, zero_${k}$, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Identity transformation, RMSD not equal to zero") + if(allocated(error)) return + + ! Random set of transformation but with scale disabled. + call generate_random_set_complex_${s}$(P_original, Q_original, d, N, .false.) + ! Call Kabsch–Umeyama + call kabsch_umeyama(P_original, Q_original, R_recovered, t_recovered, c_recovered, rmsd, scale=.false.) + ! Apply recovered transform + do j = 1, N + do i = 1, d + P_recovered(i,j) = c_recovered * & + stdlib_dot_product_kahan(conjg(R_recovered(i,:)), Q_original(:,j)) + & + t_recovered(i) + end do + end do + call check(error, is_close(abs(det(R_recovered)), one_${k}$, abs_tol=${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation (scale disabled), abs(det(R_recovered)) not equal to one") + if(allocated(error)) return + call check(error, all_close(P_recovered, P_original, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation (scale disabled), P_recovered not equal to P_original") + if(allocated(error)) return + call check(error, is_close(c_recovered, one_${s}$, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation (scale disabled), c_recovered not equal to one") + if(allocated(error)) return + call check(error, is_close(rmsd, zero_${k}$, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation (scale disabled), RMSD not equal to zero") + if(allocated(error)) return + + ! Random set of transformation but with weights. Since P = c*R*Q + t, weights should not matter + call random_number(W) + call generate_random_set_complex_${s}$(P_original, Q_original, d, N, .true.) + ! Call Kabsch–Umeyama + call kabsch_umeyama(P_original, Q_original, R_recovered, t_recovered, c_recovered, rmsd, W=W) + ! Apply recovered transform + do j = 1, N + do i = 1, d + P_recovered(i,j) = c_recovered * & + stdlib_dot_product_kahan(conjg(R_recovered(i,:)), Q_original(:,j)) + & + t_recovered(i) + end do + end do + call check(error, is_close(abs(det(R_recovered)), one_${k}$, abs_tol=${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation (weighted), abs(det(R_recovered)) not equal to one") + if(allocated(error)) return + call check(error, all_close(P_recovered, P_original, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation (weighted), P_recovered not equal to P_original") + if(allocated(error)) return + call check(error, is_close(rmsd, zero_${k}$, abs_tol = ${k}$tol), .true., & + "Kabsch_Umeyama ${t}$ failed: Random transformation (weighted), RMSD not equal to zero") + if(allocated(error)) return + end block + #:endfor + end subroutine + +end module + + +program tester + use, intrinsic :: iso_fortran_env, only : error_unit + use testdrive, only : run_testsuite, new_testsuite, testsuite_type + use test_kabsch_umeyama, only : collect_suite + implicit none + integer :: stat, is + type(testsuite_type), allocatable :: testsuites(:) + character(len=*), parameter :: fmt = '("#", *(1x, a))' + + stat = 0 + + testsuites = [ & + new_testsuite("kabsch_umeyama", collect_suite) & + ] + + do is = 1, size(testsuites) + write(error_unit, fmt) "Testing:", testsuites(is)%name + call run_testsuite(testsuites(is)%collect, error_unit, stat) + end do + + if (stat > 0) then + write(error_unit, '(i0, 1x, a)') stat, "test(s) failed!" + error stop + end if +end program