Fitting 2D rigid-body/RST transformation to points

written by Jarno Elonen <elonen@iki.fi>, November 2007, released into the Public Domain

We are trying to fit a 2D ``rigid body transformation'' into given to 2D point sets 'from' and 'to'. More precisely, we want rotation, scale and translation but no skewing or non-uniform scaling. This is sometimes called RST (rotation, scale, translation) transform, and can be written as...

$x_i' (x, y) = cx_i - sy_i + t_x \par y_i' (x, y) = sx_i + c_{} y_i + t_y$

...or in matrix notation...

$\left(\begin{array}{c} x'_i\ y_i' \end{array}\right) = \left(\begin{array}{c... ... y_i \end{array}\right) + \left(\begin{array}{c} t_x\ t_y \end{array}\right)$

...where $s = \sin (\alpha), c = \cos (\alpha)$ , alpha is the rotation angle around origin and $ (x_i', y_i')$ is the i'th point in 'to' set, corresponding to the i'th point $ (x_i, y_i)$ in the 'from' set.

To obtain the unknowns ( $ s, c, t_x, t_y$ ) we formulate a linear least squares problem:

$\ensuremath{\boldsymbol{A}}\ensuremath{\boldsymbol{c}} \hat{\approx} \ensurema... ...left(\begin{array}{c} x'_0\ y'_0\ x'_1\ y'_1\ \ldots \end{array}\right)$

This can be solved by standard linear least squares fitting methods (e.g. by definition: $\ensuremath{\boldsymbol{c}}= (\ensuremath{\boldsymbol{A}}^T \ensuremath{\boldsymbol{A}})^{- 1} \ensuremath{\boldsymbol{A}}^T \ensuremath{\boldsymbol{b}}$ or faster and more robustly by QR decomposition or SVD).

In python, using the NumPy linear algebra package:

#!/usr/bin/python
import numpy as np

# Input points
from_pt = [[1,1], [1,2], [2,2], [2,1]] # A rectangle at 1,1
to_pt =   [[2,2], [4,4], [6,2], [4,0]] # The same, transformed

# Fill the matrices
A_data = []
for pt in from_pt:
  A_data.append( [-pt[1], pt[0], 1, 0] )
  A_data.append( [ pt[0], pt[1], 0, 1] )

b_data = []
for pt in to_pt:
  b_data.append(pt[0])
  b_data.append(pt[1])

# Solve
A = np.matrix( A_data )
b = np.matrix( b_data ).T
c = np.linalg.lstsq(A, b)[0].T
c = np.array(c)[0]

print("Solved coefficients:")
print(c)

print("Translated 'from_pt':")
# These will be identical to 'to_pt' since
# our example transformation is exact
for pt in from_pt:
  print ("%f, %f" % (
    c[1]*pt[0] - c[0]*pt[1] + c[2],
    c[1]*pt[1] + c[0]*pt[0] + c[3] ))