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
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()
abs(eular2rotmat(0,math.pi/2,0) - eular2rotmat(0,math.pi/4,math.pi/4)).sum()<1e-10 # Ignoring numerical errors
True
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)$')
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)$')
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