四元数坐标旋转

绝地灬酷狼 2023-05-21 15:18 20阅读 0赞

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

设有三维点坐标为 ( x 0 , y 0 , z 0 ) (x0, y0, z0) (x0,y0,z0)，空间旋转后的点坐标为 ( x 1 , y 1 , z 1 ) (x1, y1, z1) (x1,y1,z1)，四元数为 Q = q 0 + q 1 i + q 2 j + q 3 k Q=q\_\{0\}+q\_\{1\}i+q\_\{2\}j+q\_\{3\}k Q=q0\+q1i\+q2j\+q3km，则他们之间的转换公式如下：

\[ x 1 y 1 z 1 \] = \[ q 0 2 + q 1 2 − q 2 2 − q 3 2 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 0 q 2 + q 1 q 3 ) 2 ( q 0 q 3 + q 1 q 2 ) q 0 2 − q 1 2 + q 2 2 − q 3 2 2 ( q 2 q 3 − q 0 q 1 ) 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 0 q 1 + q 2 q 3 ) q 0 2 − q 1 2 − q 2 2 + q 3 2 \] \[ x 0 y 0 z 0 \] \\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\] ⎣⎡x1y1z1⎦⎤=⎣⎡q02\+q12−q22−q322(q0q3\+q1q2)2(q1q3−q0q2)2(q1q2−q0q3)q02−q12\+q22−q322(q0q1\+q2q3)2(q0q2\+q1q3)2(q2q3−q0q1)q02−q12−q22\+q32⎦⎤⎣⎡x0y0z0⎦⎤

使用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);
    }