Commit f81f56bf authored by rmapaah's avatar rmapaah

Issue #13: bug removed

The array multiplication for numpy is np.dot(a, b) and not a*b. The error is corrected and unwanted transpose removed.
parent ec766db9
......@@ -3,10 +3,9 @@
"""Functions for pivot calibration."""
import numpy as np
# from numpy.linalg import svd
def pivot_calibration(matricesNx4x4):
def pivot_calibration(matrices4x4):
"""
Performs Pivot Calibration and returns Residual Error.
......@@ -14,21 +13,20 @@ def pivot_calibration(matricesNx4x4):
2. matricesNx4x4 and moving should have 4 columns
3. matricesNx4x4 and moving should have at least 4 rows
:param matricesNx4x4: point set, N x 4 x 4 ndarray
:param matrices4x4: point set, N x 4 x 4 ndarray
:returns: residual_error
:raises: TypeError, ValueError
"""
if not isinstance(matricesNx4x4, np.ndarray):
raise TypeError("matricesNx4x4 is not a numpy array'")
if not isinstance(matrices4x4, np.ndarray):
raise TypeError("matrices4x4 is not a numpy array'")
if not matricesNx4x4.shape[1] == 4: # pylint: disable=literal-comparison
raise ValueError("matricesNx4x4 should have 4 columns per matrix")
if not matrices4x4.shape[1] == 4: # pylint: disable=literal-comparison
raise ValueError("matrices4x4 should have 4 columns per matrix")
if not matricesNx4x4.shape[0] == 4: # pylint: disable=literal-comparison
raise ValueError("matricesNx4x4 should have 4 rows per matrix")
if not matrices4x4.shape[0] == 4: # pylint: disable=literal-comparison
raise ValueError("matrices4x4 should have 4 rows per matrix")
number_of_matrices = len(matricesNx4x4)
number_of_matrices = len(matrices4x4)
size_a = 3 * number_of_matrices, 6
a_values = np.zeros(size_a, dtype=np.float64)
......@@ -40,19 +38,19 @@ def pivot_calibration(matricesNx4x4):
b_values = np.zeros(size_b, dtype=np.float64)
for i in range(number_of_matrices):
b_values[i * 3 + 0, 0] = -1 * matricesNx4x4[i, 0, 3]
b_values[i * 3 + 1, 0] = -1 * matricesNx4x4[i, 1, 3]
b_values[i * 3 + 2, 0] = -1 * matricesNx4x4[i, 2, 3]
a_values[i * 3 + 0, 0] = matricesNx4x4[i, 0, 0]
a_values[i * 3 + 1, 0] = matricesNx4x4[i, 1, 0]
a_values[i * 3 + 2, 0] = matricesNx4x4[i, 2, 0]
a_values[i * 3 + 0, 1] = matricesNx4x4[i, 0, 1]
a_values[i * 3 + 1, 1] = matricesNx4x4[i, 1, 1]
a_values[i * 3 + 2, 1] = matricesNx4x4[i, 2, 1]
a_values[i * 3 + 0, 2] = matricesNx4x4[i, 0, 2]
a_values[i * 3 + 1, 2] = matricesNx4x4[i, 1, 2]
a_values[i * 3 + 2, 2] = matricesNx4x4[i, 2, 2]
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
......@@ -83,14 +81,14 @@ def pivot_calibration(matricesNx4x4):
rank += 1
if rank < 6:
print("PivotCalibration: Failed. Rank < 6")
exit()
# Residual Matrix
x_values = np.transpose(x_values)
residual_matrix = (a_values * x_values - b_values)
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 + residual_matrix[i, 0] * residual_matrix[i, 0]
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)
......@@ -100,8 +98,6 @@ def pivot_calibration(matricesNx4x4):
output_matrix = np.identity(4)
x_values = np.transpose(x_values)
output_matrix[0, 3] = x_values[0, 0]
output_matrix[1, 3] = x_values[1, 0]
output_matrix[2, 3] = x_values[2, 0]
......
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