Orthogonal Procrustes Analysis with anisotropic scaling

The following matlab code implements an iterative solution to solve the Orthogonal Procrustes Analysis problem with anisotropic scaling. The mathematical foundation was initially presented in [1] in which the Majorization Principle was applied to derived the solution. It has guaranteed convergence. I presented the psudo/matlab code in [2] in the context of ultrasound probe calibration.

This algorithm is a generalization of the Orthogonal Procrustes Analysis with rigid or isotropic scaling. That is, these two algorithms should converge to the same solution if the datasets are inherently rigid or isotropically scaled relative to each other.

function [ R, t, A ] = AOPA( X, Y, tol ) 
% Solves   Y = R A X + t
% INPUTS:  X : (mxn) mD coordinates,
%          Y : (mxn) mD coordinates
%        tol : exit condition (1e-6)
% OUTPUTS: R : mxm orthonormal rotation
%          t : mx1 translation
%          A : mxm diagonal scaling
%
% Elvis C.S. Chen
% Aug. 24, 2020
[m, n]=size(X); e=ones(n,1); II=eye(n)-((e*e')./n);  
mX  = normr(X*II); mX(isnan(mX)) = 0; mY = (Y*II); 
B   = mY*mX'; [U,~,V] = svd( B ); 
D   = eye(m); D(m,m) = det(U*V'); R = U*D*V';
err = +Inf; E_old = 10000*ones(m,n);
while err > tol                                     
  [U,~,V] = svd( B*diag(diag(R'*B)) ); 
  D(m,m) = det(U*V'); R = U*D*V';  
  E = R*mX-mY; err = norm(E-E_old,'fro'); E_old = E;
end
B = Y*II*X'; A = diag( diag(B'*R)./diag(X*II*X') ); 
t = mean( Y-R*A*X, 2); 

The code above is optimize for length, not necessary for computational efficiency. Note that if an initial estimate of rotation is known, it can be used for initialization (right before the loop begins).

If you decide to use this code, please consider citing [2].

References

[1] Bennani Dosse, M., & Ten Berge, J. (2010). Anisotropic Orthogonal Procrustes Analysis. Journal of Classification, 27(1), 111–128. https://doi.org/10.1007/s00357-010-9046-8

[2] E.C.S. Chen et al., (2014). “Solving for free-hand and real-time 3D ultrasound calibration with anisotropic orthogonal Procrustes analysis”; in SPIE Medical Imaging: Image-Guided Procedures, Robotic Interventions, and Modeling, 90361Z, pp. 524-530. https://doi.org/10.1117/12.2043080 BibTeX