[PYTHON] Notiz der 3D-Rotation (1)

Ich las das Buch "Dreidimensionale Berechnung und Optimierung von Rotationsparametern durch Lees Algebra". Es war ein sehr leicht verständliches und gutes Buch, aber es gab fast keine numerischen Beispiele, also bewegte ich meine Hände und überprüfte verschiedene Dinge.

Das Folgende ist der Inhalt verschiedener Implementierungen des Inhalts von Kapitel 3 (Da es von selbst abgeleitet ist, lesen Sie bitte das Buch für Details).

Ich werde verschiedene Dinge anhand der Punktgruppendaten berühmter Kaninchen ausprobieren

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

# Read PLY file
from pyntcloud import PyntCloud  #Wird zum Lesen von PLY verwendet(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)

Daten wie diese vorerst

output_2_0.png

Ölerwinkel

Generieren Sie eine Rotationsmatrix aus der Euler-Winkeldarstellung und drehen Sie die Punktgruppe

#Generieren Sie eine Rotationsmatrix aus dem Euler-Winkel

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)Versuchen Sie, zu wechseln und zu drehen

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

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

Notiz über die Anzeige des Ölerwinkels

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

True

Rod League Zeremonie

Rotation nach der Rod League-Formel. Zerlegen in eine Rotationsachse und den Rotationswinkel um diese Achse

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

#90 Grad um die Z-Achse drehen
z_axis = np.identity(3)[:,2]
Rr1 = rodrigues2rotmat(z_axis, math.pi/2)
bunny2 = Rr1.dot(bunny.T).T

#90 Grad um die x-Achse drehen
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)]Um 90 Grad drehen
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


#Hilfslinienplot für Rod League
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])

#Handlung

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

Quarternion

Rotation durch Quarternion (vierfach). Am gebräuchlichsten? (Ich denke, die Bedeutung ist in Rod League leichter zu verstehen, aber was ist der Vorteil der Verwendung von Vierfachen?)

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]  #Konstante 1 in Lehrbüchern/4 fehlt, falsch
    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

Kehren Sie von der Rotationsmatrix zu einer quaternären Zahl zurück.

rotmat2quaternion(Rq)==q

True

(Ich habe in den Kapiteln 4 und 6 Jupyter-Notizbücher erstellt, aber es ist ziemlich mühsam, von Jupyter auf Qiita-Markdown umzustellen ...)

Recommended Posts

Notiz der 3D-Rotation (1)
Himbeer-Pi-Memo
Pandas Memo
HackerRank-Memo
Python-Memo
Graphen-Memo
Kolben Memo
Matplotlib-Memo
pytest memo
sed memo
Python-Memo
Installieren Sie Memo
networkx memo
Python-Memo
Kater Memo
Befehlsnotiz
Generator Memo.
psycopg2 memo
Python-Memo
SSH-Memo
Notiz: rtl8812
Pandas Memo
Shell Memo
Python-Memo
Pycharm-Memo