What am I doing wrong with Affine Transform of Parallelogram into Rectangle?

Question

I have two shapes, a rectangle and a parallelogram that signify two gantry systems. The one gantry system has a camera on it and can detect the position of the other gantry system as it sits above. I cannot via a series of transforms (translate, rotate, shear x, shear y, translate) get it even remotely close to fitting to the system 1. Could I please get some pointers/insight as to what I am doing wrong?

I've tested each transform with a unit vector so I know the math works. I suspect either I am using the incorrect angles(using the same on the unit vectors though), there are linearity issues where it is not quite linear and therefore transforms wont work (this also seems unlikely due to the physical nature), or most likely my order of operations are incorrect.

from matplotlib import pyplot as plt
import numpy as np
from mpl_toolkits.axes_grid1.inset_locator import TransformedBbox, BboxPatch, BboxConnector


def get_angle(array, array_2, side=3):
    if side == 0:
        # Get start and end points from array
        vector = array[1] - array[0]
        # Get start and end points from array
        vector_2 = array_2[1] - array_2[0]
    elif side == 1:
        # Get start and end points from array
        vector = array[2] - array[1]
        # Get start and end points from array
        vector_2 = array_2[2] - array_2[1]
    elif side == 2:
        # Get start and end points from array
        vector = array[2] - array[3]
        # Get start and end points from array
        vector_2 = array_2[2] - array_2[3]
    elif side == 3:
        # Get start and end points from array
        vector = array[3] - array[0]
        # Get start and end points from array
        vector_2 = array_2[3] - array_2[0]

    # Calculate unit vectors
    dot = vector[0] * vector_2[0] + vector[1] * vector_2[1]  # dot product between [x1, y1] and [x2, y2]
    det = vector[0] * vector_2[1] - vector[1] * vector_2[0]  # determinant
    angle = np.arctan2(det, dot)  # atan2(y, x) or atan2(sin, cos)
    return angle


def shear_trans_x(coords, phi):
    shear_x = np.array([[1, np.tan(phi), 0],
                        [0, 1, 0],
                        [0, 0, 1]])
    coords = np.append(coords, np.ones((coords.shape[0], 1)), axis=1)
    resultant = coords @ shear_x.T
    return resultant[:, 0:2]


def shear_trans_y(coords, psi):
    shear_y = np.array([[1, 0, 0],
                        [np.tan(psi), 1, 0],
                        [0, 0, 1]])
    coords = np.append(coords, np.ones((coords.shape[0], 1)), axis=1)
    resultant = coords @ shear_y.T
    return resultant[:, 0:2]


def translate(coordinates, offset):
    coordinates = np.append(coordinates, np.ones((coordinates.shape[0], 1)), axis=1)
    a = np.array([[1, 0, offset[0]],
                  [0, 1, offset[1]],
                  [0, 0,     1   ]])

    result = coordinates @ a.T
    return result[:, 0:2]


def rotate(coords, theta, origin=[0,0]):
    cos = np.cos(theta)
    sin = np.sin(theta)
    a = np.array([[cos, -sin, 0],
                  [sin, cos, 0],
                  [0, 0, 1]])
    if np.all(origin == [0, 0]):
        coords = np.append(coords, np.ones((coords.shape[0], 1)), axis=1)
        result = coords @ a.T
        return result[:, 0:2]
    else:
        coords = translate(coords, -origin)
        coords = rotate(coords, theta, origin=[0, 0])
        coords = translate(coords, origin)
        return coords


def mark_inset(parent_axes, inset_axes, loc1a=1, loc1b=1, loc2a=2, loc2b=2, **kwargs):
    '''
    draw a bbox of the region of the inset axes in the parent axes and
    connecting lines between the bbox and the inset axes area
    loc1, loc2 : {1, 2, 3, 4}
    '''
    rect = TransformedBbox(inset_axes.viewLim, parent_axes.transData)

    p1 = BboxConnector(inset_axes.bbox, rect, loc1=loc1a, loc2=loc1b, **kwargs)
    inset_axes.add_patch(p1)
    p1.set_clip_on(False)
    p2 = BboxConnector(inset_axes.bbox, rect, loc1=loc2a, loc2=loc2b, **kwargs)
    inset_axes.add_patch(p2)
    p2.set_clip_on(False)

    pp = BboxPatch(rect, fill=False, **kwargs)
    parent_axes.add_patch(pp)

    return pp, p1, p2


if __name__ == '__main__':
    # calibration data
    gantry_1_coords = np.array([[169.474, 74.4851], [629.474, 74.4851], [629.474, 334.4851], [169.474, 334.4851]])
    gantry_2_coords_error = np.array([[-0.04, 0.04], [-0.04, -0.31], [0.76, -0.57], [1.03, 0.22]])
    # gantry_2_coords_error = np.array([[0.13, 0.04], [-0.13, -0.75], [0.31, -0.93], [0.58, -0.31]])
    # add error to gantry 1 coords
    gantry_2_coords = gantry_1_coords + gantry_2_coords_error

    # append first point to end for plotting to display a closed box
    gantry_1_coords = np.append(gantry_1_coords, np.array([gantry_1_coords[0]]), axis=0)
    gantry_2_coords = np.append(gantry_2_coords, np.array([gantry_2_coords[0]]), axis=0)

    # get length of diagonal direction
    magnitude = np.linalg.norm(gantry_1_coords[0] - gantry_1_coords[2])
    magnitude_gantry_2 = np.linalg.norm(gantry_2_coords[0] - gantry_2_coords[2])

    # translate to gantry_1 first position
    translated_gantry_2 = translate(gantry_2_coords, (gantry_1_coords[0] - gantry_2_coords[0]))
    print('translation_offset_1', ' = ', gantry_1_coords[0] - gantry_2_coords[0])

    # rotate gantry_2 to gantry_1
    theta = get_angle(translated_gantry_2, gantry_1_coords,  side=0)
    rotate_gantry_2_coords = rotate(translated_gantry_2, theta, translated_gantry_2[0])
    print('rotation angle', ' = ', theta)

    # un-shear x axis gantry_2
    shear_phi = get_angle(rotate_gantry_2_coords, gantry_1_coords, side=3)
    sheared_x_gantry_2 = shear_trans_x(rotate_gantry_2_coords, shear_phi)
    print('shear x angle', ' = ', shear_phi)

    # un-shear y axis gantry_2
    shear_psi = get_angle(sheared_x_gantry_2, gantry_1_coords,  side=2)
    sheared_y_gantry_2 = shear_trans_y(sheared_x_gantry_2, shear_psi)
    print('shear y angle', ' = ', shear_psi)

    # translate to gantry_1 first position
    final_gantry_2_coords = translate(sheared_y_gantry_2, (gantry_1_coords[0] - sheared_y_gantry_2[0]))
    print('translation_offset_2', ' = ', gantry_1_coords[0] - sheared_y_gantry_2[0])

    # create exaggerated errors for plotting
    ex_gantry_2_coords = (gantry_2_coords - gantry_1_coords) * 50 + gantry_2_coords
    ex_gantry_2_final_coords = (final_gantry_2_coords - gantry_1_coords) * 50 + final_gantry_2_coords

    # separate out x & y components for plotting
    gantry_1_x, gantry_1_y = gantry_1_coords.T
    gantry_2_x, gantry_2_y = ex_gantry_2_coords.T
    gantry_2_final_x, gantry_2_final_y = ex_gantry_2_final_coords.T


    # plot results
    fig, ax = plt.subplots()
    ax.plot(gantry_1_x, gantry_1_y, color='black', linestyle='--', label='gantry_1')
    ax.plot(gantry_2_x, gantry_2_y, color='blue', linestyle='--', label='gantry_2 original')
    ax.plot(gantry_2_final_x, gantry_2_final_y, color='red', linestyle='--', label='gantry_2 transformed')
    # get legend lines and labels from center graph
    lines, labels = ax.get_legend_handles_labels()
    fig.legend(lines, labels)
    plt.show()

    # print('gantry 1 positions: ', gantry_1_coords)
    # print('transformed gantry 2 positions: ', final_gantry_2_coords)

Nick ODell · Accepted Answer

Fixing existing code

In terms of the existing code, I applied the transformations one by one, and I think you're missing a negative sign here:

sheared_x_gantry_2 = shear_trans_x(rotate_gantry_2_coords, -shear_phi)

                                                         # ^--- here

After applying that, the graph looks better:

Least squares fit

However, I think this is the wrong general approach. For example, when you fix the shear, that's going to break the translation and rotation, at least a little bit. You can repeatedly apply the fixes, and converge on the right answer, but there's a better way.

Instead, I would suggest finding a least-squares fit for the transformation matrix, rather than building up a bunch of rotation and shear matrices. Numpy has a function that will do this.

def add_bias_term(matrix):
    return np.append(np.ones((matrix.shape[0], 1)), matrix, axis=1)

x, _, _, _ = np.linalg.lstsq(add_bias_term(gantry_2_coords), gantry_1_coords, rcond=None)
final_gantry_2_coords = add_bias_term(gantry_2_coords) @ x

This is both a heck of a lot shorter, and produces a better fit to boot:

And here is the matrix that it finds:

array([[ 0.19213806, -0.37107611],
       [ 1.00028902,  0.00123954],
       [-0.00359818,  1.00014869]])

(Note that the first row is the bias term.)

Although, the fit is not perfect. Here are the residuals:

array([[-0.06704727, -0.10997465],  # point 0
       [ 0.06716097,  0.11016114],  # point 1
       [-0.06720015, -0.1102254 ],  # point 2
       [ 0.06708645,  0.11003891]]) # point 3

Unfortunately, this remaining error is nonlinear, by definition. (If there were an affine matrix which reduced the error better, lstsq would have found it.)

Adding nonlinearity

Eyeballing the residuals, the error goes in one direction when both x and y are large, and in the other direction when only one of x or y are large. That suggests to me that you need an interaction term. In other words, you need to preprocess the input matrix so that it has a column with X, a column with Y, and a column with X*Y.

The code to do that looks like this:

def add_bias_term(matrix):
    return np.append(np.ones((matrix.shape[0], 1)), matrix, axis=1)

def add_interaction(matrix):
    inter = (matrix[:, 0] * matrix[:, 1]).reshape(matrix.shape[0], 1)
    return np.append(inter, matrix, axis=1)
    

x, _, _, _ = np.linalg.lstsq(add_bias_term(add_interaction(gantry_2_coords)), gantry_1_coords, rcond=None)
final_gantry_2_coords = (add_bias_term(add_interaction(gantry_2_coords)) @ x)

And the graph for that looks like this:

And that's close enough that the two graphs are right on top of each other.

What am I doing wrong with Affine Transform of Parallelogram into Rectangle?

Answers (1)

Fixing existing code

Least squares fit

Adding nonlinearity

Related Questions