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 c2ce46fcf..d20f83e1b 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -46,6 +46,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 114b22b42..f57a5996a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -46,6 +46,7 @@ ADD_SUBDIR(system) ADD_SUBDIR(stats) add_subdirectory(sparse) +add_subdirectory(spatial) set(fppFiles stdlib_version.fypp @@ -73,5 +74,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..68c1861d7 --- /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}_lapack ${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..339f6375e --- /dev/null +++ b/src/spatial/stdlib_spatial_kabsch_umeyama.fypp @@ -0,0 +1,178 @@ +#: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 + use stdlib_optval, only: optval + use stdlib_linalg_lapack, only: gemm, gemv, ger, gerc + +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, point, d, N + ${t}$, allocatable :: covariance(:,:), U(:,:), Vt(:,:), vec(:), tmp_N(:), tmp_d1(:), tmp_d2(:), c_P(:), c_Q(:) + real(${k}$) :: sum_w, variance_p + real(${k}$), allocatable :: S(:) + ${t}$ :: temp + #:if t.startswith('complex') + real(${k}$) :: rtemp + #:endif + logical :: scale_ + #:if t.startswith('real') + logical :: reflect_ + #:endif + real(${k}$) :: rmsd_err + + scale_ = optval(scale, .true.) + ! Dimension checks + d = size(P,dim=1) + N = size(P,dim=2) + if(any(shape(P)/=shape(Q)) .or. any(shape(R)/=[d,d]) .or. size(t)/=d) then + call error_stop("array sizes do not match") + end if + if (present(W)) then + if (size(W) /= N) then + call error_stop("array sizes do not match") + end if + end if + + if(present(W)) then + sum_w = stdlib_sum_kahan(W) + else + sum_w = real(N, kind = ${k}$) + end if + !> leave opportunity for future discussion on how to add spmd reduction needed here to reduce sum_w before division + + sum_w = one_${k}$ / sum_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}$ + if(reflect_) Vt(d,:) = -Vt(d,:) + #:endif + + ! Optimal rotation matrix. + call gemm(transa='N', transb='N', m=d,n=d,k=d, alpha=one_${s}$, a=U,lda=d, b=Vt, ldb=d, beta=zero_${s}$, c=R, ldc=d) + + ! Scaling factor + c = one_${s}$ + if(scale_) then + #:if t.startswith('real') + if(reflect_) then + c = sum(S(1:d-1)) - S(d) + else + c = sum(S(1:d)) + end if + #:else + c = sum(S(1:d)) + #:endif + c = variance_p / c + end if + + ! Translation vector t = c_P - c*R*c_Q + t = c_P + call gemv(trans='N', m=d, n=d, alpha=-c, A=R, lda=d, x=c_Q, incx=1, beta=one_${s}$, y=t, incy=1) + + ! 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 + vec = t + call gemv(trans='N', m=d, n=d, alpha=c, A=R, lda=d, x=Q(:, point), incx=1, beta=one_${s}$, y=vec, incy=1) + vec = vec - P(:,point) + temp = stdlib_dot_product_kahan(vec,vec) + #:if t.startswith('complex') + rtemp = real(temp, kind=${k}$) + call kahan_kernel(rtemp, rmsd, rmsd_err) + #:else + call kahan_kernel(temp, rmsd, rmsd_err) + #:endif + 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 d90af3fa1..a89ece537 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -50,6 +50,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