[PYTHON] 3D rotation memo (1)

I read the book "3D Rotation Parameter Calculation and Lie Algebra Optimization". It was a very easy-to-understand and good book, but there were almost no numerical examples, so I moved my hand and checked various things.

The following is the content of various implementations of the contents of Chapter 3 (Because it is derived by itself, please read the book for details)

I will try various things using the point cloud data of the famous rabbit

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

# Read PLY file
from pyntcloud import PyntCloud  #Used to read 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)

For the time being, data like this

output_2_0.png

Euler angles

Generate a rotation matrix from Euler angles representation and rotate the point cloud

#Generate a rotation matrix from Euler angles

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)Try changing and rotating

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

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

Notes on Euler angles display

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

True

Rod League Ceremony

Rotation using the Rod League formula. How to decompose into a rotation axis and the rotation angle around that axis

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

#Rotate 90 degrees around the z-axis
z_axis = np.identity(3)[:,2]
Rr1 = rodrigues2rotmat(z_axis, math.pi/2)
bunny2 = Rr1.dot(bunny.T).T

#Rotate 90 degrees around the x-axis
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)]Rotate 90 degrees around
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


#Auxiliary line plot for 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])

#plot

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

Quaternion

Rotation by quaternion (quaternion). Most common? (I think the meaning is easier to understand in Rod League, but what is the advantage of using quaternions?)

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]  #Constant 1 in textbooks/4 is missing, wrong
    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

Return from the rotation matrix to a quaternion.

rotmat2quaternion(Rq)==q

True

(I made jupyter notebook for chapters 4 and 6, but it's quite troublesome to convert from jupyter to qiita markdown ...)

Recommended Posts

3D rotation memo (1)
Raspberry-pi memo
Pandas memo
HackerRank memo
python memo
graphene memo
Flask memo
Matplotlib memo
pytest memo
sed memo
Python memo
Install Memo
networkx memo
python memo
tomcat memo
command memo
Generator memo.
psycopg2 memo
Python memo
SSH memo
Command memo
Memo: rtl8812
pandas memo
Shell memo
Python memo
Pycharm memo
Python memo