3次元回転のメモ (1)

「3次元回転 パラメータ計算とリー代数による最適化」という本を読んだ．
とてもわかりやすくて良い本だったが，数値例などがほぼなかったので，自分で手を動かして，いろいろ確認してみた．

以下は3章の内容をいろいろ実装してみた内容です (単体では導出などしていなので，詳細は本を読んでください)


有名なウサギの点群データを使っていろいろやってみます

```python
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import math

# Read PLY file
from pyntcloud import PyntCloud  # PLY を読み込むのに使う (pipでおｋ)
x = PyntCloud.from_file("bunny/data/bun000.ply")
bunny = x.points.dropna().values  # x.points is a pandas dataframe
bunny -= bunny.mean(axis=0,keepdims=True)  # normalize
bunny = bunny[:,[2,0,1]]   # swap axis for visibility

# Plot bunny
fig = plt.figure(figsize=(5,5))
ax = plt.subplot(111,projection='3d')
ax.plot(bunny[:,0],bunny[:,1],bunny[:,2],'.',markersize=1,color='#00000011')
ax.view_init(20,20.5)
```

とりあえずこんな感じのデータ

![output_2_0.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/28651/0052a5f0-1e60-8537-b478-fc383da3e803.png)


## オイラー角

オイラー角表現から回転行列を生成して，点群を回転させる

```python
# オイラー角から回転行列を生成

def eular2rotmat(theta, phi, psi):
    cos_t, sin_t = math.cos(theta), math.sin(theta)
    cos_h, sin_h = math.cos(phi), math.sin(phi)
    cos_s, sin_s = math.cos(psi), math.sin(psi)
    R = np.array([[cos_h*cos_s-sin_h*cos_t*sin_s, -cos_h*sin_s-sin_h*cos_t*cos_s, sin_h*sin_t],
                  [sin_h*cos_s+cos_h*cos_t*sin_s, -sin_h*sin_s+cos_h*cos_t*cos_s,-cos_h*sin_t],
                  [sin_t*sin_s                  , sin_t*cos_s                   , cos_t    ]])
    return R
```


```python
# (theta, phi, psi) をいろいろ変えて回転させてみる

phi = math.pi/2
R1 = eular2rotmat(0,phi,0)
bunny2 = R1.dot(bunny.T).T

theta = math.pi/4
R2 = eular2rotmat(theta,phi,0)
bunny3 = R2.dot(bunny.T).T

psi = -math.pi/4
R3 = eular2rotmat(theta,phi,psi)
bunny4 = R3.dot(bunny.T).T

# プロット
fig = plt.figure(figsize=(10,10))

ax = plt.subplot(221,projection='3d')
ax.plot(bunny[:,0],bunny[:,1],bunny[:,2],'.',markersize=1,color='#00000011')
ax.view_init(20,20.5)
ax.set_title("original")

ax = plt.subplot(222,projection='3d')
ax.plot(bunny2[:,0],bunny2[:,1],bunny2[:,2],'.',markersize=1,color='#00000011')
ax.view_init(20,20.5)
ax.set_title(r"$(\theta, \phi, \psi)=(0,%d,0)$ degree rotated" % int(phi/math.pi*180))

ax = plt.subplot(223,projection='3d')
ax.plot(bunny3[:,0],bunny3[:,1],bunny3[:,2],'.',markersize=1,color='#00000011')
ax.view_init(20,20.5)
ax.set_title(r"$(\theta, \phi, \psi)=(%d,%d,0)$ degree rotated" % (int(theta/math.pi*180), int(phi/math.pi*180)))

ax = plt.subplot(224,projection='3d')
ax.plot(bunny4[:,0],bunny4[:,1],bunny4[:,2],'.',markersize=1,color='#00000011')
ax.view_init(20,20.5)
ax.set_title(r"$(\theta, \phi, \psi)=(%d,%d,%d)$ degree rotated" % (int(theta/math.pi*180), int(phi/math.pi*180), int(psi/math.pi*180)))

plt.show()
```


![output_6_0.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/28651/eb1d090e-4650-7bd8-b6d3-a05d3a9f5245.png)


### オイラー角表示に関するメモ

+ $\theta=0$ で一意性がなくなる (Gimbal lock)
+ $\phi+\psi$ が一定ならすべて同じ回転を意味する
+ 以下で確認


```python
abs(eular2rotmat(0,math.pi/2,0) - eular2rotmat(0,math.pi/4,math.pi/4)).sum()<1e-10  # Ignoring numerical errors
```

    True



## ロドリーグの式

ロドリーグの式を使った回転．回転軸とその軸回りの回転角度に分解して考える方法

```python
def rodrigues(x, axis_vec, omega):
    # axis_vec is a 3d vector with l2-norm=1
    cos_omega = math.cos(omega)
    return cos_omega * x + np.outer(axis_vec, x) * math.sin(omega) + axis_vec.dot(x) * x * (1-cos_omega)

def rodrigues2rotmat(axis_vec, omega):
    cos_omega = math.cos(omega)
    m_cos_omega = 1-cos_omega
    sin_omega = math.sin(omega)
    l1,l2,l3 = axis_vec
    R = np.array([[cos_omega + l1*l1*m_cos_omega, l1*l2*m_cos_omega-l3*sin_omega, l1*l3*m_cos_omega+l2*sin_omega],
                 [l2*l1*m_cos_omega+l3*sin_omega,cos_omega+l2*l2*m_cos_omega, l2*l3*m_cos_omega-l1*sin_omega],
                 [l3*l1*m_cos_omega-l2*sin_omega,l3*l2*m_cos_omega+l1*sin_omega, cos_omega+l3*l3*m_cos_omega]])
    return R
```


```python
# z軸回りに90度回転
z_axis = np.identity(3)[:,2]
Rr1 = rodrigues2rotmat(z_axis, math.pi/2)
bunny2 = Rr1.dot(bunny.T).T

# x軸回りに90度回転
x_axis = np.identity(3)[:,0]
Rr2 = rodrigues2rotmat(x_axis, math.pi/2)
bunny3 = Rr2.dot(bunny.T).T

# [1/sqrt(3),1/sqrt(3),1/sqrt(3)] 周りに90度回転
axis111 = np.array([(-1.0)**i/math.sqrt(3) for i in range(3)])
Rr3 = rodrigues2rotmat(axis111, math.pi/2)
bunny4 = Rr3.dot(bunny.T).T


# ロドリーグ用の補助線プロット
def plot_rodrigue(ax, axis):
    lims = ax.get_xlim(), ax.get_ylim(), ax.get_zlim()
    #x, y, z = [(-min(lims[i][0],axis[i]), min(lims[i][1],axis[i])) for i in range(3)]
    x,y,z = [np.linspace(-axis[i],axis[i],100) for i in range(3)]
    def ok(p,lim):
        if lim[0]<=p and p<=lim[1]:
            return True
        return False
    points = []
    for i in range(100):
        if ok(x[i],lims[0]) and ok(y[i],lims[1]) and ok(z[i],lims[2]):
            points.append([x[i],y[i],z[i]])
    points = np.array(points)
    ax.plot(points[:,0],points[:,1],points[:,2],color="blue",linestyle='dashed',linewidth=3,zorder=-9999)
    ax.set_xlim(lims[0])
    ax.set_ylim(lims[1])
    ax.set_zlim(lims[2])

# プロット

fig = plt.figure(figsize=(10,10))

ax = plt.subplot(221,projection='3d')
ax.plot(bunny[:,0],bunny[:,1],bunny[:,2],'.',markersize=1,color='#00000011')
ax.view_init(20,20.5)
ax.set_title("original")

ax = plt.subplot(222,projection='3d')
ax.plot(bunny2[:,0],bunny2[:,1],bunny2[:,2],'.',markersize=1,color='#00000011')
plot_rodrigue(ax, z_axis)
ax.view_init(20,20.5)
ax.set_title(r"Rotation with $(l, \Omega)=(%s, %d deg)$" % (str(z_axis),90))

ax = plt.subplot(223,projection='3d')
ax.plot(bunny3[:,0],bunny3[:,1],bunny3[:,2],'.',markersize=1,color='#00000011')
plot_rodrigue(ax, x_axis)
ax.view_init(20,20.5)
ax.set_title(r"Rotation with $(l, \Omega)=(%s, %d deg)$" % (str(x_axis),90))

ax = plt.subplot(224,projection='3d')
ax.plot(bunny4[:,0],bunny4[:,1],bunny4[:,2],'.',markersize=1,color='#00000011')
plot_rodrigue(ax, axis111)
ax.view_init(20,20.5)
ax.set_title(r"Rotation with $(l, \Omega)=(%s, %d deg)$" % (str([round(xx,2) for xx in axis111]),90))

```




    Text(0.5, 0.92, 'Rotation with $(l, \\Omega)=([0.58, -0.58, 0.58], 90 deg)$')




![output_13_1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/28651/072acf5f-725f-6c9b-9ff9-978267eb6dd1.png)


## クォータニオン

クォータニオン (四元数) による回転．一番一般的？
(意味合いはロドリーグのほうがわかりやすいと思うけど四元数を使う利点って何？)

```python
def normalize_quaternion(q):
    q0, q1, q2, q3 = q
    Q = math.sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3)
    return (q0/Q,q1/Q,q2/Q,q3/Q)

def quaternion2rotmat(q):
    q0, q1, q2, q3 = q
    R = np.array([[q0*q0+q1*q1-q2*q2-q3*q3, 2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2)],
                  [2*(q2*q1+q0*q3), q0*q0-q1*q1+q2*q2-q3*q3, 2*(q2*q3-q0*q1)],
                  [2*(q3*q1-q0*q2), 2*(q3*q2+q0*q1), q0*q0-q1*q1-q2*q2+q3*q3]])
    return R

def rotmat2quaternion(R):
    q0 = math.sqrt(1+sum([R[i,i] for i in range(3)]))/2.0
    q1, q2, q3 = [-(R[1,2]-R[2,1])/4./q0, -(R[2,0]-R[0,2])/4./q0, -(R[0,1]-R[1,0])/4./q0]  # 教科書では定数1/4が抜けており，間違い
    return (q0,q1,q2,q3)
```


```python
q = normalize_quaternion((1,1,1,1))
Rq = quaternion2rotmat(q)
bunny2 = Rq.dot(bunny.T).T


fig = plt.figure(figsize=(10,5))

ax = plt.subplot(121,projection='3d')
ax.plot(bunny[:,0],bunny[:,1],bunny[:,2],'.',markersize=1,color='#00000011')
ax.view_init(20,20.5)
ax.set_title("original")

ax = plt.subplot(122,projection='3d')
ax.plot(bunny2[:,0],bunny2[:,1],bunny2[:,2],'.',markersize=1,color='#00000011')
ax.view_init(20,20.5)
ax.set_title(r"Rotation with $q=%s$" % str(q))
```




    Text(0.5, 0.92, 'Rotation with $q=(0.5, 0.5, 0.5, 0.5)$')




![output_17_1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/28651/8f8d9bcf-4168-4752-313f-31ecd72b3e8e.png)


回転行列から四元数に戻す．

```python
rotmat2quaternion(Rq)==q
```

    True

(4章と6章も jupyter notebook は作ったけど，jupyter から qiita markdown に変換するの結構面倒...)