Commit efe762bb authored by Matt Clarkson's avatar Matt Clarkson

Merge branch '13-implement-pivot-calibration' into 'master'

Resolve "Implement Pivot Calibration"

Closes #13

See merge request WEISS/SoftwareRepositories/SNAPPY/scikit-surgerycore!8
parents d4515bf2 90764ad6
Pipeline #1608 passed with stages
in 51 minutes and 17 seconds

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]
......@@ -123,7 +123,7 @@ def orthogonal_procrustes(fixed, moving):
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)))):
if 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()
......@@ -132,6 +132,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