JOIN THE OBJECT DETECTION COURSE
Master the object detection skills with our new practical course. Only here you will get step-by-step algorithms explanation to build strong and deep understanding of the process.
Read About the CourseIf something moved it doesn't mean that it changed
We need to find the best rotation & translation params between two sets of points in 3D space. This type of transformation called Euclidean as it preserves sizes.
Our task to solve the equation
\begin{align}R*A + t = B\end{align}
There are many ways of getting things done almost in any case, but currently we will use SVD for this. You ask why? — Because the matrix is basically a transformation and with SVD can be decomposed on Rotation(U), Scaling(Σ), Rotation(V) (in special case when A is mxm matrix, but this gives us a clue. deeper explanation you can find in paper Least-Squares Rigid Motion Using SVD)
RotationTo get rotation first we need to find out the center of it. So we will find centroids of two datasets.
\begin{align}centroid_p = mean([{x_i,y_i,z_i} \in X])\end{align}
First, we will re-center our datasets and create covariance matrix H of two them, where Pa, Pb are points in datasets.
\begin{align}H = \sum _{i=1}^N{(P_A^i-centroid_A)}{(P_B^i-centroid_B)}^T\end{align}
Now we use SVD to find out U and V matrices
\begin{align}[U,S,V] = \mathrm{SVD}(H)\end{align}
And our rotation matrix are equal
\begin{align}R = V*U.T\end{align}
Because solving SVD implies some sort of randomness(there can be a different number of correct solutions) it sometimes makes a rotation matrix to be sign reflected.
We can fix this by checking the determinant of R matrix and if it negative then multiply 3rd column of R by -1
This is very simple.
\begin{align}t = -R*centroid_A + centroid_B\end{align}