Commit 469fc7af authored by Matt Clarkson's avatar Matt Clarkson

Issue #1: Import procrustes point based registration.

parent f81ccbdc
# -*- coding: utf-8 -*-
"""Functions for point based registration using Orthogonal Procrustes."""
import numpy as np
def validate_procrustes_inputs(fixed, moving):
"""
Validates the fixed and moving set of points
1. fixed and moving must be numpy array
2. fixed and moving should have 3 columns
3. fixed and moving should have at least 3 rows
4. fixed and moving should have the same number of rows
:param fixed: point set, N x 3 ndarray
:param moving: point set, N x 3 ndarray of corresponding points
:returns: nothing
:raises: TypeError, ValueError
"""
if not isinstance(fixed, np.ndarray):
raise TypeError("fixed is not a numpy array'")
if not isinstance(moving, np.ndarray):
raise TypeError("moving is not a numpy array")
if not fixed.shape[1] == 3: # pylint: disable=literal-comparison
raise ValueError("fixed should have 3 columns")
if not moving.shape[1] == 3: # pylint: disable=literal-comparison
raise ValueError("moving should have 3 columns")
if fixed.shape[0] < 3:
raise ValueError("fixed should have at least 3 points (rows)")
if moving.shape[0] < 3:
raise ValueError("moving should have at least 3 points (rows)")
if not fixed.shape[0] == moving.shape[0]:
raise ValueError("fixed and moving should have "
+ "the same number of points (rows)")
def compute_fre(fixed, moving, rotation, translation):
"""
Computes the Fiducial Registration Error, equal
to the root mean squared error between corresponding fiducials.
:param fixed: point set, N x 3 ndarray
:param moving: point set, N x 3 ndarray of corresponding points
:param rotation: 3 x 3 ndarray
:param translation: 3 x 1 ndarray
:returns: Fiducial Registration Error (FRE)
"""
validate_procrustes_inputs(fixed, moving)
transformed_moving = np.matmul(rotation, moving.transpose()) + translation
squared_error_elementwise = np.square(fixed
- transformed_moving.transpose())
square_distance_error = np.sum(squared_error_elementwise, 1)
sum_squared_error = np.sum(square_distance_error, 0)
mean_squared_error = sum_squared_error / fixed.shape[0]
fre = np.sqrt(mean_squared_error)
return fre
# pylint: disable=invalid-name, line-too-long
def orthogonal_procrustes(fixed, moving):
"""
Implements point based registration via the Orthogonal Procrustes method.
Based on Arun's method:
Least-Squares Fitting of two, 3-D Point Sets, Arun, 1987,
`10.1109/TPAMI.1987.4767965 <http://dx.doi.org/10.1109/TPAMI.1987.4767965>`_.
Also see `this <http://eecs.vanderbilt.edu/people/mikefitzpatrick/papers/2009_Medim_Fitzpatrick_TRE_FRE_uncorrelated_as_published.pdf>`_
and `this <http://tango.andrew.cmu.edu/~gustavor/42431-intro-bioimaging/readings/ch8.pdf>`_.
:param fixed: point set, N x 3 ndarray
:param moving: point set, N x 3 ndarray of corresponding points
:returns: 3x3 rotation ndarray, 3x1 translation ndarray, FRE
:raises: ValueError
"""
validate_procrustes_inputs(fixed, moving)
# This is what we are calculating
R = np.eye(3)
T = np.zeros((3, 1))
# Arun equation 4
p = np.ndarray.mean(moving, 0)
# Arun equation 6
p_prime = np.ndarray.mean(fixed, 0)
# Arun equation 7
q = moving - p
# Arun equation 8
q_prime = fixed - p_prime
# Arun equation 11
H = np.matmul(q.transpose(), q_prime)
# Arun equation 12
# Note: numpy factors h = u * np.diag(s) * v
svd = np.linalg.svd(H)
# Arun equation 13
X = np.matmul(svd[2].transpose(), svd[0].transpose())
# Arun step 5, after equation 13.
det_X = np.linalg.det(X)
if det_X < 0 and np.all(np.flip(np.isclose(svd[1], np.zeros((3, 1))))):
# Don't yet know how to generate test data.
# If you hit this line, please report it, and save your data.
raise ValueError("Registration fails as determinant < 0"
" and no singular values are close enough to zero")
elif det_X < 0 and np.any(np.isclose(svd[1], np.zeros((3, 1)))):
# Implement 2a in section VI in Arun paper.
v_prime = svd[2].transpose()
v_prime[0][2] *= -1
v_prime[1][2] *= -1
v_prime[2][2] *= -1
X = np.matmul(v_prime, svd[0].transpose())
# Compute output
R = X
tmp = p_prime.transpose() - np.matmul(R, p.transpose())
T[0][0] = tmp[0]
T[1][0] = tmp[1]
T[2][0] = tmp[2]
fre = compute_fre(fixed, moving, R, T)
return R, T, fre
# -*- coding: utf-8 -*-
import six
import numpy as np
import pytest
import sksurgerycore.algorithms.procrustes as p
def test_empty_fixed():
with pytest.raises(TypeError):
p.orthogonal_procrustes(None, np.ones((1, 3)))
def test_empty_moving():
with pytest.raises(TypeError):
p.orthogonal_procrustes(np.ones((1, 3)), None)
def test_three_columns_fixed():
with pytest.raises(ValueError):
p.orthogonal_procrustes(np.ones((3, 4)), np.ones((3, 3)))
def test_three_columns_moving():
with pytest.raises(ValueError):
p.orthogonal_procrustes(np.ones((3, 3)), np.ones((3, 4)))
def test_at_least_three_points_fixed():
with pytest.raises(ValueError):
p.orthogonal_procrustes(np.ones((1, 3)), np.ones((3, 3)))
def test_at_least_three_points_moving():
with pytest.raises(ValueError):
p.orthogonal_procrustes(np.ones((3, 3)), np.ones((1, 3)))
def test_same_number_points():
with pytest.raises(ValueError):
p.orthogonal_procrustes(np.ones((4, 3)), np.ones((5, 3)))
def test_identity_result():
fixed = np.zeros((3, 3))
fixed[0][1] = 1
fixed[2][0] = 2
moving = np.zeros((3, 3))
moving[0][1] = 1
moving[2][0] = 2
rotation, translation, error = p.orthogonal_procrustes(fixed, moving)
assert np.allclose(rotation, np.eye(3), 0.0000001)
assert np.allclose(translation, np.zeros((3, 1)), 0.0000001)
assert error < 0.0000001
def test_reflection_data():
fixed = np.zeros((4, 3))
fixed[0][1] = 1
fixed[2][0] = 2
fixed[3][0] = 4
moving = np.zeros((4, 3))
moving[0][1] = 1
moving[2][0] = -2
moving[3][0] = -4
rotation, translation, error = p.orthogonal_procrustes(fixed, moving)
expected_rotation = np.eye(3)
expected_rotation[0][0] = -1
expected_rotation[2][2] = -1
assert np.allclose(rotation, expected_rotation, 0.0000001)
assert np.allclose(translation, np.zeros((3, 1)), 0.0000001)
assert error < 0.0000001
def test_translation_result():
fixed = np.zeros((3, 3))
fixed[0][1] = 1
fixed[2][0] = 2
moving = np.zeros((3, 3))
moving[0][1] = 1
moving[2][0] = 2
moving[0][2] += 1
moving[1][2] += 1
moving[2][2] += 1
expected_translation = np.zeros((3, 1))
expected_translation[2][0] = -1
rotation, translation, error = p.orthogonal_procrustes(fixed, moving)
assert np.allclose(rotation, np.eye(3), 0.0000001)
assert np.allclose(translation, expected_translation, 0.0000001)
assert error < 0.0000001
def test_rotation_result():
fixed = np.zeros((5, 3))
fixed[0][0] = 39.3047
fixed[0][1] = 71.7057
fixed[0][2] = 372.72
fixed[1][0] = 171.844
fixed[1][1] = 65.8063
fixed[1][2] = 376.325
fixed[2][0] = 312.044
fixed[2][1] = 77.5614
fixed[2][2] = 196
fixed[3][0] = 176.528
fixed[3][1] = 78.7922
fixed[3][2] = 8
fixed[4][0] = 43.4659
fixed[4][1] = 53.2688
fixed[4][2] = 10.5
moving = np.zeros((5, 3))
moving[0][0] = 192.833
moving[0][1] = 290.286
moving[0][2] = 155.423
moving[1][0] = 295.688
moving[1][1] = 287.97
moving[1][2] = 71.5776
moving[2][0] = 301.155
moving[2][1] = 183.623
moving[2][2] = -131.876
moving[3][0] = 102.167
moving[3][1] = 66.2676
moving[3][2] = -150.349
moving[4][0] = 15.3302
moving[4][1] = 48.0055
moving[4][2] = -47.9328
expected_translation = np.zeros((3, 1))
expected_rotation = np.zeros((3, 3))
expected_rotation[0][0] = 0.7431
expected_rotation[0][1] = 0
expected_rotation[0][2] = -0.6691
expected_rotation[1][0] = -0.4211
expected_rotation[1][1] = 0.7771
expected_rotation[1][2] = -0.4677
expected_rotation[2][0] = 0.52
expected_rotation[2][1] = 0.6293
expected_rotation[2][2] = 0.5775
rotation, translation, error = p.orthogonal_procrustes(fixed, moving)
assert np.allclose(expected_translation, translation, 0.001, 0.001)
assert np.allclose(expected_rotation, rotation, 0.001, 0.001)
assert error < 0.001
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment