pivot.py 3.41 KB
Newer Older
1 2 3 4 5 6
#  -*- coding: utf-8 -*-
"""Functions for pivot calibration."""

import numpy as np


rmapaah's avatar
rmapaah committed
7
def pivot_calibration(matrices4x4):
8

9
    """
rmapaah's avatar
rmapaah committed
10 11 12 13 14 15
    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

rmapaah's avatar
rmapaah committed
16
    :param matrices4x4: point set, N x 4 x 4 ndarray
rmapaah's avatar
rmapaah committed
17 18
    :returns: residual_error
    :raises: TypeError, ValueError
19
    """
rmapaah's avatar
rmapaah committed
20 21
    if not isinstance(matrices4x4, np.ndarray):
        raise TypeError("matrices4x4 is not a numpy array'")
rmapaah's avatar
rmapaah committed
22

rmapaah's avatar
rmapaah committed
23 24
    if not matrices4x4.shape[1] == 4:  # pylint: disable=literal-comparison
        raise ValueError("matrices4x4 should have 4 columns per matrix")
25

rmapaah's avatar
rmapaah committed
26
    number_of_matrices = len(matrices4x4)
27 28 29 30 31 32 33 34 35 36 37

    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
38 39 40 41 42 43 44 45 46 47 48 49 50
        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]
51 52 53 54 55 56 57 58 59 60 61

        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

62 63 64 65 66 67 68 69 70 71 72 73
    # 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
74 75 76 77
    for i, item in enumerate(s_values):
        if item < 0.01:
            item = 0
        if item != 0:
78
            rank += 1
79 80

    if rank < 6: # pylint: disable=literal-comparison
81
        raise ValueError("PivotCalibration: Failed. Rank < 6")
82 83 84

    # Residual Matrix

85
    residual_matrix = (np.dot(a_values, x_values) - b_values)
rmapaah's avatar
rmapaah committed
86
    residual_error = 0.0
87
    for i in range(number_of_matrices * 3):
88 89 90
        residual_error = residual_error + \
                         np.dot(residual_matrix[i, 0],
                                residual_matrix[i, 0])
91

rmapaah's avatar
rmapaah committed
92 93
    residual_error = residual_error / float(number_of_matrices * 3)
    residual_error = np.sqrt(residual_error)
94

95
    # Output
rmapaah's avatar
rmapaah committed
96
    # MakeIdentity matrix
97

rmapaah's avatar
rmapaah committed
98
    output_matrix = np.identity(4)
99

rmapaah's avatar
rmapaah committed
100 101 102
    output_matrix[0, 3] = x_values[0, 0]
    output_matrix[1, 3] = x_values[1, 0]
    output_matrix[2, 3] = x_values[2, 0]
103

104 105 106
    print("pivotCalibration=(", x_values[3, 0], ","
          , x_values[4, 0], ",", x_values[5, 0],
          "),residual=", residual_error)
107

108
    return residual_error, x_values[0, 0], x_values[1, 0], x_values[2, 0]