Calculate rotated quatanion using rotation fomula.
$$
w, x, y, z = QAQ^-
$$
The following formula is used for the multiplication of the above quaternions.
$$
q_1q_2=\begin{pmatrix}
w \\
x \\
y \\
z \\
\end{pmatrix}=\begin{pmatrix}
q_{1w}q_{2w}-q_{1x}q_{2x}-q_{1y}q_{2y}-q_{1z}q_{2z} \\
q_{1w}q_{2x}+q_{1x}q_{2w}+q_{1y}q_{2z}-q_{1z}q_{2y} \\
q_{1w}q_{2y}-q_{1x}q_{2z}+q_{1y}q_{2w}+q_{1z}q_{2x} \\
q_{1w}q_{2z}+q_{1x}q_{2y}-q_{1y}q_{2x}+q_{1z}q_{2w} \\
\end{pmatrix}
$$
As a result, $(w, x, y, z)$ is obtained, so extract only the $xyz$ components and make it a vector.
If you want to return a scalar value that is equivalent to the vector before it was normalized, multiply it back by the original scalar value.
$$
\begin{pmatrix}
x \\
y \\
z \\
\end{pmatrix}\times |A|
$$
$N$ vectors $a_1, ... , a_N$ with rotation $R$, we obtain $a'_1, ... , a'_N$ are obtained.
$$
a\prime_i=Ra, i=1,..,N
$$
If there is no error in the data, two points are sufficient to find $R$.
As shown in the image below, the points $ori1$ and $ori2$ were rotated to create the points $rot1$ and $rot2$.
The method to find the Rotation matrix using $ori1, ori2, rot1, rot2$ is shown below.
1. Find the outer product of two vectors and normalize it to a unit vector.
The result obtained is $ori1 \times ori2$ and $rot1\times rot2$ in the image below.
$R_1$ and $R_2$ are orthogonal matrices, mapping the basis $(e_1, e_2, e_3)$ to $(r_1, r_2, r_3)$ and $(r_1', r_2', r_3')$ respectively. Thus, the following $R$ maps $(r_1, r_2, r_3)$ to $(r_1', r_2', r_3')$. This R is the rotation matrix to be sought.
$$
R = R_2R_1^\intercal
$$
The following command can be used to perform a series of processes.
During the process, $tr(AB)=tr(BA)$ was used. And $T$ was assumed
$$
T=V^\intercal RU \tag{9}
$$
Since $U$ and $V$ are orthogonal matrices and $R$ is a rotation matrix (and thus an orthogonal matrix), $T$ is also an orthogonal matrix. And if $T=(T_{ij})$, then
Since an orthogonal matrix is a matrix with orthogonal unit vectors as rows and columns, no element has a size greater than $1$. And since $\sigma_1,\sigma_2,\sigma_3\geq0$
$$tr(TS)\leq\sigma_1+\sigma_2+\sigma_3...(11)$$
The equal sign holds for $T_{11}=T_{22}=T_{33}=1$, which implies $T=I$. Hence, if there exists a rotation $R$ that makes $T$ in equation (9) $I$, it is the rotation that maximizes $K$.
Let $I$ be $T$ in equation (9) and multiply by $V$ from the left and $U^\intercal$ from the right, such that R,
$$
R=VU^\intercal \tag{12}
$$
If $|VU|(=|V||U|)=1$, $|R|=1$ and $R$ is rotation matrix. Since $V$ and $U$ are orthogonal matrices, $|V|=\pm1$ and $|U|=\pm1$, but not necessarily $|VU|=1$.
When $T11=T22=T33=1$ cannot be achieved no matter how $R$ is chosen, the following equation is true at the expense of the smallest $\sigma_3$.
The equal sign is formed by $T_{11}=T_{22}=1,T_{33}=-1$. Since both rows and columns of $T$ are orthonormal, this implies $T=diag(1,1,-1)$. Letting $T$ in equation (9) be $diag(1,1,-1)$ and multiplying by $V$ from the left and $U^\intercal$ from the right, such that $R$
If $|VU|=1$, then $|R|=-1$, but if $|VU|=-1$, then $|R|=1$, where $R$ is the rotation matrix.
From the above, the rotation matrix $R$ that maximizes $K$, i.e., the least-squares solution $R$ that minimizes Eq. (1), is given by combining Eqs. (12) and (14) as follows
The following command can be used to perform a series of processes.
python3 estimate_R_with_isotropic_errors.py
The blue point cloud in the image below is the green point cloud rotated by $90$ degrees with the unit vector as the rotation axis, and isotropic error is added.
The red point cloud is a rotation of the green point cloud using the estimated $R$ It overlaps the blue point cloud of the true value and is purple.
Using the $4$-dimensional vector $q=(q_0,q_1,q_2,q_3)^\intercal$, equation (19) can be written as
$$
K=\langle q,N\prime q \rangle \tag{21}
$$
Since $N\prime$ is a symmetric matrix, from the Min-max theorem, $q$ that maximizes Eq.(21) is the unit eigenvector for the largest eigenvalue of matrix $N\prime$. Substituting $q$ into equation (17), we can determine $R$ that maximizes $K$.
The following command can be used to perform a series of processes.
$R$ estimated from image and sensor data may not be an exact rotation matrix due to errors.
We then consider the problem of correcting this to the exact rotation matrix $R$.
Specifically, the estimated $R\prime$ is replaced by the rotation matrix $R$ that minimizes $||R-R\prime||$.
However, we define the matrix norm of the m*n matrix $A=(Aij)$ as
Therefore, the $R$ that minimizes $R-R\prime$ is the $R$ that maximizes $tr(RR\prime^\intercal)$. This is the same form as the maximization in Eq. (5).
Hence, the solution is determined by the singular value decomposition of $R\prime$.
$$
R\prime=USV^\intercal
$$
Since $R\prime^\intercal=USV^\intercal$, from equation (15) the solution is
