kabsh umeyama algorithm for vectors aligment

[jalvesz]

Motivation

I would like to propose to add the the Kabsh-Umeyama algorithm (https://en.wikipedia.org/wiki/Kabsch_algorithm) in a (yet to be) stdlib_spatial module for vector alignment purposes.

A first API implementation combining several approaches including the use of weights:

module test
    use stdlib_kinds, only: dp
    use stdlib_intrinsics, only: stdlib_sum
    use stdlib_linalg, only: svd, det, eye, trace

    integer, parameter :: ilp = 4

contains

subroutine kabsh_umeyama( P, Q, d, N, R, t, c, rmsd, W )
    integer, parameter :: wp = dp
    integer, intent(in) :: d       !> Topological dimension 
    integer(ilp), intent(in) :: N  !> Number of points in the set
    real(wp), intent(in) :: P(d,N) !> reference source set of points
    real(wp), intent(in) :: Q(d,N) !> target set of points to tansform
    real(wp), intent(out):: R(d,d) !> Rotation matrix
    real(wp), intent(out):: t(d)   !> translation vector
    real(wp), intent(out):: c      !> scaling factor
    real(wp), intent(out):: rmsd   !> root mean square deviation
    real(wp), intent(in), optional :: W(N) !> Weights per node
    
    ! -- Internal Variables
    integer(ilp) :: i, j, point
    real(wp)     :: centroid_P(d), centroid_Q(d)
    real(wp)     :: covariance(d,d), S(d), U(d,d), Vt(d,d), B(d,d)
    real(wp)     :: vec(d), variance_p, vp, vq, isum_w
    !---------------------------------------
    ! Compute centroids
    isum_w = 1._wp / N
    if(present(W)) isum_w = 1._wp / stdlib_sum(W)
    
    centroid_P = 0._wp; centroid_Q = 0._wp
    if(present(W))then
        !do concurrent( point = 1:N )
        !$omp simd reduction(+:centroid_P, centroid_Q)
        do point = 1, N
            centroid_P(1:d) = centroid_P(1:d) + P(1:d,point)*W(point)
            centroid_Q(1:d) = centroid_Q(1:d) + Q(1:d,point)*W(point)
        end do
        !$omp end simd
    else
        !do concurrent( point = 1:N )
        !$omp simd reduction(+:centroid_P, centroid_Q)
        do point = 1, N
            centroid_P(1:d) = centroid_P(1:d) + P(1:d,point)
            centroid_Q(1:d) = centroid_Q(1:d) + Q(1:d,point)
        end do
        !$omp end simd
    end if
    centroid_P = centroid_P * isum_w
    centroid_Q = centroid_Q * isum_w

    ! Compute the covariance matrix
    covariance = 0._wp; variance_p = 0._wp
    if(present(W))then
        !do concurrent( point = 1:N, j = 1:d, i = 1:d ) reduce(+:covariance,variance_p)
        !$omp simd collapse(3) reduction(+:covariance, variance_p)
        do point = 1, N; do j = 1, d; do i = 1, d
            vp = P(i,point) - centroid_P(i)
            vq = Q(j,point) - centroid_Q(j)
            covariance(i,j) = covariance(i,j) + vp*vq * W(point)
            variance_p = variance_p + vp**2 * W(point)
        end do; end do; end do
        !$omp end simd
    else
        !do concurrent( point = 1:N, j = 1:d, i = 1:d ) reduce(+:covariance,variance_p)
        !$omp simd collapse(3) reduction(+:covariance, variance_p)
        do point = 1, N; do j = 1, d; do i = 1, d
            vp = P(i,point) - centroid_P(i)
            vq = Q(j,point) - centroid_Q(j)
            covariance(i,j) = covariance(i,j) + vp*vq
            variance_p = variance_p + vp**2   
        end do; end do; end do
        !$omp end simd
    end if
    covariance = covariance * isum_w
    variance_p = variance_p * isum_w / d

    ! SVD
    call svd(covariance, S, U, Vt )
    
    ! Optimal rotation
    R = matmul( U , Vt )
    
    ! Validate right-handed coordinate system
    B = eye(d,d)
    B(d,d) = sign( 1._wp, det(R))

    R = matmul( U , matmul( B , Vt ) )
    
    ! Scaling factor
    c = variance_p / (sum(S(1:d-1))+B(d,d)*S(d))

    ! Optimal translation
    t = centroid_P - c * matmul(R , centroid_Q )
    
    ! root mean square deviation
    rmsd = 0._wp
    !do concurrent( point = 1:N ) reduce(+:rmsd)
    !$omp simd reduction(+:rmsd)
    do point = 1, N
        vec(1:d) = c * matmul(R , Q(1:d,point) )
        vec(1:d) = vec(1:d) + t(1:d)
        vec(1:d) = vec(1:d) - P(1:d,point)
        rmsd = rmsd + dot_product(vec,vec)
    end do
    !$omp end simd
    rmsd = sqrt( rmsd * isum_w )

end subroutine

end module

program main
use test

integer, parameter :: wp = 8
integer, parameter :: d = 2
real(wp), allocatable :: P(:,:)
real(wp), allocatable :: Q(:,:)
real(wp) :: R(d,d), t(d), vec(d), c, rmsd
integer :: i, N
N = 7
P = reshape( &
    [23._wp, 178._wp,&
     66._wp, 173._wp,&
     88._wp, 187._wp,&
    119._wp, 202._wp,&
    122._wp, 229._wp,&
    170._wp, 232._wp,&
    179._wp, 199._wp] , [d,N] )

Q = reshape( & 
    [232._wp, 38._wp,&
     208._wp, 32._wp,&
     181._wp, 31._wp,&
     155._wp, 45._wp,&
     142._wp, 33._wp,&
     121._wp, 59._wp,&
     139._wp, 69._wp] , [d,N] )

call kabsh_umeyama( P, Q, d, N, R, t, c, rmsd )
print *, 'RMSD        :', rmsd
print *, 'Translation :', t
print *, 'Scaling     :', c
print *, 'Rotation    :'
do i = 1, d
    print *, R(i,:)
end do


end program

gives:

 RMSD        :   16.242818369005487     
 Translation :   271.33459510449791        396.07800316838268     
 Scaling     :   1.4616613091002038     
 Rotation    :
 -0.81034281019830057       0.58595608193782001     
 -0.58595608193782023      -0.81034281019830057       

Prior Art

• "Aligning point patterns with Kabsch–Umeyama algorithm": https://zpl.fi/aligning-point-patterns-with-kabsch-umeyama-algorithm/?utm_source=chatgpt.com
• SciPy: https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.align_vectors.html
• Implementations in (Numpy, Torch, Jax): https://hunterheidenreich.com/posts/kabsch-algorithm/
• MATLAB : https://www.mathworks.com/matlabcentral/fileexchange/25746-kabsch-algorithm?utm_source=chatgpt.com
• Eigen (C++) : https://libeigen.gitlab.io/eigen/docs-nightly/Umeyama_8h_source.html?utm_source=chatgpt.com
• R : https://rdrr.io/cran/pracma/man/procrustes.html?utm_source=chatgpt.com

Additional Information

No response

[jvdp1]

Thank you. I think it will be a nice addition, even if I wonder where I would use it.

[jalvesz]

I'm already using it for graphs alignment and comparison. It is very useful and effective. I would like to polish the API though.

[loiseaujc]

That's pretty cool. It is particular instance of a more general problem known as (orthogonal) Procustes problem. Many variants of linear system identification problems, or Koopman theoretic algorithms can be formulated as variation around this general problem. Hence, I am wondering whether it should be included in a stdlib_spatial module or a more general stdlib_optimization_procustes one (or something similar) ?

[jalvesz]

This is a very good point! When proposing the module name I simply took inspiration from SciPy's hierarchy within the spatial module.

I asked Claude AI to give me an analysis of this debate:
#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

Claude's AI take on the category for this implementation

This is a genuinely interesting API design question. Let me lay out the tradeoffs carefully, because the answer hinges on what abstraction you're actually selling to the user.

The core tension: algorithm identity vs. mathematical generality
The Kabsch–Umeyama algorithm does three things simultaneously:

1. Finds an optimal rotation (and optionally scaling) between two point sets — this is a geometric/spatial concern
2. Solves a constrained least-squares problem — this is an optimization concern
3. Is a special case of the Procrustes problem (specifically, the orthogonal/rotation-constrained variant) — this is a matrix algebra concern

Where you put it in stdlib signals which of these identities you're prioritizing for the user.

Option A: stdlib_linalg (recommended primary home)

The strongest argument is that Kabsch–Umeyama is fundamentally an SVD-based matrix algorithm. The existing stdlib_linalg module already houses SVD, eigendecomposition, least-squares solvers, and matrix norms Fortran-lang — all the primitives Kabsch–Umeyama relies on. Placing it there means:

• The implementation lives right next to svd, which it wraps
• Users doing point cloud registration or molecular alignment will find it where they look for matrix routines first
• It follows the stdlib precedent: the goal of stdlib routines is to be general, with simple intuitive interfaces, NumPy/MATLAB style GitHub — and numpy.linalg is where such things live in that ecosystem

A natural name: stdlib_linalg_procrustes as a submodule, with the public interface exposed through stdlib_linalg. The function itself could be:

call kabsch(A, B, R, rmsd, scale, err)
! or more generically:
call align_vectors(A, B, R, rmsd, weights, allow_scale, err)

Option B: stdlib_optimization_procrustes (conceptually honest, but premature)

The orthogonal Procrustes problem is indeed the right mathematical frame — it asks for an orthogonal matrix Ω that most closely maps matrix A to matrix B, minimizing the Frobenius norm, and is solved via SVD Wikipedia. Kabsch is the rotation-constrained variant of this.
The issue is that stdlib doesn't yet have a stdlib_optimization module at all. Creating one solely to house Kabsch–Umeyama would mean:

• Building a module infrastructure around a single algorithm
• Implying a broader optimization framework that doesn't exist yet
• Burying a geometrically intuitive operation inside an algebraic abstraction most users won't search for

This could make sense if you're planning to also implement the generalized Procrustes problem (multiple point sets), the weighted variant, or other matrix nearness problems — then the procrustes namespace earns its keep.

Option C: stdlib_spatial (SciPy's choice, but wrong for Fortran stdlib)

SciPy puts this under scipy.spatial.transform.Rotation.align_vectors because it has a rich Rotation object ecosystem. Fortran stdlib has no such object — and adding a spatial module just for this would be even more premature than the optimization route. This is a case where mimicking SciPy's taxonomy doesn't translate well.

My recommendation

Put it in stdlib_linalg now, under a name that hints at the Procrustes framing without requiring users to know what Procrustes means:

! In stdlib_linalg, backed by stdlib_linalg_procrustes submodule
interface procrustes
  ! rotation only (Kabsch)
  ! rotation + isotropic scale (Umeyama)
  ! weighted variants
end interface

This gives you:

• Discoverability: users looking for "align two matrices" check linalg first
• Extensibility: the submodule name stdlib_linalg_procrustes signals the math and can grow to host the generalized multi-matrix variant later
• Precedent alignment: consistent with how stdlib exposes constrained least-squares, eigendecomposition, and SVD variants all through the stdlib_linalg umbrella Fortran-lang
• Future path: if stdlib ever gets a stdlib_optimization module, Procrustes can be re-exported from there while keeping the linalg binding

The one concrete argument for the optimization_procrustes route is if the community discussion reveals users think of this as an optimization primitive rather than a linear algebra primitive — worth raising on the Fortran Discourse before committing.

#>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

It makes good points and maybe the Option A seem legit.

Other opinions ?

My only concern is that I'm still trying to think how to enable passing parallel reduction procedures in a agnostic manner because there are a few places in the algorithm where one would need to reduce some internal variables in the case of distributed parallel computation. Given that stdlib does not handle either coarray or mpi (and probably shouldn't ?) it needs a way of passing such reduction procedures in a framework-agnostic manner (hence my other issue #1048) ... but this would be independent of the choice for placing the module in any case.