【三维空间刚体运动表示】

前言

\quad 三维空间刚体运动由旋转及平移构成。其中平移普遍利用向量表示，而旋转则有旋转矩阵、旋转向量、欧拉角、四元数等表示方法，这四种方法的优缺点已属老生常谈，本文整理常见表示方法之间的转换关系及其实现方式，便于日常开发应用。

1. 旋转矩阵

1.1 旋转矩阵与旋转向量

\quad 旋转矩阵 $\mathbf{R}$ 与旋转向量 $\theta \mathbf{n}$ 间通过Rodrigues（罗德里格斯）公式进行转换。旋转向量转换旋转矩阵可表示为：

\quad 上文中式（1）和式（2）两种表达形式等价，具体推导过程可参考如下博文：

• 罗德里格斯公式 理解、推导
• 旋转矩阵罗德里格斯公式（Rodriguez formula）的理解证明

\quad 旋转矩阵转换为旋转向量可表示为：

\quad 上文中，式（3）中$tr(R)$ 表示矩阵R的迹，即为对角线元素之和。式（6）和式（7）均可用于求解旋转向量，其中式（7）即为求解矩阵$\mathbf{R}$的特征值1对应的特征向量。
\quad 旋转矩阵与旋转向量间的转换，可借助于opencv的函数实现，或利用实现如下：

//[3] 旋转矩阵转旋转向量
void RotMat2RotVec(cv::Mat& rot, Vec3f &vec)
{
	// 可直接调用opencv Rodrigues函数
	//cv::Rodrigues(rot, vec);
	float theta = acos((trace(rot)[0] - 1.0) / 2);

	Mat N = (rot - rot.t()) / (2 * sin(theta));

	vec[0] = theta * N.at<float>(2, 1);
	vec[1] = theta * N.at<float>(0, 2);
	vec[2] = theta * N.at<float>(1, 0);
}

//[4] 旋转向量转旋转矩阵
void RotVec2RotMat( Vec3f &vec, cv::Mat& rot)
{
	// 可直接调用opencv Rodrigues函数
	//Rodrigues(vec, rot);

	float theta = norm(vec);
	Vec3f norm_vec = vec / theta;

	Mat N = (Mat_<float>(3, 3) <<
		0,				-norm_vec[2],	norm_vec[1],
		norm_vec[2],	0,				-norm_vec[0],
		-norm_vec[1],	norm_vec[0],	0
		);

	rot = Mat::eye(Size(3, 3), CV_32FC1) + sin(theta) *N + (1 - cos(theta))*N*N;
}

1.2. 旋转矩阵与欧拉角

\quad 欧拉角转化为旋转矩阵遵循如下公式：

\quad 基于上式可得出如下表达：
$$\mathbf{R}=\left[\begin{array}{cccc}cos{\theta }_{y}cos{\theta }_z& sin{\theta }_{x}sin{\theta }_{y}cos{\theta }_z-cos{\theta }_{x}sin{\theta }_z& cos{\theta }_{x}sin{\theta }_{y}cos{\theta }_z+sin{\theta }_{x}sin{\theta }_z& \\ cos{\theta }_{y}sin{\theta }_z& sin{\theta }_{x}sin{\theta }_{y}sin{\theta }_z+cos{\theta }_{x}cos{\theta }_z& cos{\theta }_{x}sin{\theta }_{y}sin{\theta }_z-sin{\theta }_{x}cos{\theta }_z& \\ -sin{\theta }_y& sin{\theta }_{x}cos{\theta }_y& cos{\theta }_{x}cos{\theta }_y& \end{array}\right]$$
\quad 由此可得出，旋转向量转换为欧拉角遵循如下关系：
$$\begin{gathered} {\theta }_x=arctran(r_{21}/r_{22}) \\ {\theta }_y=arctran(-r_{00}/sqrt(r_{10}^2+r_{00}^2)) \\ {\theta }_z=arctran(r_{10}/r_{00}) \end{gathered}$$
\quad 转换代码实现如下：

//[1] 旋转矩阵转换为欧拉角
void RotMat2EulerAngle(cv::Mat& rot, Vec3f &euler)
{
	// 校验绕y轴的旋转是否为±90°, 若是, 则属于万向锁现象, 等效为两次旋转
	float sy = sqrt(rot.at<float>(0, 0) * rot.at<float>(0, 0) + rot.at<float>(1, 0) * rot.at<float>(1, 0));
	bool singular = sy < 1e-6;

	if (!singular)
	{
		euler[0] = atan2(rot.at<float>(2, 1), rot.at<float>(2, 2));
		euler[1] = atan2(-rot.at<float>(2, 0), sy);
		euler[2] = atan2(rot.at<float>(1, 0), rot.at<float>(0, 0));
	}
	else
	{
		euler[0] = atan2(-rot.at<float>(1, 2), rot.at<float>(1, 1));
		euler[1] = atan2(-rot.at<float>(2, 0), sy);
		euler[2] = 0;
	}
}

//[2] 欧拉角转换为旋转矩阵
void EulerAngle2RotMat(Vec3f &euler, cv::Mat& rot)
{
	Mat Rx = (Mat_<float>(3, 3) <<
		1, 0, 0,
		0, cos(euler[0]), -sin(euler[0]),
		0, sin(euler[0]), cos(euler[0])
		);

	Mat Ry = (Mat_<float>(3, 3) <<
		cos(euler[1]), 0, sin(euler[1]),
		0, 1, 0,
		-sin(euler[1]), 0, cos(euler[1])
		);

	Mat Rz = (Mat_<float>(3, 3) <<
		cos(euler[2]), -sin(euler[2]), 0,
		sin(euler[2]), cos(euler[2]), 0,
		0, 0, 1
		);

	rot = Rz * Ry * Rx;
}

2. 四元数

\quad 为什么可以利用四元数$\mathbf{q}$描述三维空间的旋转？该问题可以类比“利用复数描述二维平面内的旋转”。对于该理解的详细描述，推荐参考博客：基于四元数的旋转理解。这里引用一下该博客内的一个结论：
\quad 四元数本身是一个四维向量，利用四元数定义的旋转${\mathbf{p}}^{\prime }=\mathbf{q}\mathbf{p}$本身就应该是四维旋转，但这个四维旋转可以拆分为一个左旋转${\mathbf{q}}_{\mathbf{L}}$与一个右旋转${\mathbf{q}}_{\mathbf{R}}$，即${\mathbf{p}}^{\prime }={\mathbf{q}}_{\mathbf{L}}\mathbf{p}{\mathbf{q}}_{\mathbf{R}}$。而在使用过程中为什么将${\mathbf{q}}_{\mathbf{L}}$表示为$[\mathbf{c}\mathbf{o}\mathbf{s}(\frac{\theta }{2}),\mathbf{s}\mathbf{i}\mathbf{n}(\frac{\theta }{2}){\mathbf{n}}_{\mathbf{x}},\mathbf{s}\mathbf{i}\mathbf{n}(\frac{\theta }{2}){\mathbf{n}}_{\mathbf{y}},\mathbf{s}\mathbf{i}\mathbf{n}(\frac{\theta }{2}){\mathbf{n}}_{\mathbf{z}}]$，${\mathbf{q}}_{\mathbf{R}}={{\mathbf{q}}_{\mathbf{L}}}^{-1}$，则是由于需要将该旋转限定在3维超平面内，引用博客中解释：

2.1. 四元数与旋转向量

\quad 基于上述理解，四元数（特指单位四元数，下同）转旋转向量表示为如下形式，反之易得旋转向量至四元数的转换关系：

\quad 二者转换代码实现如下：

typedef struct _Quaternionf_
{
	float w;	// q0
	float x;	// q1
	float y;	// q2
	float z;	// q3
}Quaternionf;

//[5] 旋转向量转四元数
void RotVec2Quad(Vec3f &vec, Quaternionf &q)
{
	float theta = norm(vec);
	float qtheta = theta/2;
	q.w = cos(qtheta);
	q.x = sin(qtheta) * vec[0] / theta;
	q.y = sin(qtheta) * vec[1] / theta;
	q.z = sin(qtheta) * vec[2] / theta;
}

//[6] 四元数转旋转向量
void Quad2RotVec(Quaternionf &q, Vec3f &vec)
{
	float theta = 2 * acos(q.w);
	float sint = sin(theta / 2);

	float nx = q.x / sint;
	float ny = q.y / sint;
	float nz = q.z / sint;
	
	vec[0] = theta * nx;
	vec[1] = theta * ny;
	vec[2] = theta * nz;
}

2.2. 四元数与旋转矩阵

\quad 四元数转旋转矩阵公式如下，其推导过程可参考高博《SLAM十四讲》3.4.4节，大致思路是将四元数的旋转表示：${\mathbf{p}}^{\prime }={\mathbf{q}}_{\mathbf{L}}\mathbf{p}{\mathbf{q}}_{\mathbf{R}}$，写作矩阵形式即可得到下述结果。

\quad 而旋转矩阵转四元数的思路，与转欧拉角的思路类似，都是从旋转矩阵的元素构成出发，构建方程求解四元数的元素：
$$\begin{gathered} tr(R)=3-4(q_1^2+q_2^2+q_3^2)=4q_0^2-1 \\ {q}_0=\frac{\sqrt{tr(R)+1}}{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}$$
\quad 此时会发现，上式不适用于$q_0$接近0的情况，而通过其他方式构造的方程大致都会遇到某种失效情况。因此，一般的做法是，先确定四元数的4个分量中哪个最大，而后再根据该分量构造求解方程，参考代码如下：

//[7] 四元数转旋转矩阵
void Quad2RotMat(Quaternionf &q, cv::Mat &rot)
{
	rot = Mat(Size(3, 3), CV_32FC1);

	rot.at<float>(0, 0) = 1 - 2 * q.y*q.y - 2 * q.z*q.z;
	rot.at<float>(0, 1) = 2 * q.x*q.y - 2 * q.w*q.z;
	rot.at<float>(0, 2) = 2 * q.x*q.z + 2 * q.w*q.y;
	rot.at<float>(1, 0) = 2 * q.x*q.y + 2 * q.w*q.z;
	rot.at<float>(1, 1) = 1 - 2 * q.x*q.x - 2 * q.z*q.z;
	rot.at<float>(1, 2) = 2 * q.y*q.z - 2 * q.w*q.x;
	rot.at<float>(2, 0) = 2 * q.x*q.z - 2 * q.w*q.y;
	rot.at<float>(2, 1) = 2 * q.y*q.z + 2 * q.w*q.x;
	rot.at<float>(2, 2) = 1 - 2 * q.x*q.x - 2 * q.y*q.y;
}

//[8] 旋转矩阵转四元数
void RotMat2Quad(cv::Mat &rot, Quaternionf &q)
{
	float r00 = rot.at<float>(0, 0);
	float r01 = rot.at<float>(0, 1);
	float r02 = rot.at<float>(0, 2);
	float r10 = rot.at<float>(1, 0);
	float r11 = rot.at<float>(1, 1);
	float r12 = rot.at<float>(1, 2);
	float r20 = rot.at<float>(2, 0);
	float r21 = rot.at<float>(2, 1);
	float r22 = rot.at<float>(2, 2);

	float qs[4];
	qs[0] = r00 + r11 + r22; //4*w*w -1
	qs[1] = r00 - r11 - r22; //4*x*x -1
	qs[2] = -r00 + r11 - r22; //4*y*y -1
	qs[3] = -r00 - r11 + r22; //4*z*z -1

	int index = -1;
	float maxqs = -1.0;

	for (int i = 0; i < 4; ++i)
	{
		if (maxqs < qs[i])
		{
			maxqs = qs[i];
			index = i;
		}
	}

	float qmax = sqrt(maxqs + 1) / 2;
	float denominator = qmax * 4;

	switch (index)
	{
	case 0:
		q.w = qmax;
		q.x = (r21 - r12) / denominator;
		q.y = (r02 - r20) / denominator;
		q.z = (r10 - r01) / denominator;
		break;

	case 1:
		q.x = qmax;
		q.w = (r21 - r12) / denominator;
		q.y = (r10 + r01) / denominator;
		q.z = (r20 + r02) / denominator;
		break;

	case 2:
		q.y = qmax;
		q.w = (r02 - r20) / denominator;
		q.x = (r10 + r01) / denominator;
		q.z = (r21 + r12) / denominator;
		break;


	case 3:
		break;
		q.z = qmax;
		q.w = (r10 - r01) / denominator;
		q.x = (r20 + r02) / denominator;
		q.y = (r21 + r12) / denominator;

	default:
		break;
	}

}

2.3. 四元数与欧拉角

\quad 由欧拉角转四元数的思路，与欧拉角转旋转矩阵类似，其表达形式如下：
$$\begin{gathered} q=q_z{q}_y{q}_x \\ {q}_x=\left[\begin{array}{c}cos(\frac{{\theta }_x}{2}),sin(\frac{{\theta }_x}{2}),0,0\end{array}\right] \\ {q}_y=\left[\begin{array}{c}cos(\frac{{\theta }_y}{2}),0,sin(\frac{{\theta }_y}{2}),0\end{array}\right] \\ {q}_z=\left[\begin{array}{c}cos(\frac{{\theta }_z}{2}),0,0,sin(\frac{{\theta }_z}{2})\end{array}\right] \end{gathered}$$
\quad 由此可得，最终表达式如下，后附参考代码：
$$\mathbf{q}=\left[\begin{array}{c}cos(\frac{{\theta }_x}{2})cos(\frac{{\theta }_y}{2})cos(\frac{{\theta }_z}{2})+sin(\frac{{\theta }_x}{2})sin(\frac{{\theta }_y}{2})sin(\frac{{\theta }_z}{2})\\ sin(\frac{{\theta }_x}{2})cos(\frac{{\theta }_y}{2})cos(\frac{{\theta }_z}{2})-cos(\frac{{\theta }_x}{2})sin(\frac{{\theta }_y}{2})sin(\frac{{\theta }_z}{2})\\ cos(\frac{{\theta }_x}{2})sin(\frac{{\theta }_y}{2})cos(\frac{{\theta }_z}{2})+sin(\frac{{\theta }_x}{2})cos(\frac{{\theta }_y}{2})sin(\frac{{\theta }_z}{2})\\ cos(\frac{{\theta }_x}{2})cos(\frac{{\theta }_y}{2})sin(\frac{{\theta }_z}{2})-sin(\frac{{\theta }_x}{2})sin(\frac{{\theta }_y}{2})cos(\frac{{\theta }_z}{2})\end{array}\right]$$
\quad 从四元数转欧拉角的思路与此前一致，仍然从元素构成出发，但由于其复杂度较高，未作推导，建议以旋转矩阵作为中转。

//[9] 欧拉角转四元数
void EulerAngle2Quad(Vec3f &euler, Quaternionf &q)
{
	float tx = euler[0] / 2;
	float ty = euler[1] / 2;
	float tz = euler[2] / 2;

	q.w = cos(tx)*cos(ty)*cos(tz) + sin(tx)*sin(ty)*sin(tz);
	q.x = sin(tx)*cos(ty)*cos(tz) - cos(tx)*sin(ty)*sin(tz);
	q.y = cos(tx)*sin(ty)*sin(tz) + sin(tx)*cos(ty)*sin(tz);
	q.z = cos(tx)*cos(ty)*sin(tz) - sin(tx)*sin(ty)*cos(tz);
}

参考链接

[1] https://www.3dgep.com/understanding-quaternions/#Introduction
[2] https://ntrs.nasa.gov/citations/19770019231
[3] https://zhuanlan.zhihu.com/p/45404840
[4] https://blog.csdn.net/lql0716/article/details/72597719