三维空间的旋转矩阵

最近在做基于人体骨骼的动作识别实验时，需要统一人体骨骼在三维空间中的角度问题，解决这个问题的时候涉及到了旋转矩阵的问题，所以在博客里mark一下。

一. 旋转矩阵是啥？

旋转矩阵（Rotation matrix）是在乘以一个向量的时候有改变向量的方向但不改变大小的效果并保持了手性的矩阵

二. 旋转矩阵怎么求？

实验中用的方法是角-轴表示。在三维空间中，旋转可以通过单一的旋转角 θ 和所围绕的单位向量 v⃗ =（x,y,z） 来定义。

M(v⃗ ,θ)=⎡⎣⎢⎢cosθ+(1−cosθ)x2(1−cosθ)yx+(sinθ)z(1−cosθ)zx−(sinθ)y(1−cosθ)xy−(sinθ)zcosθ+(1−cosθ)y2(1−cosθ)zy+(sinθ)x(1−cosθ)xz+(sinθ)y(1−cosθ)yz−(sinθ)xcosθ+(1−cosθ)z2⎤⎦⎥⎥

其中旋转角 θ 和所围绕的单位向量 v⃗  可以通过一下方式求得。
不妨设 a⃗  ， b⃗  ，且 b⃗  是由 a⃗  旋转得到的，则，
旋转角 θ 可以由下面公式得到：

θ=arccosa⃗ ⋅b⃗ |a⃗ ||b⃗ |

单位向量 v⃗  可以由下面公式得到：

v⃗ =b⃗ ×a⃗ |b⃗ ×a⃗ |

三. 实验代码

import math
import numpy

def cross(a, b):
    return (a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0])

def dot(a, b):
    return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]

def normalize(a):
    a = numpy.array(a)
    return numpy.sqrt(numpy.sum(numpy.power(a, 2)))

def cal_rotate_matrix(a, b):
    rot_axis = cross(b, a)
    rot_angle = math.acos(dot(a, b) / normalize(a) / normalize(b))

    norm = normalize(rot_axis)
    rot_mat = numpy.zeros((3, 3), dtype = "float32")

    rot_axis = (rot_axis[0] / norm, rot_axis[1] / norm, rot_axis[2] / norm)

    rot_mat[0, 0] = math.cos(rot_angle) + rot_axis[0] * rot_axis[0] * (1 - math.cos(rot_angle))
    rot_mat[0, 1] = rot_axis[0] * rot_axis[1] * (1 - math.cos(rot_angle)) - rot_axis[2] * math.sin(rot_angle)
    rot_mat[0, 2] = rot_axis[1] * math.sin(rot_angle) + rot_axis[0] * rot_axis[2] * (1 - math.cos(rot_angle))

    rot_mat[1, 0] = rot_axis[2] * math.sin(rot_angle) + rot_axis[0] * rot_axis[1] * (1 - math.cos(rot_angle))
    rot_mat[1, 1] = math.cos(rot_angle) + rot_axis[1] * rot_axis[1] * (1 - math.cos(rot_angle))
    rot_mat[1, 2] = -rot_axis[0] * math.sin(rot_angle) + rot_axis[1] * rot_axis[2] * (1 - math.cos(rot_angle))

    rot_mat[2, 0] = -rot_axis[1] * math.sin(rot_angle) + rot_axis[0] * rot_axis[2] * (1 - math.cos(rot_angle))
    rot_mat[2, 1] = rot_axis[0] * math.sin(rot_angle) + rot_axis[1] * rot_axis[2] * (1 - math.cos(rot_angle))
    rot_mat[2, 2] = math.cos(rot_angle) + rot_axis[2] * rot_axis[2] * (1 - math.cos(rot_angle))

    return numpy.matrix(rot_mat)

if __name__ == '__main__':
    a = (-0.006576016845720566, 0.20515224329972243, 0.011860567926381188)
    b = (0, 0.2056, 0)
    rot_mat = cal_rotate_matrix(a, b)

    print b
    print numpy.array(a) * rot_mat

四. 实验结果

(0, 0.2056, 0)
[[  3.11651308e-10   2.05599994e-01  -5.09138034e-10]]