pivot.py 3.53 KB
Newer Older
1 2 3 4 5 6 7 8
#  -*- coding: utf-8 -*-

"""Functions for pivot calibration."""

import numpy as np
# from numpy.linalg import svd


rmapaah's avatar
rmapaah committed
9
def pivot_calibration(matricesNx4x4):
10
    """
rmapaah's avatar
rmapaah committed
11 12 13 14 15 16 17 18 19
    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 matricesNx4x4: point set, N x 4 x 4 ndarray
    :returns: residual_error
    :raises: TypeError, ValueError
20
    """
rmapaah's avatar
rmapaah committed
21 22 23 24 25
    if not isinstance(matricesNx4x4, np.ndarray):
        raise TypeError("matricesNx4x4 is not a numpy array'")

    if not matricesNx4x4.shape[1] == 4:  # pylint: disable=literal-comparison
        raise ValueError("matricesNx4x4 should have 4 columns per matrix")
26

rmapaah's avatar
rmapaah committed
27 28 29 30 31
    if not matricesNx4x4.shape[0] == 4:  # pylint: disable=literal-comparison
        raise ValueError("matricesNx4x4 should have 4 rows per matrix")


    number_of_matrices = len(matricesNx4x4)
32 33 34 35 36 37 38 39 40 41 42

    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):
rmapaah's avatar
rmapaah committed
43 44 45 46 47 48 49 50 51 52 53 54 55
        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]
56 57 58 59 60 61 62 63 64 65 66

        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

67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88
    # 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 in range(len(s_values)):
        if s_values[i] < 0.01:
            s_values[i] = 0
        if s_values[i] != 0:
            rank += 1
    if rank < 6:
        print("PivotCalibration: Failed. Rank < 6")

    # Residual Matrix

89
    x_values = np.transpose(x_values)
rmapaah's avatar
rmapaah committed
90 91
    residual_matrix = (a_values * x_values - b_values)
    residual_error = 0.0
92
    for i in range(number_of_matrices * 3):
rmapaah's avatar
rmapaah committed
93
        residual_error = residual_error + residual_matrix[i, 0] * residual_matrix[i, 0]
94

rmapaah's avatar
rmapaah committed
95 96
    residual_error = residual_error / float(number_of_matrices * 3)
    residual_error = np.sqrt(residual_error)
97

98
    # Output
rmapaah's avatar
rmapaah committed
99
    # MakeIdentity matrix
100

rmapaah's avatar
rmapaah committed
101
    output_matrix = np.identity(4)
102 103 104

    x_values = np.transpose(x_values)

rmapaah's avatar
rmapaah committed
105 106 107
    output_matrix[0, 3] = x_values[0, 0]
    output_matrix[1, 3] = x_values[1, 0]
    output_matrix[2, 3] = x_values[2, 0]
108

rmapaah's avatar
rmapaah committed
109 110
    print("pivotCalibration=(", x_values[3, 0], ",", x_values[4, 0], ",", x_values[5, 0], "),residual=", residual_error)
    return residual_error