四元数坐标旋转

最新推荐文章于 2025-07-02 21:18:34 发布

_寒潭雁影

最新推荐文章于 2025-07-02 21:18:34 发布

阅读量5.1k

点赞数 4

CC 4.0 BY-SA版权

分类专栏： PCL 文章标签：四元数坐标转换

本文链接：https://blog.youkuaiyun.com/weixinhum/article/details/105581240

PCL 专栏收录该内容

7 篇文章

订阅专栏

在处理空间转换时经常会遇到使用四元数进行转换的场景，因此在这里做下记录。本篇博客不讲理论(因为作者也不懂，可能如果学习了会补充)，只放出转换公式以及代码。

设有三维点坐标为 $(x 0, y 0, z 0)$ ，空间旋转后的点坐标为 $(x 1, y 1, z 1)$ ，四元数为 $Q=q_{0}+q_{1}i+q_{2}j+q_{3}k$ m，则他们之间的转换公式如下：

$[x1y1z1]=[q02+q12−q22−q322(q1q2−q0q3)2(q0q2+q1q3)2(q0q3+q1q2)q02−q12+q22−q322(q2q3−q0q1)2(q1q3−q0q2)2(q0q1+q2q3)q02−q12−q22+q32][x0y0z0]\left[\begin{array}{l} x_{1} \\ y_{1} \\ z_{1} \end{array}\right]=\left[\begin{array}{ccc} q_{0}^{2}+q_{1}^{2}-q_{2}^{2}-q_{3}^{2} & 2\left(q_{1} q_{2}-q_{0} q_{3}\right) & 2\left(q_{0} q_{2}+q_{1} q_{3}\right) \\ 2\left(q_{0} q_{3}+q_{1} q_{2}\right) & q_{0}^{2}-q_{1}^{2}+q_{2}^{2}-q_{3}^{2} & 2\left(q_{2} q_{3}-q_{0} q_{1}\right) \\ 2\left(q_{1} q_{3}-q_{0} q_{2}\right) & 2\left(q_{0} q_{1}+q_{2} q_{3}\right) & q_{0}^{2}-q_{1}^{2}-q_{2}^{2}+q_{3}^{2} \end{array}\right]\left[\begin{array}{l} x_{0} \\ y_{0} \\ z_{0} \end{array}\right]$

使用C++进行转换代码如下:

void quaternionTrans(pcl::PointXYZRGB p1,pcl::PointXYZRGB &p2, Pose q)
{
	p2.x = p1.x * (q.q0*q.q0 + q.q1*q.q1 - q.q2*q.q2 - q.q3*q.q3) +
		p1.y * 2 * (q.q1*q.q2 - q.q0*q.q3) +
		p1.z * 2 * (q.q0*q.q2 + q.q1*q.q3);

	p2.y = p1.x * 2 * (q.q0*q.q3 + q.q1*q.q2) +
		p1.y * (q.q0*q.q0 - q.q1*q.q1 + q.q2*q.q2 - q.q3*q.q3) +
		p1.z * 2 * (q.q2*q.q3 - q.q0*q.q1);

	p2.z = p1.x * 2 * (q.q1*q.q3 - q.q0*q.q2) +
		p1.y * 2 * (q.q0*q.q1 + q.q2*q.q3) +
		p1.z * (q.q0*q.q0 - q.q1*q.q1 - q.q2*q.q2 + q.q3*q.q3);
}