Representation of Rigid Body Pose and Rotation

In the coordinate system, the state of a rigid body can be described by its position and attitude. The position is the spatial position of the rigid body in the coordinate system, which can be expressed by a coordinate vector. To describe the attitude of a rigid body, there are many representation methods, the following are commonly used ones:

1 Rotation matrix (Rotation matrix)

The rotation matrix is ​​a matrix that changes the direction of the vector but does not change the size when it is multiplied by a vector. The rotation matrix can be directly calculated. It uses 9 quantities to express a rotation, but in fact there are only 3 freedoms for a rotation. Spend. Therefore the rotation matrix expression is redundant. At the same time, there are constraints on the rotation matrix itself, it must be an orthogonal matrix, and the determinant is 1, these constraints will make it difficult to solve.
The rotation matrix is ​​easier to understand, not much to say here, it can be seen: coordinate transformation and rotation matrix

2 Euler angles

2.1 Euler angles

The Euler angle is a set of 3 angle parameters that describe the attitude of the rigid body. At the same time, it is necessary to specify the coordinate axes around which the three angles rotate, and the order of rotation. Once these are determined, the description of the Euler angle can be Decomposed into three basic rotations, the rotation matrix of the three basic rotations is: the rotation
matrix of θ around the X axis: $R_X\left(i\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\theta & -sin\theta \\ 0& sin\theta & cos\theta \end{array}\right]$
Equivalent graph: $R_Y\left(i\right)=\left[\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& 1& 0\\ -sin\theta & 0& cos\theta \end{array}\right]$
Equation: $R_Z\left(i\right)=\left[\begin{array}{ccc}cos\theta & -sin\theta & 0\\ sin\theta & cos\theta & 0\\ 0& 0& 1\end{array}\right]$
Personal understanding of Euler angles

2.1 Euler angles and rotation matrix

Any rotation matrix about rigid body rotation can be composed of three basic rotation matrices. However, it should be considered that Euler angles are divided into internal rotation and external rotation. The three basic rotation combinations of internal rotation and external rotation are different. The three basic rotation matrices need to be multiplied to the right for internal rotation, and the three basic rotation matrices for external rotation The rotation matrix needs to be multiplied to the left.

Outer rotation Euler angle calculation

任亏三界长x，y，z的欧拉角are respectively a,b,c，外旋，then the rotation matrix is ​​(for the sake of 简洁，使用sin和cos 电影用s和c电影) R ( a , b ,
$$R(\alpha ,b,c)=R_z\left(c\right)R_y\left(b\right)R_x\left(a\right)=\left[\begin{array}{ccc}c\beta c\gamma & ccsasb-c\alpha s\gamma & s\alpha s\gamma +caccsb\\ cbsc& c\alpha c\gamma +sasbsc& casbsc-c\gamma s\gamma \\ -s\beta & cbs& c\alpha c\beta \end{array}\right]$$
Conversely, corresponding to this rotation, the known rotation matrix $\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$, Euler angles can be solved to get
$$\begin{gathered} a=arctg(\frac{r_{32}}{r_{33}}) \\ b=arctg\left(\frac{-r_{31}}{\sqrt{r_{32}^2+r_{33}^2}}\right) \\ c=arctg\left(\frac{r_{21}}{r_{11}}\right) \end{gathered}$$

Internal Rotation Euler Angles Calculation

Let the internal rotation sequence be ZXY (Z-axis up, Y-axis forward), and the rotation angles are yaw, pitch, roll respectively, then the rotation matrix is ​​R ( yaw , pitch , roll ) = R z ( yaw ) R x (
$$R(yaw,pitch,roll)=R_z(yaw)R_x(pitch)R_y\left(roll\right)$$
Conversely, corresponding to this rotation, the known rotation matrix$\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$, the internal rotation Euler angle can be solved as
$$\begin{gathered} yaw=-arctg(\frac{r_{12}}{r_{22}}) \\ pitch=arcsin(r_{32}) \\ roll=-arctg\left(\frac{r_{31}}{r_{33}}\right) \end{gathered}$$

3 Axial angle

3.1 Shaft angle

Axis angles are rotations represented by two parameters: an axis or line, and the angle of rotation around that axis. It is also called rotated exponential coordinates.
Given a vector $\hat{K}=(k_x,k_y,k_z)$ , with this vector$\hat{K}$ is the rotation axis, rotate$\theta$ angle, so we use a four-dimensional vector$(k_x,k_y,k_z,\theta )$ can represent this rotation.

3.2 Rotation vector

The axis angle is sometimes represented by a rotation vector. The axis angle is described above as a four-dimensional vector to express a rotation, and a more compact representation is to use a unit vector to represent the axis, and use the length of the three-dimensional vector to represent the angle value theta. In this way, it can be simplified to represent the rotation with a three-dimensional vector, which is the rotation vector.
That is: the above axial vector $\hat{K}=(k_x,k_y,k_z)$ is taken as the unit vector${\hat{K}}_0=(k_x,k_y,k_z)$ ,determine the following:
$$\mathbf{R}=\left[\begin{array}{c}{R}_x\\ R_y\\ R_z\end{array}\right]=\left[\begin{array}{c}i\cdot k_x\\ i\cdot k_y\\ i\cdot k_z\end{array}\right]$$
Similarly, the axis angle parameter
$$\begin{gathered} i=\sqrt{R_x^2+R_y^2+R_z^2} \\ {k}_x=R_x/i,k_y=R_y/i,k_z=R_z/i \end{gathered}$$

3.3 Axis Angle and Rotation Matrix

(k_x,k_y,k_z,θ )$(k_x,k_y,k_z,\theta )$ , in addition to the Rodrigues equation, we obtained the equation
$$R_{\hat{K}}\left(i\right)=c\theta I+(1-c\theta )r{r}^T+s\theta \left[\begin{array}{ccc}0& -k_z& k_y\\ k_z& 0& -k_x\\ -k_y& k_x& 0\end{array}\right]$$
其中$r=(k_x,k_y,k_z{)}^T$，即：$$R_{\hat{K}}\left(i\right)=\left[\begin{array}{ccc}ci+k_x^2(1-ci)& k_x{k}_y(1-ci)-k_{z}s\theta & k_x{k}_z(1-ci)+k_{y}s\theta \\ k_y{k}_x(1-ci)+k_{z}s\theta & ci+k_y^2(1-ci)& k_y{k}_z(1-ci)-k_{x}s\theta \\ k_z{k}_x(1-ci)-k_{y}s\theta & k_z{k}_y(1-ci)+k_{x}s\theta & ci+k_z^2(1-ci\end{array}\right]$$
The above conversion formula has a related implementation function in Opencv: Rodrigues2().
Conversely, the known rotation matrix $R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$,individually define the equation
$$s\theta \left[\begin{array}{ccc}0& -k_z& k_y\\ k_z& 0& -k_x\\ -k_y& k_x& 0\end{array}\right]=\frac{R-R^T}{2}$$
即
$$\begin{gathered} i=arccos(\frac{r_{11}+r_{22}+r_{33}-1}{2}) \\ \hat{K}=\left[\begin{array}{c}{k}_x\\ k_y\\ k_z\end{array}\right]=\frac{1}{2sin\theta }\left[\begin{array}{c}{r}_{32}-r_{23}\\ r_{13}-r_{31}\\ r_{21}-r_{12}\end{array}\right] \end{gathered}$$
It can be seen that when $\theta$不0或$2\Pi$ , the axis of rotation cannot be determined and is singular.

4 Quaternions

4.1 Quaternions

Quaternions are introduced to solve the shortcomings of Euler angles, axis angles, rotation matrices, etc.:

• The gimbal lock problem of Euler angles;
• The singularity of the axis angle;
• The 9 values ​​of the rotation matrix are not independent of each other.

Quaternions are simply hypercomplex numbers. Complex numbers are real numbers plus an imaginary unit $i$ composition, of which$i^2=-1$ .$\_$Similarly, a quaternion is a real number plus three imaginary units$i$、$j$、$k$ , and they have the following relationship:$i^2=j^2=k^2=-1，i^0=j^0=k^0=1$ , each quaternion is 1,$i$、$j$ and$The\; linear\; combination\; of\; k$ , that is, the quaternion can generally be expressed as$a+bi+cj+dk$ , where$a$、$b$、$c$、$d$ is a real number.
Let unit quaternion$\mathbf{q}=(q_0,q_1,q_2,q_3)$ ,when$q_0^2+q_1^2+q_2^2+q_3^2=1$ , if there is a unit vector$(x,y,z)$ is the rotation axis, the rotation angle$\theta$ , inherently infinite:$\mathbf{q}=(cos\frac{}{2},xsin\frac{}{2},ysin\frac{}{2},zsin\frac{}{2})$
Set a point${\mathbf{P}}_1=(p_x,p_y,p_z)$ , we need to use the above rotation quaternion to transform its rotation to${\mathbf{P}}_1^{\prime }=(p_x^{\prime },p_y^{\prime },p_z^{\prime })$ , the calculation process is as follows:

• 1 Definition: ${\mathbf{q}}_p=(0,p_x,p_y,p_z)=0+p_x\ast i+p_y\ast j+p_z\ast k$
• 2 Perform quaternion operations: $q_p^{\prime }=\mathbf{q}{\mathbf{q}}_p{q}^{-1}$ , where$q^{-1oneq}$ \$q$ Definition:${\mathbf{q}}^{-1}=(cos\frac{}{2},-xsin\frac{}{2},-ysin\frac{}{2},-zsin\frac{}{2})$
• 3 The resulting $q_p^{\prime }$It must be a pure quaternion, that is to say, its first item is 0, and it has the following form: $q_p^{\prime }=(0,p_x^{\prime },p_y^{\prime },p_z^{\prime })=0+p_x^{\prime }\ast i+p_y^{\prime }\ast j+p_z^{\prime }\ast$
  The last three terms of$k$$(p_x^{\prime },p_y^{\prime },p_z^{\prime })$ is the coordinate vector after rotation transformation.

Therefore, after the introduction of quaternions, the rotation of three-dimensional coordinates can be directly operated by quaternion multiplication, which is equivalent to the operation of the rotation matrix. There is no singularity phenomenon of axis angle calculation, the representation method is more compact, and the calculation amount can also be small. Some.
Similarly, if you have a quaternion: $\mathbf{q}=(q_0,q_1,q_2,q_3)=(cos\frac{}{2},xsin\frac{}{2},ysin\frac{}{2},zsin\frac{}{2})$ , then it corresponds to a vector$(x,y,z)$ is the rotation operation of axis rotation θ angle.

4.2 Quaternion and rotation matrix

Known quaternion $\mathbf{q}=(q_0,q_1,q_2,q_3)$，可求得旋转矩阵
$$R=\left[\begin{array}{ccc}2q_0^2+2q_1^2-1& 2q_1{q}_2+2q_0{q}_3& 2q_1{q}_3-2q_0{q}_2\\ 2q_1{q}_2-2q_0{q}_3& 2q_0^2+2q_2^2-1& 2q_2{q}_3+2q_0{q}_1\\ 2q_1{q}_3+2q_0{q}_2& 2q_2{q}_3-2q_0{q}_1& 2q_0^2+2q_3^2-1\end{array}\right]$$
On the contrary, the known rotation matrix $\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$, to find a quaternion, divided into two cases:
Case 1:
$$\begin{gathered} q_0=\frac{\sqrt{1+r_{11}+r_{22}+r_{33}}}{2} \\ {q}_1=\frac{r_{32}-r_{23}}{4q_0},q_2=\frac{r_{13}-r_{31}}{4q_0},q_3=\frac{r_{21}-r_{12}}{4q_0}, \end{gathered}$$
case 2: if$q_0$tends to 0, the process of solving the quaternion is:
2.1 If $max(r_{11},r_{22},r_{33})=r_{11}$，
$$\begin{gathered} t=\sqrt{1+r_{11}-r_{22}-r_{33}} \\ {q}_0=\frac{r_{32}-r_{23}}{t},q_1=t/4,q_2=\frac{r_{13}+r_{31}}{t},q_3=\frac{r_{12}+r_{21}}{t} \end{gathered}$$
2.2 If $max(r_{11},r_{22},r_{33})=r_{22}$，
$$\begin{gathered} t=\sqrt{1-r_{11}+r_{22}-r_{33}} \\ {q}_0=\frac{r_{13}-r_{31}}{t},q_1=\frac{r_{12}+r_{21}}{t},q_2=t/4,q_3=\frac{r_{32}+r_{23}}{t} \end{gathered}$$
2.3 If $max(r_{11},r_{22},r_{33})=r_{33}$，
$$\begin{gathered} t=\sqrt{1-r_{11}-r_{22}+r_{33}} \\ {q}_0=\frac{r_{21}-r_{12}}{t},q_1=\frac{r_{13}+r_{31}}{t},q_2=\frac{r_{23}-r_{32}}{t},q_3=t/4 \end{gathered}$$

reference

Brief book: Attitude description - quaternion, rotation matrix
is ​​known by Euler angle: how to understand quaternion visually?
Rotation matrix, Euler angle, quaternion theory and its conversion relationship
3D reconstruction learning (1): Basic knowledge: rotation matrix and rotation vector