I have 4 3d points(x, y, z). Using these points how can I calculate rotation angle? Consequently, I want to have a 4x4 transformation matrix including rotation and translation information.

4 points are in a plane and they are rectangle and I set the top-left point as the origin of the rectangle coordinate. Look at the bellow pictures.

enter image description here enter image description here Here I only have four 3D points(Xw,Tw,Zw) based on the world coordinate. According to the 4 points, I can calculate 4 points based on the rectangle coordinate. For instance, value [Xr] in P2 is the distance between P1 and P2. And [-Yr] in P3 is the distance between P1 and P3.

Then how could I get rotation angle of the rectangle based on the world coordinate using those information?

Additional information (3d points in the world coordinate):
P1(-401.428, 485.929, 772.921)
P2(-21.4433, 475.611, 772.511)
P3(-400.982, 483.56, 403.703)
P4(-21.589, 473.028, 403.242)
  • $\begingroup$ So you want find the SE(3) transformation between the two sets of four points? Thus preferably represent the rotation with a rotation matrix? $\endgroup$
    – fibonatic
    May 29 '20 at 2:20
  • $\begingroup$ @fibonatic Yes I need a rotation matrix. $\endgroup$ May 29 '20 at 5:13

You want to find a translation vector $t \in \mathbb{R}^3$ and rotation matrix $R \in \mathbb{R}^{3 \times 3}$ such that vectors $p_k \in \mathbb{R}^3$ map to $p_k' \in \mathbb{R}^3$ using

$$ \begin{bmatrix} p_k' \\ 1 \end{bmatrix} = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} p_k \\ 1 \end{bmatrix}, $$

or equivalently $p_k' = R\,p_k + t$. Since the zero vector in rectangle coordinates should map to P1 in world coordinates it follows that $t$ should be equal to P1 in world coordinates. The rotation matrix can be found by using a solution to Wahba's problem, which in your case can be defined as

$$ \min_{R \in SO(3)} \sum_{k=1}^n a_k\, \|w_k - R\,v_k\|^2, $$

with $w_k = p_k' - t$ and $v_k = p_k$. The weights $a_k$ can be any positive number, often used to indicate how "trustworthy" each vector is and/or normalize with respect to vector length. However, one can always just set all $a_k$ equal to one.

If the world coordinates are subjected to any disturbance it might be that using P1 in world coordinates as $t$ gives a slightly sub-optimal solution. However, the solution proposed in this answer should still give a good approximated optimal solution (with respect to $\sum \|R\,p_k + t - p_k'\|^2$).


Also check this blog post for an alternative solution: http://nghiaho.com/?page_id=671


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.