四元数坐标旋转

最新推荐文章于 2024-07-26 14:07:16 发布

_寒潭雁影

最新推荐文章于 2024-07-26 14:07:16 发布

阅读量4.7k

点赞数 3

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

本文链接：https://blog.csdn.net/weixinhum/article/details/105581240

版权

PCL 专栏收录该内容

7 篇文章 2 订阅

订阅专栏

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

设有三维点坐标为 $(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，则他们之间的转换公式如下：

$\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);
}