def __init__(self):
gl.GLViewWidget.__init__(self)
#w.setBackgroundColor((135,206,235)) # skyblue from gnuplot
self.setBackgroundColor((255,255,255)) # white, good for report
#w.setBackgroundColor((255,255,255)) # black, also useful?
Nx,Ny,Nz = 16,16,16
Lx,Ly,Lz = 16,16,16
self.data = 2*np.random.rand(Nx,Ny,Nz) - 1
self.Nx,self.Ny,self.Nz,self.Lx,self.Ly,self.Lz = Nx,Ny,Nz,Lx,Ly,Lz
self.transparency = 1.
self.sliceHeight = Nz
self.powerParameter = 1.0
# Add explicit bounding box
boundingBox = gl.GLBoxItem()
boundingBox.scale(Lx, Ly, Lz)
boundingBox.setColor([0,0,0]) # Paint it black
self.addItem(boundingBox)
self.boundingBox = boundingBox
# Add a GL volume item, the main attraction
# It works by drawing a configurable number of planes per small volume
# Uses lot's of interpolation.
volItem = gl.GLVolumeItem([])
volItem.scale(Lx/Nx, Ly/Ny, Lz/Nz)
self.addItem(volItem)
self.volItem = volItem # store a reference
self.updateVolumeData()
def draw(self,
pw,
stepOffset,
drawBounds=True,
drawSlices=True,
drawInner=False,
drawZ=False):
if self.size == 0:
return
zStart = self.corners[4] - stepOffset
zEnd = self.corners[5] - stepOffset
if self.depth == 0:
xSize = self.corners[1] - self.corners[0]
ySize = self.corners[3] - self.corners[2]
zSize = self.corners[5] - self.corners[4]
# Draw bounding box
if drawBounds:
b = gl.GLBoxItem(color=(255, 255, 255, 255))
b.setSize(xSize, ySize, zSize)
pw.addItem(b)
b.translate(self.corners[0], self.corners[2], zStart)
if drawSlices:
for i in range(zSize):
box = gl.GLBoxItem(color=(200, 200, 200, 255))
box.setSize(xSize, ySize, 0)
box.translate(self.corners[0], self.corners[2], i)
pw.addItem(box)
if self.children != None:
if drawInner:
if drawZ:
self.drawInner(pw, stepOffset)
else:
self.drawInnerMarks(pw, stepOffset)
for child in self.children:
child.draw(pw, stepOffset, drawBounds, drawSlices, drawInner,
drawZ)
def update_lidar3d(self, label_idx=None):
path = self.get_path('lidar3d')
timestamp = self.get_timestamp_from_data_name(topic='lidar')
if timestamp is not None:
time_diff = get_time_difference(self.stereo_timestamp, timestamp)
self.timeLidar3dEdit.setText('{} ms'.format(time_diff))
else:
self.timeLidar3dEdit.setText('No timestamp found!')
pc = np.fromfile(path, dtype=np.float32)
try:
pc = pc.reshape((-1, 5))
except Exception:
pc = pc.reshape((-1, 4))
norm = mpl.colors.Normalize(vmin=3, vmax=80)
cmap = cm.jet
m = cm.ScalarMappable(norm=norm, cmap=cmap)
if label_idx is None:
label_idx = slice(len(self.labels_rgb))
colors = m.to_rgba(np.linalg.norm(pc[:, 0:3], axis=1))
colors[:, [2, 1, 0, 3]] = colors[:, [0, 1, 2, 3]]
colors[:, 3] = 0.5
try:
self.w.removeItem(self.plot)
for box in self.boxes:
self.w.removeItem(box)
except Exception:
pass
self.boxes = []
size = QtGui.QVector3D(1, 1, 1)
for objects in self.labels_rgb[label_idx]:
box = gl.GLBoxItem(size, color=(255, 255, 255, 255))
box.setSize(objects['length'], objects['width'], objects['height'])
box.translate(-objects['length']/2, -objects['width']/2, -objects['height']/2)
box.rotate(angle=-objects['rotz'] * 180 / 3.14159265359, x=0, y=0, z=1)
box.rotate(angle=-objects['roty'] * 180 / 3.14159265359, x=0, y=1, z=0)
box.rotate(angle=-objects['rotx'] * 180 / 3.14159265359, x=1, y=0, z=0)
box.translate(0, 0, objects['height']/2)
box.translate(objects['posx_lidar'], objects['posy_lidar'], objects['posz_lidar'])
self.boxes.append(box)
self.plot = gl.GLScatterPlotItem(pos=np.asarray(pc[:, 0:3]), size=5.5, color=colors)
self.w.addItem(self.plot)
for box in self.boxes:
self.w.addItem(box)
self.lidar3dGridLayout.addWidget(self.w)
def setupGL(self):
shape = self.relativeInputShape
w = self.glw
size = QtGui.QVector3D(-1*shape[0],1*shape[1],0)
self.xy = gl.GLBoxItem(size=size,color=(0,0,255))
w.addItem(self.xy)
size = QtGui.QVector3D(-1*shape[0],0,-1*shape[2])
self.xz = gl.GLBoxItem(size=size,color=(0,255,0))
w.addItem(self.xz)
size = QtGui.QVector3D(0,1*shape[1],-1*shape[2])
self.yz = gl.GLBoxItem(size=size,color=(255,0,0))
w.addItem(self.yz)
self.planes[0] = self.yz
self.planes[1] = self.xz
self.planes[2] = self.xy
# self.xy.setSize
size = QtGui.QVector3D(-1*shape[0],-1*shape[1], 0)
self.sections[0] = gl.GLBoxItem(size=size,color=(255,0,0))
size = QtGui.QVector3D(-1*shape[0],0,-1*shape[2])
self.sections[1] = gl.GLBoxItem(size=size,color=(0,255,0))
size = QtGui.QVector3D(0,-1*shape[1],-1*shape[2])
self.sections[2] = gl.GLBoxItem(size=size,color=(0,0,255))
w.addItem(self.sections[0])
w.addItem(self.sections[1])
w.addItem(self.sections[2])
def drawInnerFrame(self, view, stepOffset):
zStart = self.corners[4] - stepOffset
zEnd = self.corners[5] - stepOffset
xSize = self.corners[1] - self.corners[0]
ySize = self.corners[3] - self.corners[2]
zVals = (zStart, zEnd)
for z in zVals:
box = gl.GLBoxItem()
box.setSize(xSize, ySize, 0)
box.translate(0, 0, z)
view.addItem(box)
def add_extruder(self, widget, x_pos, y_pos, z_pos):
"""Add the extruder object to a OpenGL Widget
Arguments:
widget {int} -- Widget to draw extruder on
x_pos {float} -- Extruder x position
y_pos {float} -- Extruder y position
z_pos {float} -- Extruder z position
"""
extruder_gl = gl.GLBoxItem(size=QtGui.QVector3D(0.5, 0.5, 8),
color=(55, 155, 55, 255))
extruder_gl.translate(x_pos, y_pos, z_pos)
self.extruders.append(extruder_gl)
self.widgets[widget].addItem(self.extruders[-1])
def add_new_box(self,
pos=False,
color=np.array((255, 255, 255)),
size=0.25):
if pos is False:
return
if pos.shape[0] == 3:
new_box_item = gl.GLBoxItem()
new_box_item.setSize(x=size, y=size, z=size)
new_box_item.translate(pos[0], pos[1], pos[2])
self.box_item_list.append(new_box_item)
self.widget.addItem(new_box_item)
else:
for new in pos:
self.add_new_box(pos=new, color=color, size=size)
def add_new_box(self, pos, color=255):
if pos is False:
return
if pos.shape[0] == 3:
if self.world[pos[0] + self.world.shape[0] // 2,
pos[1] + self.world.shape[1] // 2,
pos[2] + self.world.shape[2] // 2] == 0:
new_box_item = gl.GLBoxItem()
new_box_item.setSize(x=self.sampling,
y=self.sampling,
z=self.sampling)
new_box_item.translate(pos[0] - (0.5 * self.sampling),
pos[1] - (0.5 * self.sampling),
pos[2] - (0.5 * self.sampling))
new_box_item.setColor((255, 255, 255, color))
self.box_item[pos[0] + pos[1] + pos[2]] = new_box_item
self.widget.addItem(new_box_item)
def add_box(self, position, colour, widget):
"""Create a brick on the display
Arguments:
position {tuple} -- XYZ Tuple position of the brick
colour {tuple} -- RGBW of the brick
widget {int} -- Widget to draw brick to
"""
box = gl.GLMeshItem(vertexes=self.root.premade_widgets["block_verts"],
faces=self.root.premade_widgets["block_faces"],
color=colour,
glOptions='opaque',
smooth=False)
box.translate(position[0], position[1], position[2])
frame = gl.GLBoxItem(color=(0, 0, 0, 255))
frame.translate(position[0], position[1], position[2])
self.root.widgets[widget].addItem(box)
self.root.widgets[widget].addItem(frame)
"""
Very basic 3D graphics example; create a view widget and add a few items.
"""
import pyqtgraph as pg
import pyqtgraph.opengl as gl
pg.mkQApp("GLViewWidget Example")
w = gl.GLViewWidget()
w.show()
w.setWindowTitle('pyqtgraph example: GLViewWidget')
w.setCameraPosition(distance=20)
ax = gl.GLAxisItem()
ax.setSize(5,5,5)
w.addItem(ax)
b = gl.GLBoxItem()
w.addItem(b)
ax2 = gl.GLAxisItem()
ax2.setParentItem(b)
b.translate(1,1,1)
if __name__ == '__main__':
pg.exec()
def brick(x, y, z, blocktype, screen):
global extruder
"""Add a brick to the scene
Arguments:
x {int} -- Block x from home
y {int} -- Block y from home
z {int} -- Block z from home
blocktype {int} -- Block colour
0 -- movement
1 -- red
2 -- blue
3 -- yellow
screen {int} -- Screen
0 -- both
1 -- preview
2 -- animated
"""
verts_box = np.array([
[0, 0, 0],
[1, 0, 0],
[1, 1, 0],
[0, 1, 0],
[0, 0, 1],
[1, 0, 1],
[1, 1, 1],
[0, 1, 1]],
dtype=float)
faces_box = np.array([
[0, 1, 2],
[0, 2, 3],
[0, 1, 4],
[1, 5, 4],
[1, 2, 5],
[2, 5, 6],
[2, 3, 6],
[3, 6, 7],
[0, 3, 7],
[0, 4, 7],
[4, 5, 7],
[5, 6, 7],
])
color_list = ()
no_skip_block = True
if blocktype == 0:
no_skip_block = False
elif blocktype == 1:
color_list = (1, 0, 0, 0.8)
elif blocktype == 2:
color_list = (0, 0, 1, 0.8)
elif blocktype == 3:
color_list = (1, 1, 0, 0.8)
if no_skip_block:
box = gl.GLMeshItem(vertexes=verts_box, faces=faces_box,
color=color_list, glOptions='translucent', smooth=True)
frame = gl.GLBoxItem(color=(0, 0, 0, 255))
box.translate(x, y, z)
frame.translate(x, y, z)
if screen == 0:
if no_skip_block:
animated_preview_widget.addItem(box)
preview_widget.addItem(box)
animated_preview_widget.addItem(frame)
preview_widget.addItem(frame)
if extruder in animated_preview_widget.items:
animated_preview_widget.removeItem(extruder)
extruder = gl.GLBoxItem(size=QtGui.QVector3D(
0.5, 0.5, 8), color=(55, 155, 55, 255))
extruder.translate(x+0.25, y+0.25, z+1)
animated_preview_widget.addItem(extruder)
elif screen == 1:
if no_skip_block:
preview_widget.addItem(box)
preview_widget.addItem(frame)
else:
pass
elif screen == 2:
if no_skip_block:
animated_preview_widget.addItem(box)
animated_preview_widget.addItem(frame)
if extruder in animated_preview_widget.items:
animated_preview_widget.removeItem(extruder)
extruder = gl.GLBoxItem(size=QtGui.QVector3D(
0.5, 0.5, 8), color=(55, 155, 55, 255))
extruder.translate(x+0.25, y+0.25, z+1)
animated_preview_widget.addItem(extruder)
pos=x_verts_line, color=axis_color_line, width=2, antialias=True)
x_line.translate(0.5, 0.5, 0)
y_line = gl.GLLinePlotItem(
pos=y_verts_line, color=axis_color_line, width=2, antialias=True)
y_line.translate(0.5, 0.5, 0)
z_line = gl.GLLinePlotItem(
pos=z_verts_line, color=axis_color_line, width=2, antialias=True)
z_line.translate(0.5, 0.5, 0)
animated_preview_widget.addItem(x_line)
animated_preview_widget.addItem(y_line)
animated_preview_widget.addItem(z_line)
preview_widget.addItem(x_line)
preview_widget.addItem(y_line)
preview_widget.addItem(z_line)
extruder = gl.GLBoxItem(size=QtGui.QVector3D(
0.5, 0.5, 8), color=(55, 155, 55, 255))
extruder.translate(0.25, 0.25, 0)
animated_preview_widget.addItem(extruder)
def brick(x, y, z, blocktype, screen):
global extruder
"""Add a brick to the scene
Arguments:
x {int} -- Block x from home
y {int} -- Block y from home
z {int} -- Block z from home
blocktype {int} -- Block colour
0 -- movement
1 -- red
2 -- blue
## Add path to library (just for examples; you do not need this)
import pyqtgraph.opengl as gl
from pyqtgraph.Qt import QtCore, QtGui
app = QtGui.QApplication([])
w = gl.GLViewWidget()
w.opts['distance'] = 10
w.show()
w.setWindowTitle('pyqtgraph example: GLViewWidget')
ax = gl.GLAxisItem()
ax.setSize(5, 5, 5)
w.addItem(ax)
b = gl.GLBoxItem(color=(255, 255, 255, 80), glOptions='translucent')
b.paint()
w.addItem(b)
ax2 = gl.GLAxisItem()
ax2.setParentItem(b)
b.translate(1, 1, 1)
## Start Qt event loop unless running in interactive mode.
if __name__ == '__main__':
import sys
if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'):
QtGui.QApplication.instance().exec_()
interval = 30
win = pg.GraphicsWindow()
win.setWindowTitle("DataScope")
p1 = win.addPlot()
p1.showGrid(True, True)
# p2=win.addPlot(row=1,col=0)
# p2.showGrid(True,True)
data = np.array([[0.0] * 400] * 3)
curve1 = p1.plot(pen='r')
curve2 = p1.plot(pen='g')
#curve3=p1.plot(pen='b')
boxwin = gl.GLViewWidget()
box = gl.GLBoxItem()
box.scale(0.5, 0.8, 1)
tr = box.transform()
boxwin.addItem(box)
ax = gl.GLAxisItem()
boxwin.addItem(ax)
boxwin.show()
pos = 0
buff = b''
A = np.mat([[1, 0.005], [0, 1]])
Q = np.mat([[0.001, 0.0], [0., 0.001]])
R = np.mat([[0.8514, 0], [0, 0.01281]])
