# 3D Rotation of a Point Cloud¶

In [200]:
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from IPython.html.widgets import interact
from IPython.html import widgets
import pandas as pd

In [201]:
%matplotlib inline
np.set_printoptions(suppress=True, precision=3) # to avoid 1e-17 expressions in rotation matrix


## Read Point Cloud Library File¶

Following Scene was generated with BlenSor and a Point Cloud Library File (*.pcd) was exported.

In [202]:
pc = pd.read_csv('Pointcloud00002.pcd', names=['x','y','z','rgb'], skiprows=11, sep=' ')
pc=pc[['x','y','z']]

Out[202]:
x y z
0 14.718567 1.070656 -12.857123
1 13.841802 0.494996 -11.860050
2 12.500180 -0.431674 -10.255011

### Quick Look¶

In [203]:
fig = plt.figure()
ax = Axes3D(fig)
ax.scatter(pc.x.values, pc.y.values, pc.z.values)
plt.xlabel('x');
plt.ylabel('y');


Look's like it is in local sensor coordinate system. If we rotate the point cloud like the scanner was rotated, we might get the correct obstacle.

## Now let's Rotate it¶

Z-Y-X-Konvention

In [204]:
def Rypr(y, p, r):
'''
Rotationsmatrix für y=yaw, p=pitch, r=roll in degrees
'''
y = y*np.pi/180.0
p = p*np.pi/180.0
r = r*np.pi/180.0

Rr = np.matrix([[1.0, 0.0, 0.0],[0.0, np.cos(r), -np.sin(r)],[0.0, np.sin(r), np.cos(r)]])
Rp = np.matrix([[np.cos(p), 0.0, np.sin(p)],[0.0, 1.0, 0.0],[-np.sin(p), 0.0, np.cos(p)]])
Ry = np.matrix([[np.cos(y), -np.sin(y), 0.0],[np.sin(y), np.cos(y), 0.0],[0.0, 0.0, 1.0]])

return Ry*Rp*Rr


You need to download the IPython Notebook and run it locally, to enable interactivity

In [205]:
@interact
def plotrot3Dpersp(gieren = widgets.FloatSliderWidget(min=-180.0, max=180.0, step=1.0, value=-120.0, description=""), \
nicken = widgets.FloatSliderWidget(min=-180.0, max=180.0, step=1.0, value=0.0, description=""), \
wanken = widgets.FloatSliderWidget(min=-180.0, max=180.0, step=1.0, value=60.0, description=""), \
kippen = widgets.FloatSliderWidget(min=0.0, max=90.0, step=1.0, value=2.0, description=""), \
rotieren = widgets.FloatSliderWidget(min=-180.0, max=180.0, step=1.0, value=100.0, description="")):

R = Rypr(gieren, nicken, wanken)
print('%s' % R)

x, y, z = R * np.array([pc.values]).T

fig = plt.figure(figsize=(14,6))
ax = Axes3D(fig)
ax.scatter(x.tolist(), y.tolist(), z.tolist())
ax.axis('equal')
ax.set_zlim3d(-6.0, 1.0)
plt.xlabel('x');
plt.ylabel('y');
plt.title('\nG: %i, N: %i, W: %i' % (gieren, nicken, wanken))
ax.view_init(kippen, rotieren)

[[-0.5    0.433 -0.75 ]
[-0.866 -0.25   0.433]
[ 0.     0.866  0.5  ]]

In [206]:
print('Nice!')

Nice!