Commit 469fc7af by 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 `_. Also see `this `_ and `this `_. :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!