10

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 ?

Widawensen
  • 8,517
John
  • 101

2 Answers2

9

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

combinatorial
  • 103
ratchet freak
  • 1,940
  • The thing is that i don't know neither the rotation nor the translation.How do i get them? – John Oct 31 '11 at 10:43
  • @John you solve $A$, you have 6 unknowns and N equations – ratchet freak Oct 31 '11 at 11:22
  • @John I made an edit explaining the next step – ratchet freak Oct 31 '11 at 11:49
  • thanks very much ..it was helpful. I was just wondering what represent the θ ...do you mean the axis and angle rotation representation ? I see that i need just the matrix A, but what represents the θ? Then,i'm thinking to solve that system (4 unknowns and N equations) with the pseudo-inverse.Is it right? – John Oct 31 '11 at 21:04
  • @john $\theta$ is represented by $\sin(\theta)$ and $\cos(\theta)$ (most math libraries have a atan2(x,y) function you can pass those in to get $\theta$) – ratchet freak Oct 31 '11 at 22:16
  • i think that the matrix A will work only if the two frames are rotated by angle θ about axis z since the matrix $$A=\begin{bmatrix} cos(\theta) & -sin(\theta) & x_{trans} \ sin(\theta) & cos(\theta) & y_{trans} \ 0 & 0 & 1 \ \end{bmatrix} $$ describes a rotation about z!!! – John Nov 01 '11 at 17:28
  • no it describes a rotation in a 2d plane with homogeneous coordinates – ratchet freak Nov 01 '11 at 17:58
  • so do you mean that in the projective geometry is like the points of the two frames are projected on the same plane and that θ refers to a rotation on this plane? – John Nov 02 '11 at 16:43
  • @John pretty much yeah – ratchet freak Nov 02 '11 at 17:31
  • @ ratchet freak , what should i do if there's also a scaling?How can i modify the problem? – John Nov 06 '11 at 13:57
  • 1
    then you get $A=\begin{bmatrix} sccos(\theta) & sc-sin(\theta) & x_{trans} \ scsin(\theta) & sccos(\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}$ – ratchet freak Nov 06 '11 at 15:33
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<P_jW,Q_j\right>\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} $$

robjohn
  • 353,833
  • Since my problem is referred to 2D points, do you mean i've to solve the same algorithm (scalar) two times, one time for x,w points and the other time for y,z points? – John Oct 31 '11 at 21:43
  • No. The algorithm above takes an array of $m$ points in $\mathbb{R}^n$ and produces an $n\times n$ array, $M$, and an $n$-vector, $R$. In your case, $n=2$. – robjohn Nov 01 '11 at 00:00
  • It is a very good work.Please ,can you send me the code in which you implemented that?It would be very helpful for me.My email is jay.cage@gmail.com – John Nov 01 '11 at 15:32
  • @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. – robjohn Nov 01 '11 at 15:57