affine.m

#!/bin/octave
function [ affine_rotation, translation, s ] = affine( p, q )
  ## this function "affine(p,q)" finds the optimal affine transform in
  ## 3-dimensional euclidian space, using least squares and
  ## single-value-decomposition. Given a 3xn matrix (set of n 3d
  ## positions), it returns and the rotation matrtix "affine_rotation",
  ## and the translation vector "translation".
  ##
  ## K. S. Arun, T. S. Huang and S. D. Blostein, "Least-Squares Fitting of
  ## Two 3-D Point Sets," in IEEE Transactions on Pattern Analysis and
  ## Machine Intelligence, vol. PAMI-9, no. 5, pp. 698-700, Sept. 1987,
  ## doi: 10.1109/TPAMI.1987.4767965.
  ##
  ## Berthold K. P. Horn, "Closed-form solution of absolute orientation
  ## using unit quaternions," J. Opt. Soc. Am. A 4, 629-642 (1987)
  ##
  ## @ moritz siegel

  global hd
  global wd
  global nwd
  
  ## check stuff.
  assert( nargin == 2 && size( p ) == size( q ), ...
          "need 2 identical input matrices\n" );
  assert( size( p, 2 ) == 3, "input matrix p must be nx3\n" );
  assert( size( q, 2 ) == 3, "input matrix q must be nx3\n" );
  n = size( p, 1 );
  assert( n > 2, "need at least 3 points\n" );

  ## 1) center to get rid of translation.
  centroid_p = mean( p, 1 );
  centroid_q = mean( q, 1 );
  p_shifted = p - repmat( centroid_p, n, 1 );
  q_shifted = q - repmat( centroid_q, n, 1 );
  
  ## 2) orthogonal reduction.
  ## ------------------------
  
  ## variance (?) matrices.
  s_p = p_shifted' * p_shifted;
  s_q = q_shifted' * q_shifted;

  ## square-roots of the covariance matrices.
  ## choleski decomposition: "A -> LL*" , any advantage over sqrtm()?
  #s_sqrt_p = sqrtm( s_p );
  #s_sqrt_q = sqrtm( s_q );
  s_sqrt_p = chol( s_p, "lower" );
  s_sqrt_q = chol( s_q, "lower" );
  
  ## left division. "x\y" is conceptually equivalent to the expression
  ## "inv(x) * y" but it is computed without forming the inverse of x.
  ## if the system is not square, or if the coefficient matrix is
  ## singular, a minimum norm solution is computed.
  p_orthogonal = ( s_sqrt_p \ p_shifted' )';
  q_orthogonal = ( s_sqrt_q \ q_shifted' )';
  
  ## "p" and "q" are now solely related by a rotational matrix
  ## "r = s_inv_sqrt_q * affine * s_sqrt_p" (no inverse!).

  ## 3) solve least squares problem for best rotation.
  ## -------------------------------------------------
  
  ## covariance matrix again.
  h = p_orthogonal' * q_orthogonal;

  ## singular value decomposition.
  [ u, s, v ] = svd( h );
  rotation = v * u';

  ## reflection? theres more to that.
  if ( det( rotation ) < 0 )
    printf( "warning: det(rotation) < 0\n" );
    if ( any( s( : ) ) < 0 )
      printf( "found reflection, correcting.\n" );
      v( :, 3 ) = - v( :, 3 );
      rotation = v * u';
    else
      printf( "error: single-value-decomposition might have failed! \
provided data seems is too noisy for least-squares.\n" );  
    endif
  endif

  ## 4)  reverse the rotation to the non-orthogonal affine transform using
  ## right division: "x/y" is conceptually equivalent to the expression
  ## "x * inv(y)" but it is computed without forming the inverse of x.
  affine_rotation = ( s_sqrt_q * rotation ) / s_sqrt_p;
  
  ## 5) compute translation.
  translation = centroid_q - ( affine_rotation * centroid_p' )';
  
  ## save stuff for plotting.
  ## chdir( nwd )
  ## save p_shifted.mat p_shifted;
  ## save q_shifted.mat q_shifted;
  ## save p_orthogonal.mat p_orthogonal;
  ## save q_orthogonal.mat q_orthogonal;
  ## save rotation.mat rotation
  ## save affine_rotation.mat affine_rotation
  ## save translation.mat translation
  
endfunction