# How to calculate rotation angle from 3D points

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.

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)

• So you want find the SE(3) transformation between the two sets of four points? Thus preferably represent the rotation with a rotation matrix? May 29 '20 at 2:20
• @fibonatic Yes I need a rotation matrix. May 29 '20 at 5:13

## 2 Answers

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