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
%matplotlib inline
np.set_printoptions(suppress=True, precision=3) # to avoid 1e-17 expressions in rotation matrix
Following Scene was generated with BlenSor and a Point Cloud Library File (*.pcd) was exported.
pc = pd.read_csv('Pointcloud00002.pcd', names=['x','y','z','rgb'], skiprows=11, sep=' ')
pc=pc[['x','y','z']]
pc.head(3)
x | y | z | |
---|---|---|---|
0 | 14.718567 | 1.070656 | -12.857123 |
1 | 13.841802 | 0.494996 | -11.860050 |
2 | 12.500180 | -0.431674 | -10.255011 |
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.
Z-Y-X-Konvention
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
@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 ]]
print('Nice!')
Nice!