2次アフィン変換のサンプル

2D-Affine.py

import numpy as np

# src -> [[x,y],[x,y],...,[x,y]] List型
# dst -> [[x,y],[x,y],...,[x,y]] List型
# srcとdstは同じ数である必要があり、計算の都合上で最低3点が必要

def CalcModel(src, dst):
    if (len(src) != len(dst)) or (len(src) < 3) or (len(src) < 3):
        return

    A = np.zeros((6, 6))
    B = np.zeros(6)
    C = np.zeros(6)

    for i in range(len(src)):
        x, y = src[i]
        x_target, y_target = dst[i]

        # 1nd row
        A[0][0] += pow(x, 4);
        A[0][1] += pow(x, 2) * pow(y, 2);
        A[0][2] += pow(x, 3) * y;
        A[0][3] += pow(x, 3);
        A[0][4] += pow(x, 2) * y;
        A[0][5] += pow(x, 2);

        # 2nd row
        A[1][0] = A[0][1];
        A[1][1] += pow(y, 4);
        A[1][2] += pow(y, 3) * x;
        A[1][3] += pow(y, 2) * x;
        A[1][4] += pow(y, 3);
        A[1][5] += pow(y, 2);

        # 3rd row
        A[2][0] = A[0][2];
        A[2][1] = A[1][2];
        A[2][2] += pow(x, 2) * pow(y, 2);
        A[2][3] += pow(x, 2) * y;
        A[2][4] += pow(y, 2) * x;
        A[2][5] += x * y;

        # 4th row
        A[3][0] = A[0][3];
        A[3][1] = A[1][3];
        A[3][2] = A[2][3];
        A[3][3] += pow(x, 2);
        A[3][4] += x * y;
        A[3][5] += x;

        # 5th row
        A[4][0] = A[0][4];
        A[4][1] = A[1][4];
        A[4][2] = A[2][4];
        A[4][3] = A[3][4];
        A[4][4] += pow(y, 2);
        A[4][5] += y;

        # 6th row
        A[5][0] = A[0][5];
        A[5][1] = A[1][5];
        A[5][2] = A[2][5];
        A[5][3] = A[3][5];
        A[5][4] = A[4][5];
        A[5][5] += 1;

        # B
        B[0] += pow(x, 2) * x_target
        B[1] += pow(y, 2) * x_target
        B[2] += x * y * x_target
        B[3] += x * x_target
        B[4] += y * x_target
        B[5] += x_target

        # C
        C[0] += pow(x, 2) * y_target
        C[1] += pow(y, 2) * y_target
        C[2] += x * y * y_target
        C[3] += x * y_target
        C[4] += y * y_target
        C[5] += y_target

    # Conversion
    dataA = np.array([
        [A[0][0], A[0][1], A[0][2], A[0][3], A[0][4], A[0][5]],
        [A[1][0], A[1][1], A[1][2], A[1][3], A[1][4], A[1][5]],
        [A[2][0], A[2][1], A[2][2], A[2][3], A[2][4], A[2][5]],
        [A[3][0], A[3][1], A[3][2], A[3][3], A[3][4], A[3][5]],
        [A[4][0], A[4][1], A[4][2], A[4][3], A[4][4], A[4][5]],
        [A[5][0], A[5][1], A[5][2], A[5][3], A[5][4], A[5][5]]
    ])
    dataB = np.array([B[0], B[1], B[2], B[3], B[4], B[5]]);
    dataC = np.array([C[0], C[1], C[2], C[3], C[4], C[5]]);

    matA = dataA.astype(np.float64)
    matB = dataB.astype(np.float64)
    matC = dataC.astype(np.float64)

    invMatA = np.linalg.inv(matA)

    matX = np.dot(invMatA, matB)
    matY = np.dot(invMatA, matC)

    param_x = {
        "value0": matX[0],
        "value1": matX[1],
        "value2": matX[2],
        "value3": matX[3],
        "value4": matX[4],
        "value5": matX[5]
    }

    param_y = {
        "value0": matY[0],
        "value1": matY[1],
        "value2": matY[2],
        "value3": matY[3],
        "value4": matY[4],
        "value5": matY[5]
    }

    reModel = {
        "var_x": param_x,
        "var_y": param_y,
    }

    return reModel

# model CalcModel関数の出力
# point -> (x, y) tuple型
def CalcPoint(model, point):
    param_x = model["var_x"]
    param_y = model["var_y"]

    x, y = point

    g_x = param_x["value0"] * x * x + param_x["value1"] * y * y + param_x["value2"] * x * y + param_x["value3"] * x + \
          param_x["value4"] * y + param_x["value5"]
    g_y = param_y["value0"] * x * x + param_y["value1"] * y * y + param_y["value2"] * x * y + param_y["value3"] * x + \
          param_y["value4"] * y + param_y["value5"]

    return (g_x, g_y)