import matplotlib.pyplot as plt
def visualization2D_xyz(new_cloud_data):
# set the size of pyplot charts
plt.rcParams['figure.figsize'] = (14, 6)
# split the rgb column into three columns: red, green and blue
rgb_columns = np.asarray(random_color_gen())
# normalize the rgb values (they should be between [0, 1])
rgb_columns = (rgb_columns / 255.0).astype(np.float)
# plot the points of the columns
plt.scatter(new_cloud_data[:, [0]], -new_cloud_data[:, [1]], color=rgb_columns)
# scale the axis equally
# (Note: append a semicolon to your last line to prevent jupyter notebook
# from outputting your last cell's content)
plt.axis('scaled');
print("(x) : {:2.1f}m".format(new_cloud_data[:,0:1].max() - new_cloud_data[:,0:1].min()))
print("(y) : {:2.1f}m".format(new_cloud_data[:,1:2].max() - new_cloud_data[:,1:2].min()))
print("(z) : {:2.1f}m".format(new_cloud_data[:,2:3].max() - new_cloud_data[:,2:3].min()))
%matplotlib inline
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
# Create a figure with a subplot with three axes
def visualization3D_xyz(new_cloud_data):
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# split the rgb column into three columns: red, green and blue
rgb_columns = np.asarray(random_color_gen())
# normalize the rgb values (they should be between [0, 1])
rgb_columns = (rgb_columns / 255.0).astype(np.float)
ax.scatter(new_cloud_data[:,0], new_cloud_data[:,1], new_cloud_data[:,2], color=rgb_columns);
print("(x) : {:2.1f}m".format(new_cloud_data[:,0:1].max() - new_cloud_data[:,0:1].min()))
print("(y) : {:2.1f}m".format(new_cloud_data[:,1:2].max() - new_cloud_data[:,1:2].min()))
print("(z) : {:2.1f}m".format(new_cloud_data[:,2:3].max() - new_cloud_data[:,2:3].min()))
%matplotlib inline
from random import randint
import struct
import ctypes
def random_color_gen():
""" Generates a random color
Args: None
Returns:
list: 3 elements, R, G, and B
"""
r = randint(0, 255)
g = randint(0, 255)
b = randint(0, 255)
return [r, g, b]
from mayavi import mlab
mlab.init_notebook()
s = mlab.test_plot3d()
s
# ==============================================================================
# VIZ_MAYAVI
# Input : kitti Raw Dataset
# ==============================================================================
def viz_mayavi(points, vals="distance"):
x = points[:, 0] # x position of point
y = points[:, 1] # y position of point
z = points[:, 2] # z position of point
# r = lidar[:, 3] # reflectance value of point
d = np.sqrt(x ** 2 + y ** 2) # Map Distance from sensor
# Plot using mayavi -Much faster and smoother than matplotlib
import mayavi.mlab
if vals == "height":
col = z
else:
col = d
fig = mayavi.mlab.figure(bgcolor=(0, 0, 0), size=(640, 360))
mayavi.mlab.points3d(x, y, z,
col, # Values used for Color
mode="point",
colormap='spectral', # 'bone', 'copper', 'gnuplot'
# color=(0, 1, 0), # Used a fixed (r,g,b) instead
figure=fig,
)
mayavi.mlab.show()
import numpy as np
from mayavi import mlab
try:
raw_input # Python 2
except NameError:
raw_input = input # Python 3
def draw_lidar(pc, color=None, fig=None, bgcolor=(0,0,0), pts_scale=1, pts_mode='point', pts_color=None):
''' Draw lidar points
Args:
pc: numpy array (n,3) of XYZ
color: numpy array (n) of intensity or whatever
fig: mayavi figure handler, if None create new one otherwise will use it
Returns:
fig: created or used fig
'''
if fig is None: fig = mlab.figure(figure=None, bgcolor=bgcolor, fgcolor=None, engine=None, size=(1600, 1000))
if color is None: color = pc[:,2]
mlab.points3d(pc[:,0], pc[:,1], pc[:,2], color, color=pts_color, mode=pts_mode, colormap = 'gnuplot', scale_factor=pts_scale, figure=fig)
#draw origin
mlab.points3d(0, 0, 0, color=(1,1,1), mode='sphere', scale_factor=0.2)
#draw axis
axes=np.array([
[2.,0.,0.,0.],
[0.,2.,0.,0.],
[0.,0.,2.,0.],
],dtype=np.float64)
mlab.plot3d([0, axes[0,0]], [0, axes[0,1]], [0, axes[0,2]], color=(1,0,0), tube_radius=None, figure=fig)
mlab.plot3d([0, axes[1,0]], [0, axes[1,1]], [0, axes[1,2]], color=(0,1,0), tube_radius=None, figure=fig)
mlab.plot3d([0, axes[2,0]], [0, axes[2,1]], [0, axes[2,2]], color=(0,0,1), tube_radius=None, figure=fig)
mlab.view(azimuth=180, elevation=70, focalpoint=[ 12.0909996 , -1.04700089, -2.03249991], distance=62.0, figure=fig)
return fig
def draw_gt_boxes3d(gt_boxes3d, fig, color=(1,1,1), line_width=1, draw_text=True, text_scale=(1,1,1), color_list=None):
''' Draw 3D bounding boxes
Args:
gt_boxes3d: numpy array (n,8,3) for XYZs of the box corners
fig: mayavi figure handler
color: RGB value tuple in range (0,1), box line color
line_width: box line width
draw_text: boolean, if true, write box indices beside boxes
text_scale: three number tuple
color_list: a list of RGB tuple, if not None, overwrite color.
Returns:
fig: updated fig
'''
num = len(gt_boxes3d)
for n in range(num):
b = gt_boxes3d[n]
if color_list is not None:
color = color_list[n]
if draw_text: mlab.text3d(b[4,0], b[4,1], b[4,2], '%d'%n, scale=text_scale, color=color, figure=fig)
for k in range(0,4):
#http://docs.enthought.com/mayavi/mayavi/auto/mlab_helper_functions.html
i,j=k,(k+1)%4
mlab.plot3d([b[i,0], b[j,0]], [b[i,1], b[j,1]], [b[i,2], b[j,2]], color=color, tube_radius=None, line_width=line_width, figure=fig)
i,j=k+4,(k+1)%4 + 4
mlab.plot3d([b[i,0], b[j,0]], [b[i,1], b[j,1]], [b[i,2], b[j,2]], color=color, tube_radius=None, line_width=line_width, figure=fig)
i,j=k,k+4
mlab.plot3d([b[i,0], b[j,0]], [b[i,1], b[j,1]], [b[i,2], b[j,2]], color=color, tube_radius=None, line_width=line_width, figure=fig)
#mlab.show(1)
#mlab.view(azimuth=180, elevation=70, focalpoint=[ 12.0909996 , -1.04700089, -2.03249991], distance=62.0, figure=fig)
return fig
def get_3d_box(box_size, heading_angle, center):
''' Calculate 3D bounding box corners from its parameterization.
Input:
box_size: tuple of (l,w,h)
heading_angle: rad scalar, clockwise from pos x axis
center: tuple of (x,y,z)
Output:
corners_3d: numpy array of shape (8,3) for 3D box cornders
'''
def roty(t):
c = np.cos(t)
s = np.sin(t)
return np.array([[c, 0, s],
[0, 1, 0],
[-s, 0, c]])
R = roty(heading_angle)
l,w,h = box_size
x_corners = [l/2,l/2,-l/2,-l/2,l/2,l/2,-l/2,-l/2];
y_corners = [h/2,h/2,h/2,h/2,-h/2,-h/2,-h/2,-h/2];
z_corners = [w/2,-w/2,-w/2,w/2,w/2,-w/2,-w/2,w/2];
corners_3d = np.dot(R, np.vstack([x_corners,y_corners,z_corners]))
corners_3d[0,:] = corners_3d[0,:] + center[0];
corners_3d[1,:] = corners_3d[1,:] + center[1];
corners_3d[2,:] = corners_3d[2,:] + center[2];
corners_3d = np.transpose(corners_3d)
return corners_3d
box_size = [1.734255, 0.519638, 1.711274] #l,w,h
heading_angle = -1.549531
center = [4.509659, 1.092125, 46.151784]
box3d_from_label = get_3d_box(box_size, heading_angle, center)
input = np.load("./test.npy")
fig_lidar = draw_lidar(input)
fig = draw_gt_boxes3d([box3d_from_label], fig_lidar)
mlab.show()