3838325990

Build Type

push

github-actions

Committed by Miguel Xochicale

Commit Message

uses triangulate_points_opencv in video_calibration_metric.py (#46)

Run Details

1 of 1 new or added line in 1 file covered. (100.0%)

1725 of 2002 relevant lines covered (86.16%)

4.31 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

97.76

/sksurgerycalibration/algorithms/pivot.py

#  -*- coding: utf-8 -*-

""" Functions for pivot calibration. """

import random
import numpy as np
from sksurgerycalibration.algorithms.sphere_fitting import \
                fit_sphere_least_squares


def pivot_calibration(tracking_matrices, configuration=None):
    """
    Performs pivot calibration on an array of tracking matrices

    :param tracking_matrices: an Nx4x4 array of tracking matrices
    :param configuration: an optional configuration dictionary, if not
        the algorithm defaults to Algebraic One Step. Other options
        include ransac, and sphere_fitting

    :returns: tuple containing;
            'pointer_offset' The coordinate of the pointer tip relative to
            the tracking centre
            'pivot_point' The location of the pivot point in world coordinates
            'residual_error' The RMS pointer tip error, errors in
            each direction are treated as independent variables, so for a
            calibration with n matrices, RMS error is calculated using
            nx3 measurements.

    :raises: TypeError, ValueError
    """

    if not isinstance(tracking_matrices, np.ndarray):
        raise TypeError("tracking_matrices is not a numpy array'")

    if not tracking_matrices.shape[1] == 4:
        raise ValueError("tracking_matrices should have 4 rows per matrix")

    if not tracking_matrices.shape[2] == 4:
        raise ValueError("tracking_matrices should have 4 columns per matrix")

    use_algebraic_one_step = False
    use_ransac = False
    use_sphere_fitting = False

    if configuration is not None:
        use_algebraic_one_step = False
        method = configuration.get('method', 'aos')
        if method == 'aos':
            use_algebraic_one_step = True
        elif method == 'ransac':
            use_ransac = True
        elif method == 'sphere_fitting':
            use_sphere_fitting = True
    else:
        use_algebraic_one_step = True

    if use_algebraic_one_step:
        return pivot_calibration_aos(tracking_matrices)

    if use_ransac:
        number_iterations = configuration.get('number_iterations', 10)
        error_threshold = configuration.get('error_threshold', 4)
        consensus_threshold = configuration.get('consensus_threshold',
                                                0.25)
        early_exit = configuration.get('early_exit', False)
        return pivot_calibration_with_ransac(
            tracking_matrices, number_iterations, error_threshold,
            consensus_threshold, early_exit)

    if use_sphere_fitting:
        init_parameters = configuration.get('init_parameters', None)
        return pivot_calibration_sphere_fit(tracking_matrices, init_parameters)

    raise ValueError("method key set to unknown method; ",
                     configuration.get('method', 'aos'))


def pivot_calibration_aos(tracking_matrices):

    """
    Performs Pivot Calibration, using Algebraic One Step method,
    and returns Residual Error.

    See `Yaniv 2015 <https://dx.doi.org/10.1117/12.2081348>`_.

    :param tracking_matrices: N x 4 x 4 ndarray, of tracking matrices.
    :returns: pointer offset, pivot point and RMS Error about centroid of pivot.
    :raises: ValueError if rank less than 6
    """

    # See equation in section 2.1.2 of Yaniv 2015.
    # Ax = b.

    a_values, b_values = _matrices_to_a_and_b(tracking_matrices)

    # 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.dot(np.diag(1 / s_values), c_values)
    x_values = np.dot(v_values.T, w_values)

    # Calculating the rank, and removing close to zero singular values.
    rank = _replace_small_values(s_values, 0.01, 0.0)

    if rank < 6:
        raise ValueError("PivotCalibration: Failed. Rank < 6")

    pointer_offset = x_values[0:3]
    pivot_location = x_values[3:6]
    residual_error = _residual_error_direct(a_values, b_values, x_values)

    return pointer_offset, pivot_location, residual_error


def pivot_calibration_with_ransac(tracking_matrices,
                                  number_iterations,
                                  error_threshold,
                                  concensus_threshold,
                                  early_exit=False
                                  ):
    """
    Written as an exercise for implementing RANSAC.

    :param tracking_matrices: N x 4 x 4 ndarray, of tracking matrices.
    :param number_iterations: the number of iterations to attempt.
    :param error_threshold: distance in millimetres from pointer position
    :param concensus_threshold: the minimum percentage of inliers to finish
    :param early_exit: If True, returns model as soon as thresholds are met
    :returns: pointer offset, pivot point and RMS Error about centroid of pivot.
    :raises: TypeError, ValueError
    """
    if number_iterations < 1:
        raise ValueError("The number of iterations must be > 1")
    if error_threshold < 0:
        raise ValueError("The error threshold must be a positive distance.")
    if concensus_threshold < 0 or concensus_threshold > 1:
        raise ValueError("The concensus threshold must be [0-1] as percentage")
    if not isinstance(tracking_matrices, np.ndarray):
        raise TypeError("tracking_matrices is not a numpy array'")

    number_of_matrices = tracking_matrices.shape[0]
    population_of_indices = range(number_of_matrices)
    minimum_matrices_required = 3

    highest_number_of_inliers = -1
    best_pointer_offset = None
    best_pivot_location = None
    best_residual_error = -1

    for iter_counter in range(number_iterations):
        indexes = random.sample(population_of_indices,
                                minimum_matrices_required)
        sample = tracking_matrices[indexes]

        try:
            pointer_offset, pivot_location, _ = pivot_calibration(sample)
        except ValueError:
            print("RANSAC, iteration " + str(iter_counter) + ", failed.")
            continue

        # Need to evaluate the number of inliers.
        # Slow, but it's written as a teaching exercise.
        number_of_inliers = 0
        inlier_indices = []
        for matrix_counter in range(number_of_matrices):
            offset = np.vstack((pointer_offset, 1))
            transformed_point = tracking_matrices[matrix_counter] @ offset
            diff = pivot_location - transformed_point[0:3]
            norm = np.linalg.norm(diff)
            if norm < error_threshold:
                number_of_inliers = number_of_inliers + 1
                inlier_indices.append(matrix_counter)

        percentage_inliers = number_of_inliers / number_of_matrices

        # Keep the best model so far, based on the highest number of inliers.
        if percentage_inliers > concensus_threshold \
                and number_of_inliers > highest_number_of_inliers:
            highest_number_of_inliers = number_of_inliers
            inlier_matrices = tracking_matrices[inlier_indices]
            best_pointer_offset, best_pivot_location, best_residual_error = \
                pivot_calibration(inlier_matrices)

        # Early exit condition, as soon as we find model with enough fit.
        if percentage_inliers > concensus_threshold and early_exit:
            return best_pointer_offset, best_pivot_location, best_residual_error

    if best_pointer_offset is None:
        raise ValueError("Failed to find a model using RANSAC.")

    print("RANSAC Pivot, from " + str(number_of_matrices)
          + " matrices, used " + str(highest_number_of_inliers)
          + " matrices, with error threshold = " + str(error_threshold)
          + " and consensus threshold = " + str(concensus_threshold)
          )

    return best_pointer_offset, best_pivot_location, best_residual_error


def pivot_calibration_sphere_fit(tracking_matrices, init_parameters=None):

    """
    Performs Pivot Calibration, using sphere fitting, based on

    See `Yaniv 2015 <https://dx.doi.org/10.1117/12.2081348>`_.

    :param tracking_matrices: N x 4 x 4 ndarray, of tracking matrices.
    :param init_parameters: 1X4 array of initial parameter for finding the
        pivot point in world coords and pivot radius. Default is to set to
        the mean x,y,z values and radius = 0.
    :returns: pointer offset, pivot point and RMS Error about centroid of pivot.
    """
    residual_error = -1.0
    translations = tracking_matrices[:, 0:3, 3]

    # first find the pivot point in world coordinates using sphere fitting
    if init_parameters is None:
        means = np.mean(translations, 0)
        init_parameters = np.concatenate([means, np.zeros((1))])

    result = fit_sphere_least_squares(translations, init_parameters)
    pivot_point = result.x[0:3]

    # now calculate the mean offset
    rotations = tracking_matrices[:, 0:3, 0:3]
    offsets = np.zeros((tracking_matrices.shape[0], 3))

    for index, rot in enumerate(rotations):
        offsets[index] = rot.transpose() @ (pivot_point - translations[index])

    pointer_offset = np.mean(offsets, 0)

    residual_error = _residual_error(tracking_matrices, pointer_offset,
                                     pivot_point)

    return pointer_offset, pivot_point, residual_error


def _matrices_to_a_and_b(tracking_matrices):
    """
    Helper function to convert tracking matrices into
    a_values and b_values that can be used for aos calibration or
    for calculating residuals.

    :param tracking_matrices: nx4x4 tracking matrices
    :returns: a_values, (nx3)x6 array of rotation and -Identity, b_values,
    an nx3 column vector of translations
    """
    number_of_matrices = tracking_matrices.shape[0]
    # A contains rotation matrix from each tracking matrix.
    # and -I for each tracking matrix.
    size_a = 3 * number_of_matrices, 3
    a_first = (tracking_matrices [:, 0:3, 0:3]).reshape(size_a)
    a_second = (np.eye(3) * -1.0).reshape((1, 3, 3)).repeat(
        number_of_matrices, 0).reshape(size_a)
    a_values = np.concatenate((a_first, a_second), axis=1)

    # Column vector containing -1 * translation from each tracking matrix.
    size_b = 3 * number_of_matrices, 1
    b_values = (tracking_matrices[:, 0:3, 3] * -1.0).reshape((size_b))

    return a_values, b_values


def _residual_error(tracking_matrices, pointer_offset, pivot_location):
    """
    Helper function to calculate resdiual (RMS) errors.

    :params tracking_matrices: nx4x4 array
    :params pointer_offset: 1x3 array
    :params pivot_location: 1x3 array
    :returns: The RMS residual error
    """
    x_values = np.concatenate([pointer_offset, pivot_location],
                              axis=0).reshape((6, 1))
    a_values, b_values = _matrices_to_a_and_b(tracking_matrices)
    return _residual_error_direct(a_values, b_values, x_values)


def _residual_error_direct(a_values, b_values, x_values):
    """
    Helper function to calculate resdidual (RMS) errors.

    :params a_values: (nx3)x6 array of rotation and -Identity,
    :params b_values: an nx3 column vector of translations
    :params x_values: nx6 array, pointer_offset and pivot_point
    :returns: TRhe RMS residual error
    """
    residual_matrix = (np.dot(a_values, x_values) - b_values)
    residual_error = np.mean(residual_matrix * residual_matrix)
    residual_error = np.sqrt(residual_error)
    return residual_error


def _replace_small_values(the_list, threshold=0.01, replacement_value=0.0):
    """
    replace small values in a list, this changes the list in place.

    :param the_list: the list to process.
    :param threshold: replace values lower than threshold.
    :param replacement_value: value to replace with.
    :returns: the number of items not replaced.
    """
    rank = 0
    for index, item in enumerate(the_list):
        if item < threshold:
            the_list[index] = replacement_value
        else:
            rank += 1

    return rank

1	# -- coding: utf-8 --
2
3	""" Functions for pivot calibration. """	5✔
4
5	import random	5✔
6	import numpy as np	5✔
7	from sksurgerycalibration.algorithms.sphere_fitting import \	5✔
8	fit_sphere_least_squares
9
10
11	def pivot_calibration(tracking_matrices, configuration=None):	5✔
12	"""
13	Performs pivot calibration on an array of tracking matrices
14
15	:param tracking_matrices: an Nx4x4 array of tracking matrices
16	:param configuration: an optional configuration dictionary, if not
17	the algorithm defaults to Algebraic One Step. Other options
18	include ransac, and sphere_fitting
19
20	:returns: tuple containing;
21	'pointer_offset' The coordinate of the pointer tip relative to
22	the tracking centre
23	'pivot_point' The location of the pivot point in world coordinates
24	'residual_error' The RMS pointer tip error, errors in
25	each direction are treated as independent variables, so for a
26	calibration with n matrices, RMS error is calculated using
27	nx3 measurements.
28
29	:raises: TypeError, ValueError
30	"""
31
32	if not isinstance(tracking_matrices, np.ndarray):	5✔
33	raise TypeError("tracking_matrices is not a numpy array'")	5✔
34
35	if not tracking_matrices.shape[1] == 4:	5✔
36	raise ValueError("tracking_matrices should have 4 rows per matrix")	5✔
37
38	if not tracking_matrices.shape[2] == 4:	5✔
39	raise ValueError("tracking_matrices should have 4 columns per matrix")	5✔
40
41	use_algebraic_one_step = False	5✔
42	use_ransac = False	5✔
43	use_sphere_fitting = False	5✔
44
45	if configuration is not None:	5✔
46	use_algebraic_one_step = False	5✔
47	method = configuration.get('method', 'aos')	5✔
48	if method == 'aos':	5✔
49	use_algebraic_one_step = True	5✔
50	elif method == 'ransac':	5✔
51	use_ransac = True	5✔
52	elif method == 'sphere_fitting':	5✔
53	use_sphere_fitting = True	5✔
54	else:
55	use_algebraic_one_step = True	5✔
56
57	if use_algebraic_one_step:	5✔
58	return pivot_calibration_aos(tracking_matrices)	5✔
59
60	if use_ransac:	5✔
61	number_iterations = configuration.get('number_iterations', 10)	5✔
62	error_threshold = configuration.get('error_threshold', 4)	5✔
63	consensus_threshold = configuration.get('consensus_threshold',	5✔
64	0.25)
65	early_exit = configuration.get('early_exit', False)	5✔
66	return pivot_calibration_with_ransac(	5✔
67	tracking_matrices, number_iterations, error_threshold,
68	consensus_threshold, early_exit)
69
70	if use_sphere_fitting:	5✔
71	init_parameters = configuration.get('init_parameters', None)	5✔
72	return pivot_calibration_sphere_fit(tracking_matrices, init_parameters)	5✔
73
74	raise ValueError("method key set to unknown method; ",	5✔
75	configuration.get('method', 'aos'))
76
77
78	def pivot_calibration_aos(tracking_matrices):	5✔
79
80	"""
81	Performs Pivot Calibration, using Algebraic One Step method,
82	and returns Residual Error.
83
84	See `Yaniv 2015 <https://dx.doi.org/10.1117/12.2081348>`_.
85
86	:param tracking_matrices: N x 4 x 4 ndarray, of tracking matrices.
87	:returns: pointer offset, pivot point and RMS Error about centroid of pivot.
88	:raises: ValueError if rank less than 6
89	"""
90
91	# See equation in section 2.1.2 of Yaniv 2015.
92	# Ax = b.
93
94	a_values, b_values = _matrices_to_a_and_b(tracking_matrices)	5✔
95
96	# To calculate Singular Value Decomposition
97
98	u_values, s_values, v_values = np.linalg.svd(a_values, full_matrices=False)	5✔
99	c_values = np.dot(u_values.T, b_values)	5✔
100	w_values = np.dot(np.diag(1 / s_values), c_values)	5✔
101	x_values = np.dot(v_values.T, w_values)	5✔
102
103	# Calculating the rank, and removing close to zero singular values.
104	rank = _replace_small_values(s_values, 0.01, 0.0)	5✔
105
106	if rank < 6:	5✔
107	raise ValueError("PivotCalibration: Failed. Rank < 6")	5✔
108
109	pointer_offset = x_values[0:3]	5✔
110	pivot_location = x_values[3:6]	5✔
111	residual_error = _residual_error_direct(a_values, b_values, x_values)	5✔
112
113	return pointer_offset, pivot_location, residual_error	5✔
114
115
116	def pivot_calibration_with_ransac(tracking_matrices,	5✔
117	number_iterations,
118	error_threshold,
119	concensus_threshold,
120	early_exit=False
121	):
122	"""
123	Written as an exercise for implementing RANSAC.
124
125	:param tracking_matrices: N x 4 x 4 ndarray, of tracking matrices.
126	:param number_iterations: the number of iterations to attempt.
127	:param error_threshold: distance in millimetres from pointer position
128	:param concensus_threshold: the minimum percentage of inliers to finish
129	:param early_exit: If True, returns model as soon as thresholds are met
130	:returns: pointer offset, pivot point and RMS Error about centroid of pivot.
131	:raises: TypeError, ValueError
132	"""
133	if number_iterations < 1:	5✔
134	raise ValueError("The number of iterations must be > 1")	5✔
135	if error_threshold < 0:	5✔
136	raise ValueError("The error threshold must be a positive distance.")	5✔
137	if concensus_threshold < 0 or concensus_threshold > 1:	5✔
138	raise ValueError("The concensus threshold must be [0-1] as percentage")	5✔
139	if not isinstance(tracking_matrices, np.ndarray):	5✔
140	raise TypeError("tracking_matrices is not a numpy array'")	5✔
141
142	number_of_matrices = tracking_matrices.shape[0]	5✔
143	population_of_indices = range(number_of_matrices)	5✔
144	minimum_matrices_required = 3	5✔
145
146	highest_number_of_inliers = -1	5✔
147	best_pointer_offset = None	5✔
148	best_pivot_location = None	5✔
149	best_residual_error = -1	5✔
150
151	for iter_counter in range(number_iterations):	5✔
152	indexes = random.sample(population_of_indices,	5✔
153	minimum_matrices_required)
154	sample = tracking_matrices[indexes]	5✔
155
156	try:	5✔
157	pointer_offset, pivot_location, _ = pivot_calibration(sample)	5✔
158	except ValueError:	×
159	print("RANSAC, iteration " + str(iter_counter) + ", failed.")	×
160	continue	×
161
162	# Need to evaluate the number of inliers.
163	# Slow, but it's written as a teaching exercise.
164	number_of_inliers = 0	5✔
165	inlier_indices = []	5✔
166	for matrix_counter in range(number_of_matrices):	5✔
167	offset = np.vstack((pointer_offset, 1))	5✔
168	transformed_point = tracking_matrices[matrix_counter] @ offset	5✔
169	diff = pivot_location - transformed_point[0:3]	5✔
170	norm = np.linalg.norm(diff)	5✔
171	if norm < error_threshold:	5✔
172	number_of_inliers = number_of_inliers + 1	5✔
173	inlier_indices.append(matrix_counter)	5✔
174
175	percentage_inliers = number_of_inliers / number_of_matrices	5✔
176
177	# Keep the best model so far, based on the highest number of inliers.
178	if percentage_inliers > concensus_threshold \	5✔
179	and number_of_inliers > highest_number_of_inliers:
180	highest_number_of_inliers = number_of_inliers	5✔
181	inlier_matrices = tracking_matrices[inlier_indices]	5✔
182	best_pointer_offset, best_pivot_location, best_residual_error = \	5✔
183	pivot_calibration(inlier_matrices)
184
185	# Early exit condition, as soon as we find model with enough fit.
186	if percentage_inliers > concensus_threshold and early_exit:	5✔
187	return best_pointer_offset, best_pivot_location, best_residual_error	5✔
188
189	if best_pointer_offset is None:	5✔
190	raise ValueError("Failed to find a model using RANSAC.")	5✔
191
192	print("RANSAC Pivot, from " + str(number_of_matrices)	5✔
193	+ " matrices, used " + str(highest_number_of_inliers)
194	+ " matrices, with error threshold = " + str(error_threshold)
195	+ " and consensus threshold = " + str(concensus_threshold)
196	)
197
198	return best_pointer_offset, best_pivot_location, best_residual_error	5✔
199
200
201	def pivot_calibration_sphere_fit(tracking_matrices, init_parameters=None):	5✔
202
203	"""
204	Performs Pivot Calibration, using sphere fitting, based on
205
206	See `Yaniv 2015 <https://dx.doi.org/10.1117/12.2081348>`_.
207
208	:param tracking_matrices: N x 4 x 4 ndarray, of tracking matrices.
209	:param init_parameters: 1X4 array of initial parameter for finding the
210	pivot point in world coords and pivot radius. Default is to set to
211	the mean x,y,z values and radius = 0.
212	:returns: pointer offset, pivot point and RMS Error about centroid of pivot.
213	"""
214	residual_error = -1.0	5✔
215	translations = tracking_matrices[:, 0:3, 3]	5✔
216
217	# first find the pivot point in world coordinates using sphere fitting
218	if init_parameters is None:	5✔
219	means = np.mean(translations, 0)	5✔
220	init_parameters = np.concatenate([means, np.zeros((1))])	5✔
221
222	result = fit_sphere_least_squares(translations, init_parameters)	5✔
223	pivot_point = result.x[0:3]	5✔
224
225	# now calculate the mean offset
226	rotations = tracking_matrices[:, 0:3, 0:3]	5✔
227	offsets = np.zeros((tracking_matrices.shape[0], 3))	5✔
228
229	for index, rot in enumerate(rotations):	5✔
230	offsets[index] = rot.transpose() @ (pivot_point - translations[index])	5✔
231
232	pointer_offset = np.mean(offsets, 0)	5✔
233
234	residual_error = _residual_error(tracking_matrices, pointer_offset,	5✔
235	pivot_point)
236
237	return pointer_offset, pivot_point, residual_error	5✔
238
239
240	def _matrices_to_a_and_b(tracking_matrices):	5✔
241	"""
242	Helper function to convert tracking matrices into
243	a_values and b_values that can be used for aos calibration or
244	for calculating residuals.
245
246	:param tracking_matrices: nx4x4 tracking matrices
247	:returns: a_values, (nx3)x6 array of rotation and -Identity, b_values,
248	an nx3 column vector of translations
249	"""
250	number_of_matrices = tracking_matrices.shape[0]	5✔
251	# A contains rotation matrix from each tracking matrix.
252	# and -I for each tracking matrix.
253	size_a = 3 * number_of_matrices, 3	5✔
254	a_first = (tracking_matrices [:, 0:3, 0:3]).reshape(size_a)	5✔
255	a_second = (np.eye(3) * -1.0).reshape((1, 3, 3)).repeat(	5✔
256	number_of_matrices, 0).reshape(size_a)
257	a_values = np.concatenate((a_first, a_second), axis=1)	5✔
258
259	# Column vector containing -1 * translation from each tracking matrix.
260	size_b = 3 * number_of_matrices, 1	5✔
261	b_values = (tracking_matrices[:, 0:3, 3] * -1.0).reshape((size_b))	5✔
262
263	return a_values, b_values	5✔
264
265
266	def _residual_error(tracking_matrices, pointer_offset, pivot_location):	5✔
267	"""
268	Helper function to calculate resdiual (RMS) errors.
269
270	:params tracking_matrices: nx4x4 array
271	:params pointer_offset: 1x3 array
272	:params pivot_location: 1x3 array
273	:returns: The RMS residual error
274	"""
275	x_values = np.concatenate([pointer_offset, pivot_location],	5✔
276	axis=0).reshape((6, 1))
277	a_values, b_values = _matrices_to_a_and_b(tracking_matrices)	5✔
278	return _residual_error_direct(a_values, b_values, x_values)	5✔
279
280
281	def _residual_error_direct(a_values, b_values, x_values):	5✔
282	"""
283	Helper function to calculate resdidual (RMS) errors.
284
285	:params a_values: (nx3)x6 array of rotation and -Identity,
286	:params b_values: an nx3 column vector of translations
287	:params x_values: nx6 array, pointer_offset and pivot_point
288	:returns: TRhe RMS residual error
289	"""
290	residual_matrix = (np.dot(a_values, x_values) - b_values)	5✔
291	residual_error = np.mean(residual_matrix * residual_matrix)	5✔
292	residual_error = np.sqrt(residual_error)	5✔
293	return residual_error	5✔
294
295
296	def _replace_small_values(the_list, threshold=0.01, replacement_value=0.0):	5✔
297	"""
298	replace small values in a list, this changes the list in place.
299
300	:param the_list: the list to process.
301	:param threshold: replace values lower than threshold.
302	:param replacement_value: value to replace with.
303	:returns: the number of items not replaced.
304	"""
305	rank = 0	5✔
306	for index, item in enumerate(the_list):	5✔
307	if item < threshold:	5✔
308	the_list[index] = replacement_value	5✔
309	else:
310	rank += 1	5✔
311
312	return rank	5✔

SciKit-Surgery / scikit-surgerycalibration / 3838325990

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous