【SLAM】四元数

旋转矩阵用9个量描述3自由度的运动，具有冗余性。欧拉角和旋转向量是紧凑的，但具有奇异性。

而复数的乘法表示复平面上的旋转。乘上复数i相当于逆时针旋转一个复向量90°。例如，3+4i∗i=(−4+3i)。类似的，在表达三维空间旋转时，有一种类似于复数的代数-四元数。其格式为𝐪=𝑞0+𝑞1𝑖+𝑞2𝑗+𝑞3𝑘。可以用单位四元数表示三维空间中任意一个旋转。

假设一个空间三维点𝒑=[x,y,z]∈ℝ3，用矩阵R描述旋转，则旋转之后的点p' 表示为𝒑′=R𝒑。三维空间点用虚四元数表示，可以想象四元数的3个虚部对应空间中的3个轴。用一个单位四元数q指定旋转，则旋转之后的点p' 可表示为𝒑′=𝒒𝐩𝒒−1。把p' 的虚部取出，即为旋转后点的坐标，并且计算结果的实部为0。

四元数也可以写成矩阵形式，则两个四元数的乘法可以如下表示：

对于四元数𝒒=[𝑠,𝒗]𝑇，定义如下符号+和⊕：

=，=，则四元数被映射为4x4的矩阵。上面表示的四元数乘法就可以写为，同理可证也能写为，则=

= =，由于三维空间中的点都是虚四元数，因此，该矩阵的下方，𝒗𝒗𝑇+𝑠2𝑰+2𝑠𝒗∧+(𝒗∧)2 表示了旋转矩阵。

import numpy
import math
_EPS = numpy.finfo(float).eps * 4.0
def quaternion_matrix(quaternion):
    '''Return homogeneous rotation matrix from quaternion'''
 
    q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
    n = numpy.dot(q, q)
    if n < _EPS:
        return numpy.identity(4)
    q *= math.sqrt(2.0 / n)
    q = numpy.outer(q, q)
    return numpy.array([
        [1.0-q[2, 2]-q[3, 3],     q[1, 2]-q[3, 0],     q[1, 3]+q[2, 0], 0.0],
        [    q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3],     q[2, 3]-q[1, 0], 0.0],
        [    q[1, 3]-q[2, 0],     q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0],
        [                0.0,                 0.0,                 0.0, 1.0]])

print(quaternion_matrix([0.99810947, 0.06146124, 0, 0]))

 

 

文章内容参考高博《视觉SLAM十四讲》

代码参考delta_robot/transformations.py at master · awesomebytes/delta_robot · GitHub