Commit d32b892b authored by Matt Clarkson's avatar Matt Clarkson

Merge branch '19-procrustes-reflection' into 'master'

Resolve "Reflection in Procrustes"

Closes #19

See merge request WEISS/SoftwareRepositories/SNAPPY/scikit-surgerycore!16
parents c7531ef9 e5aca6f8
Pipeline #3658 passed with stages
in 4 minutes and 34 seconds
......@@ -6,7 +6,6 @@ import numpy as np
from sksurgerycore.algorithms.errors \
import validate_procrustes_inputs, compute_fre
# pylint: disable=invalid-name, line-too-long
......@@ -53,8 +52,9 @@ def orthogonal_procrustes(fixed, moving):
# 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())
# Replace Arun Equation 13 with Fitzpatrick, chapter 8, page 470,
# to avoid reflections, see issue #19
X = _fitzpatricks_X(svd)
# Arun step 5, after equation 13.
det_X = np.linalg.det(X)
......@@ -67,7 +67,6 @@ def orthogonal_procrustes(fixed, moving):
" and no singular values are close enough to zero")
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()
v_prime[0][2] *= -1
......@@ -85,3 +84,18 @@ def orthogonal_procrustes(fixed, moving):
fre = compute_fre(fixed, moving, R, T)
return R, T, fre
def _fitzpatricks_X(svd):
"""This is from Fitzpatrick, chapter 8, page 470.
it's used in preference to Arun's equation 13,
X = np.matmul(svd[2].transpose(), svd[0].transpose())
to avoid reflections.
"""
VU = np.matmul(svd[2].transpose(), svd[0])
detVU = np.linalg.det(VU)
diag = np.eye(3, 3)
diag[2][2] = detVU
X = np.matmul(svd[2].transpose(), np.matmul(diag, svd[0].transpose()))
return X
......@@ -65,7 +65,10 @@ def test_identity_result():
def test_reflection_data():
"""This seems to be testing that a rotation
is prefered to a reflection. To transform these
points you can either reflect through yz, or
rotate 180 about y"""
fixed = np.zeros((4, 3))
fixed[0][1] = 1
fixed[2][0] = 2
......@@ -80,11 +83,60 @@ def test_reflection_data():
expected_rotation = np.eye(3)
expected_rotation[0][0] = -1
expected_rotation[2][2] = -1
print (np.linalg.det(rotation))
print (rotation)
assert np.allclose(rotation, expected_rotation, 0.0000001)
assert np.allclose(translation, np.zeros((3, 1)), 0.0000001)
assert error < 0.0000001
def test_reflection_BARD_data():
"""This is a reflection test using data taken from using
BARD at the 2019 summer school. This data set will give
you reflections, unless you replace equation 13 from Arun
with Fitzpatrick's modification. This was done at issue #19"""
ct_fids = np.zeros((4,3))
world_fids = np.zeros((4,3))
ct_fids[0][0] = 37.82
ct_fids[0][1] = 52.99
ct_fids[0][2] = -191.00
ct_fids[1][0] = 310.90
ct_fids[1][1] = 70.36
ct_fids[1][2] = -196.50
ct_fids[2][0] = 171.98
ct_fids[2][1] = 57.59
ct_fids[2][2] = -376.96
ct_fids[3][0] = 176.56
ct_fids[3][1] = 71.30
ct_fids[3][2] = -7.42
world_fids[0][0] = -7.060625292517708829e+01
world_fids[0][1] = -4.340731346419998715e+01
world_fids[0][2] = -7.678145283688323275e+01
world_fids[1][0] = -8.290290497289761618e+01
world_fids[1][1] = 2.365685716505672360e+02
world_fids[1][2] = -6.248063455571644909e+01
world_fids[2][0] = 1.061984137321565527e+02
world_fids[2][1] = 1.054365783777915624e+02
world_fids[2][2] = -6.347752061138085367e+01
world_fids[3][0] = -2.658595567434794020e+02
world_fids[3][1] = 9.556619277927123335e+01
world_fids[3][2] = -7.335614476329364209e+01
rotation, translation, error = p.orthogonal_procrustes(world_fids, ct_fids)
expected_rotation = np.eye(3)
expected_rotation[0][0] = -0.04933494
expected_rotation[0][1] = -0.01438556
expected_rotation[0][2] = -0.99867869
expected_rotation[1][0] = 0.99212873
expected_rotation[1][1] = 0.11451658
expected_rotation[1][2] = -0.05066094
expected_rotation[2][0] = 0.11509405
expected_rotation[2][1] = -0.99331717
expected_rotation[2][2] = 0.00862266
assert np.allclose(rotation, expected_rotation, 0.0000001)
def test_translation_result():
......
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