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.

Scene

In [202]:
pc = pd.read_csv('Pointcloud00002.pcd', names=['x','y','z','rgb'], skiprows=11, sep=' ')
pc=pc[['x','y','z']]
pc.head(3)
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
    '''
    # from Degree to Radians
    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!