8
$\begingroup$

The question I'm trying to figure out states that I have $N$ points $(P_{a1x},P_{a1y}) , (P_{a2x},P_{a2y}),\dots,(P_{aNx},P_{aNx})$ which correspond to a Pixel plane $xy$ of a camera, and other $N$ points $(P_{b1w},P_{b1z}), (P_{b2w},P_{b2z}),\dots,(P_{bNw},P_{bNz})$ which correspond to my $2D$ World Coordinate Frame $wz$.

I've to find the transformation (Rotation + Translation) between these two sets of points so that I can translate the point from the camera space to the world space. I've made a lot of measures and I've got the two set of points, but how should I proceed now ?

2 Answers 2

7

if you put the points in a homogeneous coordinate system (add a third dimension which is $1$ for all points) then each relation can be expressed as $A*P_a=P_b$ with

$A=\begin{bmatrix} cos(\theta) & -sin(\theta) & x_{trans} \\ sin(\theta) & cos(\theta) & y_{trans} \\ 0 & 0 & 1 \\ \end{bmatrix} $

with $\theta$ the rotation and $(x_{trans}, y_{trans})$ the translation

note this assumes a affine transformation but with only rotation and translation this is the case

edit confused $\sin$ and $\cos$ in the transformation matrix

you can read more about this at the wiki

in other words you can express the equations as $\cos(\theta)*P_{aix} - \sin(\theta)*P_{aiy} + x_{trans} = P_{bix}$ and $\sin(\theta)*P_{aix} + \cos(\theta)*P_{aiy} + y_{trans} = P_{biy}$

or

$\begin{bmatrix} P_{a1x} & -P_{a1y} & 1 &0\\ P_{a1y} & P_{a1x} & 0 &1\\ P_{a2x} & -P_{a2y} & 1 &0\\ P_{a2y} & P_{a2x} & 0 &1\\ ...\\ P_{aNx} & -P_{aNy} & 1 &0\\ P_{aNy} & P_{aNx} & 0 &1\\ \end{bmatrix}* \begin{bmatrix} \cos(\theta) \\ \sin(\theta) \\ x_{trans}\\ y_{trans} \end{bmatrix}= \begin{bmatrix} P_{b1x}\\ P_{b1y}\\ P_{b2x}\\ P_{b2y}\\ ...\\ P_{bNx}\\ P_{bNy}\\ \end{bmatrix} $

and I believe you know how to solve that

  • 0
    then you get A=\begin{bmatrix} sc*cos(\theta) & sc*-sin(\theta) & x_{trans} \\ sc*sin(\theta) & sc*cos(\theta) & y_{trans} \\ 0 & 0 & 1 \\ \end{bmatrix} and the b in the final equation would be $\begin{bmatrix} sc*\cos(\theta) \\ sc*\sin(\theta) \\ x_{trans}\\ y_{trans} \end{bmatrix}$ and sc can be gotten from $\sqrt{(sc*\cos(\theta))^2+(sc*\sin(\theta))^2}$2011-11-06
5

The Kabsch Algorithm gives the least square solution for the rotation matrix.

I solved this problem for sci.math. That solution gives the same rotation as the Kabsch Algorithm and shows that the least squares conformal affine transformation maps the mean of the source points to the mean of the destination points. I have reproduced the summary of the method below.

Summary of the method:

To find the least squares solution to $PM+R=Q$ for a given set $\{P_j\}_{j=1}^m$ and $\{Q_j\}_{j=1}^m$, under the restriction that the map be conformal, first compute the centroids $ \begin{array}{} \bar{P}=\frac{1}{m}\sum_{j=1}^mP_j&\text{and}&\bar{Q}=\frac{1}{m}\sum_{j=1}^mQ_j \end{array} $ Next, compute the matrix $ \begin{align} S &=\sum_{j=1}^m(Q_j-\bar{Q})^T(P_j-\bar{P)}\\ &=\left(\sum_{j=1}^mQ_j^TP_j\right)-m\bar{Q}^T\bar{P} \end{align} $ Let the Singular Value Decomposition of S be $ S=UDV^T $ where U and V are unitary and D is diagonal.

Next compute $\{c_k\}_{k=1}^m$ with $ \begin{align} c_k &=\sum_{j=1}^m[(P_j-\bar{P}\;)V\;]_k[(Q_j-\bar{Q})U\;]_k\\ &=\left(\sum_{j=1}^m[P_jV\;]_k[Q_jU\;]_k\right)-m[\bar{P}V\;]_k[\bar{Q}U\;]_k \end{align} $ where $[V\;]_k$ is coordinate $k$ of $V$ and define $ a_k=\operatorname{signum}(c_k) $ Let $I_k$ be the matrix with the $k^{th}$ diagonal element set to $1$ and all the other elements set to $0$. Then calculate $ E=\sum_{k=1}^ma_kI_k $ Compute the orthogonal matrix $ W=VEU^T $ If $\det(W)<0$ but $\det(W)>0$ is required, change the sign of the $a_k$ associated with the $c_k$ with the smallest absolute value.

If required, compute $r$ by $ r\sum_{j=1}^m|P_j-\bar{P}\;|^2=\sum_{j=1}^m\left<(P_j-\bar{P}\;)W,Q_j-\bar{Q}\right> $ or equivalently $ r\left(\left(\sum_{j=1}^m|P_j|^2\right)-m|\bar{P}\;|^2\right)=\left(\sum_{j=1}^m\left\right)-m\left<\bar{P}\;W,\bar{Q}\;\right> $ Finally, we have the desired conformal map $Q = PM+R$ where $ \begin{array}{} M=rW&\text{and}&R=\bar{Q}-\bar{P}M \end{array} $

  • 0
    @John: I didn't write code for it. Ray Koopman at Simon Fraser University in Canada implemented it and tested it. You might be able to contact him using the link at the beginning of my answer.2011-11-01