四元數座標旋轉

在處理空間轉換時經常會遇到使用四元數進行轉換的場景，因此在這裏做下記錄。本篇博客不講理論(因爲作者也不懂，可能如果學習了會補充)，只放出轉換公式以及代碼。

設有三維點座標爲$(x0, y0, z0)$，空間旋轉後的點座標爲$(x1, y1, z1)$，四元數爲$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);
}