[PYTHON] Mémo de rotation 3D (1)

J'ai lu le livre "Calcul des paramètres de rotation tridimensionnelle et optimisation par l'algèbre de Lee". C'était un bon livre très facile à comprendre, mais il n'y avait presque pas d'exemples numériques, alors j'ai bougé les mains et vérifié diverses choses.

Ce qui suit est le contenu de diverses implémentations du contenu du chapitre 3 (car il est dérivé par lui-même, veuillez lire le livre pour plus de détails)

Je vais essayer diverses choses en utilisant les célèbres données du groupe de points de lapin

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

# Read PLY file
from pyntcloud import PyntCloud  #Utilisé pour lire PLY(pépin)
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)

Pour le moment, des données comme celles-ci

output_2_0.png

Angle du graisseur

Générer une matrice de rotation à partir de la représentation d'angle d'Euler et faire pivoter le groupe de points

#Générer une matrice de rotation à partir de l'angle d'Euler

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)Essayez de changer et de tourner

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

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

Mémo sur l'affichage de l'angle du graisseur

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

True

Cérémonie de la Rod League

Rotation selon la formule Rod League. Comment se décomposer en un axe de rotation et l'angle de rotation autour de cet axe

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

#Faire pivoter de 90 degrés autour de l'axe z
z_axis = np.identity(3)[:,2]
Rr1 = rodrigues2rotmat(z_axis, math.pi/2)
bunny2 = Rr1.dot(bunny.T).T

#Faire pivoter de 90 degrés autour de l'axe x
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)]Faire pivoter de 90 degrés autour
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


#Graphique de la ligne auxiliaire pour 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])

#terrain

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 par quarternion (quadruple). Le plus commun? (Je pense que le sens est plus facile à comprendre dans Rod League, mais quel est l'avantage d'utiliser des quadruples?)

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]  #Constante 1 dans les manuels/4 est manquant, faux
    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

Retour de la matrice de rotation à un nombre quaternaire.

rotmat2quaternion(Rq)==q

True

(J'ai créé des cahiers jupyter dans les chapitres 4 et 6, mais il est assez difficile de passer de jupyter à qiita markdown ...)

Recommended Posts

Mémo de rotation 3D (1)
Mémo Raspberry-pi
Mémo Pandas
Mémo HackerRank
mémo python
mémo graphène
Mémo du flacon
Mémo Matplotlib
mémo pytest
mémo sed
Mémo Python
Installer Memo
mémo networkx
mémo python
mémo Tomcat
mémo de commande
Mémo du générateur.
mémo psycopg2
Mémo Python
Mémo SSH
Mémo: rtl8812
mémo pandas
Mémo Shell
Mémo Python
Mémo Pycharm