3

I am trying to calculate the 4x4 rotation and translation matrix to align one set of 3D points with another (example shows 2D case). The code I have written works OK with rotation around (0, 0) or when translating but not when rotating around an arbitrary pivot point.

I have read in various places (such as here and here) that you need to subtract the offset to the origin but this is not what I am after as it returns the rotated shape back to its original origin.

Can the rotation and translation for these examples be stored in a single 4x4 matrix?

import numpy as np
import matplotlib.pyplot as plt
from matplotlib import collections

def rotation(r):
    return np.asarray([np.cos(r), -np.sin(r), np.sin(r), np.cos(r)]).reshape(2, 2)

def calculate_rotation(A, B, R_=np.arange(0, 360)):
    yaw_rotation = np.deg2rad(R_) # Brute force!
    diff = np.zeros(len(yaw_rotation))
    for i, a in enumerate(yaw_rotation):
        C = B.copy()
        C[:, :2] = np.dot(C[:, :2], rotation(a))
        diff[i] = sum([((C[:, 0].copy() - A[:, 0])**2).mean(), 
                       ((C[:, 1].copy() - A[:, 1])**2).mean()])
    return -yaw_rotation[np.where(diff == diff.min())][0]

def rotation_matrix(rot=[0, 0, 0], tra=[0, 0, 0]):

    xA, yA, zA = rot

    rX = np.matrix([[1, 0, 0, 0], [0, np.cos(xA), -np.sin(xA), 0], [0, np.sin(xA), np.cos(xA), 0], [0, 0, 0, 1]])
    rY = np.matrix([[np.cos(yA), 0, np.sin(yA), 0], [0, 1, 0, 0], [-np.sin(yA), 0, np.cos(yA), 0], [0, 0, 0, 1]])
    rZ = np.matrix([[np.cos(zA), -np.sin(zA), 0, 0], [np.sin(zA), np.cos(zA), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
    r = np.dot(rX, np.dot(rY, rZ))

    t = np.identity(4)
    t[:3, 3] = tra

    return np.dot(t, r)

def plot_triangle(Tri, i, color='none'):

    ax = plt.subplot(1, 3, i)
    pc = collections.PolyCollection((Tri[:, :2], ), facecolor=color, alpha=.5)
    ax.add_collection(pc)

    for i, row in enumerate(Tri):
        ax.scatter(row[0], row[1])
        ax.text(row[0], row[1], i)

plt.figure(figsize=(12, 4))

X = [4, 0, 0]
Y = [0, 3, 0]
Z = [0, 0, 0]

A = np.vstack([X, Y, Z, np.ones(3)]).T

for i, (r_, t_) in enumerate([[np.deg2rad(33), [0, 0]],    # rotate
                              [0, [2, -2]],                # tranlate
                              [np.deg2rad(33), [-2, 1]]]): # combined

    Xoff, Yoff, Zoff, roll, pitch, yaw = np.zeros(6)

    # offset and rotate B
    B = A.copy()
    B[:, :2] = np.dot(B[:, :2], rotation(r_))
    B[:, 0] += t_[0]
    B[:, 1] += t_[1]
    C = B.copy() # so we can then apply solution to original B

    ################################################

    # calculate distance between objects
    Xoff = A[0, 0] - C[0, 0] 
    Yoff = A[0, 1] - C[0, 1] 
    Zoff = A[0, 2] - C[0, 2]

    # calculate and apply offset of A to 0...
    A_ = A.copy()
    A_[:, 0] -= A_[0, 0]
    A_[:, 1] -= A_[0, 1]
    A_[:, 2] -= A_[0, 2]

    # calculate offset of C to 0...
    Xrot = C[0, 0]
    Yrot = C[0, 1]
    Zrot = C[0, 2]
    C[:, 0] -= Xrot
    C[:, 1] -= Yrot
    C[:, 2] -= Zrot

    # calculate rotation
    yaw = calculate_rotation(A_, C)

    # generate 4x4 matrix
    M = rotation_matrix(rot=(roll, pitch, yaw), 
                        tra=(Xoff, Yoff, Zoff))

    D = np.asarray(np.dot(M, B.T).T[:, :3])

    plot_triangle(A[:, :3], i+1)
    plot_triangle(D, i+1, 'y')

The output of this code is where the green triangle is the starting position, the red triangle is to be aligned to and the yellow triangle is where I am at.

enter image description here

kungphil
  • 1,759
  • 2
  • 18
  • 27
  • 4
    You can compose any two transformations by multiplying the matrices. You should be able to figure out how to translate it to origin, rotate it, then translate it back, right? Then multiply those three together to get the rotate-in-place. (And if you don't know how to decompose your existing rotation-and-origin, that's fine too—if you translate to origin, then translate-and-rotate, then translate back, that has the same result you want.) – abarnert Mar 20 '18 at 23:40
  • 1
    A different way to look at this is to compose coordinate transforms and object transforms. That may be easier for you to understand, although it ends up with the same results. You transform the axes so 0,0 is at the center point you want to rotate about, then apply your transformation to the shape, then un-transform the axes. – abarnert Mar 20 '18 at 23:41
  • 1
    see [Understanding 4x4 homogenous transform matrices](https://stackoverflow.com/a/28084380/2521214) single matrix can handle both transforms at once ... as abarnert mention you can multiply the incremental changes to single matrix ... – Spektre Mar 21 '18 at 07:51
  • Thank you both (can't tag two users :-\\), your advice solved my problem! – kungphil Mar 22 '18 at 09:22

0 Answers0