Commit 98b6ac4e authored by Matt Clarkson's avatar Matt Clarkson

Merge branch 'master' into '14-fiducial-registration-error-computation'

# Conflicts:
#   sksurgerycore/algorithms/procrustes.py
parents 2ae6754a efe762bb

Too many changes to show.

To preserve performance only 1000 of 1000+ files are displayed.

File added
# -*- coding: utf-8 -*-
"""Functions for pivot calibration."""
import numpy as np
def pivot_calibration(matrices4x4):
"""
Performs Pivot Calibration and returns Residual Error.
1. matricesNx4x4 and moving must be numpy array
2. matricesNx4x4 and moving should have 4 columns
3. matricesNx4x4 and moving should have at least 4 rows
:param matrices4x4: point set, N x 4 x 4 ndarray
:returns: residual_error
:raises: TypeError, ValueError
"""
if not isinstance(matrices4x4, np.ndarray):
raise TypeError("matrices4x4 is not a numpy array'")
if not matrices4x4.shape[1] == 4: # pylint: disable=literal-comparison
raise ValueError("matrices4x4 should have 4 columns per matrix")
number_of_matrices = len(matrices4x4)
size_a = 3 * number_of_matrices, 6
a_values = np.zeros(size_a, dtype=np.float64)
size_x = 6, 1
x_values = np.zeros(size_x, dtype=np.float64)
size_b = 3 * number_of_matrices, 1
b_values = np.zeros(size_b, dtype=np.float64)
for i in range(number_of_matrices):
b_values[i * 3 + 0, 0] = -1 * matrices4x4[i, 0, 3]
b_values[i * 3 + 1, 0] = -1 * matrices4x4[i, 1, 3]
b_values[i * 3 + 2, 0] = -1 * matrices4x4[i, 2, 3]
a_values[i * 3 + 0, 0] = matrices4x4[i, 0, 0]
a_values[i * 3 + 1, 0] = matrices4x4[i, 1, 0]
a_values[i * 3 + 2, 0] = matrices4x4[i, 2, 0]
a_values[i * 3 + 0, 1] = matrices4x4[i, 0, 1]
a_values[i * 3 + 1, 1] = matrices4x4[i, 1, 1]
a_values[i * 3 + 2, 1] = matrices4x4[i, 2, 1]
a_values[i * 3 + 0, 2] = matrices4x4[i, 0, 2]
a_values[i * 3 + 1, 2] = matrices4x4[i, 1, 2]
a_values[i * 3 + 2, 2] = matrices4x4[i, 2, 2]
a_values[i * 3 + 0, 3] = -1
a_values[i * 3 + 1, 3] = 0
a_values[i * 3 + 2, 3] = 0
a_values[i * 3 + 0, 4] = 0
a_values[i * 3 + 1, 4] = -1
a_values[i * 3 + 2, 4] = 0
a_values[i * 3 + 0, 5] = 0
a_values[i * 3 + 1, 5] = 0
a_values[i * 3 + 2, 5] = -1
# To calculate Singular Value Decomposition
u_values, s_values, v_values = np.linalg.svd(a_values, full_matrices=False)
c_values = np.dot(u_values.T, b_values)
w_values = np.linalg.solve(np.diag(s_values), c_values)
x_values = np.dot(v_values.T, w_values)
# Back substitution
# Calculating the rank
rank = 0
for i, item in enumerate(s_values):
if item < 0.01:
item = 0
if item != 0:
rank += 1
if rank < 6: # pylint: disable=literal-comparison
raise ValueError("PivotCalibration: Failed. Rank < 6")
# Residual Matrix
residual_matrix = (np.dot(a_values, x_values) - b_values)
residual_error = 0.0
for i in range(number_of_matrices * 3):
residual_error = residual_error + \
np.dot(residual_matrix[i, 0],
residual_matrix[i, 0])
residual_error = residual_error / float(number_of_matrices * 3)
residual_error = np.sqrt(residual_error)
# Output
# MakeIdentity matrix
output_matrix = np.identity(4)
output_matrix[0, 3] = x_values[0, 0]
output_matrix[1, 3] = x_values[1, 0]
output_matrix[2, 3] = x_values[2, 0]
print("pivotCalibration=(", x_values[3, 0], ","
, x_values[4, 0], ",", x_values[5, 0],
"),residual=", residual_error)
return residual_error, x_values[0, 0], x_values[1, 0], x_values[2, 0]
......@@ -75,6 +75,7 @@ def orthogonal_procrustes(fixed, moving):
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())
......
# -*- coding: utf-8 -*-
# import six
import numpy as np
import pytest
import sksurgerycore.algorithms.pivot as p
from glob import glob
def test_empty_matrices4x4():
with pytest.raises(TypeError):
p.pivot_calibration(None)
def test_rank_lt_six():
with pytest.raises(ValueError):
file_names = glob('tests/data/PivotCalibration/1378476416922755200.txt')
arrays = [np.loadtxt(f) for f in file_names]
matrices = np.concatenate(arrays)
numberOf4x4Matrices = int(matrices.size/16)
matrices4x4 = matrices.reshape(numberOf4x4Matrices, 4, 4)
p.pivot_calibration(matrices4x4)
def test_four_columns_matrices4x4():
with pytest.raises(ValueError):
p.pivot_calibration(np.arange(2, 11, dtype=float).reshape(3, 3))
def test_four_rows_matrices4x4():
with pytest.raises(ValueError):
p.pivot_calibration(np.arange(2, 11, dtype=float).reshape(3, 3))
def test_return_value():
file_names = glob('tests/data/PivotCalibration/*')
arrays = [np.loadtxt(f) for f in file_names]
matrices = np.concatenate(arrays)
numberOf4x4Matrices = int(matrices.size/16)
matrices4x4 = matrices.reshape(numberOf4x4Matrices, 4, 4)
residual_error, x_value_1, x_value_2, x_value_3 = p.pivot_calibration(matrices4x4)
assert 1.838 == round(residual_error, 3)
assert -14.476 == round(x_value_1, 3)
assert 395.143 == round(x_value_2, 3)
assert -7.558 == round(x_value_3, 3)
def test_rank_if_condition():
# This test will be checking a specific if condition.
# But at the moment I dont know what data I need
# To get proper s_values to cover that if condition.
with pytest.raises(ValueError):
file_names = glob('tests/data/test_case_data.txt')
arrays = [np.loadtxt(f) for f in file_names]
matrices = np.concatenate(arrays)
numberOf4x4Matrices = int(matrices.size/16)
matrices4x4 = matrices.reshape(numberOf4x4Matrices, 4, 4)
p.pivot_calibration(matrices4x4)
0.2324886024 -0.9449805021 0.2301324457 -425.5186462402
-0.9594704509 -0.1840910465 0.2133705467 -26.5338668823
-0.1592656821 -0.2704114914 -0.9494799376 -2017.9345703125
0.0000000000 0.0000000000 0.0000000000 1.0000000000
0.2327978313 -0.9450077415 0.2297075838 -425.5144042969
-0.9593902230 -0.1844672859 0.2134059817 -26.4292411804
-0.1592967659 -0.2700596750 -0.9495747685 -2018.0262451172
0.0000000000 0.0000000000 0.0000000000 1.0000000000
0.2330945134 -0.9450278878 0.2293234169 -425.5019531250
-0.9592206478 -0.1846664846 0.2139951885 -26.3663673401
-0.1598830670 -0.2698528767 -0.9495350718 -2018.1309814453
0.0000000000 0.0000000000 0.0000000000 1.0000000000