Go to Qiita Advent Calendar 2024 Top
LoginSignup
1
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 5 years have passed since last update.

@flyio

3次元回転のメモ (1)

Posted at

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

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

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

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

オイラー角

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

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

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

# (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

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

  • $\theta=0$ で一意性がなくなる (Gimbal lock)
  • $\phi+\psi$ が一定ならすべて同じ回転を意味する
  • 以下で確認
abs(eular2rotmat(0,math.pi/2,0) - eular2rotmat(0,math.pi/4,math.pi/4)).sum()<1e-10  # Ignoring numerical errors

True

ロドリーグの式

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

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

# 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

クォータニオン

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

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)

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

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

rotmat2quaternion(Rq)==q

True

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

1
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?