def showPointCloud(pointcloud):
if len(pointcloud) == 0:
raise Exception("point cloud could not be empty!")
pango.CreateWindowAndBind("pointcloud viewer", 1024, 768)
glEnable(GL_DEPTH_TEST)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
pm = pango.ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000)
mv = pango.ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
s_cam = pango.OpenGlRenderState(pm, mv)
handler = pango.Handler3D(s_cam)
d_cam = pango.CreateDisplay()
d_cam.SetBounds(pango.Attach(0.0), pango.Attach(1.0), pango.Attach(0.0),
pango.Attach(1.0), -1024 / 768)
d_cam.SetHandler(handler)
while not pango.ShouldQuit():
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
d_cam.Activate(s_cam)
glClearColor(1.0, 1.0, 1.0, 1.0)
glPointSize(2)
glBegin(GL_POINTS)
for p in pointcloud:
glColor3d(p[3], p[3], p[3])
glVertex3d(p[0], p[1], p[2])
glEnd()
pango.FinishFrame()
time.sleep(0.005)
def main():
win = pango.CreateWindowAndBind("pySimpleDisplay", 640, 480)
glEnable(GL_DEPTH_TEST)
pm = pango.ProjectionMatrix(640,480,420,420,320,240,0.1,1000);
mv = pango.ModelViewLookAt(-0, 0.5, -3, 0, 0, 0, pango.AxisY)
s_cam = pango.OpenGlRenderState(pm, mv)
ui_width = 180
handler=pango.Handler3D(s_cam)
d_cam = pango.CreateDisplay().SetBounds(pango.Attach(0),
pango.Attach(1),
pango.Attach.Pix(ui_width),
pango.Attach(1),
-640.0/480.0).SetHandler(handler)
pango.CreatePanel("ui").SetBounds( pango.Attach(0),
pango.Attach(1),
pango.Attach(0),
pango.Attach.Pix(ui_width))
var_ui=pango.Var("ui")
var_ui.A_Button=False
var_ui.B_Button=True
var_ui.B_Double=1
var_ui.B_Str="sss"
ctrl=-96
pango.RegisterKeyPressCallback(ctrl+ord('a'), a_callback)
while not pango.ShouldQuit():
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
d_cam.Activate(s_cam)
pango.glDrawColouredCube()
pango.FinishFrame()
def main():
pangolin.CreateWindowAndBind('Main', 640, 480)
gl.glEnable(gl.GL_DEPTH_TEST)
# Define Projection and initial ModelView matrix
scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2, 100),
pangolin.ModelViewLookAt(-2, 2, -2, 0, 0, 0, pangolin.AxisY))
tree = pangolin.Renderable()
tree.Add(pangolin.Axis())
# Create Interactive View in window
handler = pangolin.SceneHandler(tree, scam)
dcam = pangolin.CreateDisplay()
dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0)
dcam.SetHandler(handler)
def draw(view):
view.Activate(scam)
tree.Render()
dcam.SetDrawFunction(draw)
while not pangolin.ShouldQuit():
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
# or
# dcam.Activate(scam)
# tree.Render()
pangolin.FinishFrame()
def main():
win = pango.CreateWindowAndBind("main py_pangolin", 640, 480)
log = pango.DataLog()
log.SetLabels(["sin(t)", "cos(t)", "sin(t)+cos(t)"])
t=0;
tinc=0.01
plotter = pango.Plotter(log,0,4*math.pi/tinc,-2,2,math.pi/(4*tinc),0.5);
plotter.Track("$i")
plotter.AddMarker(pango.Marker.Vertical, -1000, pango.Marker.LessThan, pango.Colour.Blue().WithAlpha(0.2))
plotter.AddMarker(pango.Marker.Horizontal, 100, pango.Marker.GreaterThan, pango.Colour.Red().WithAlpha(0.2))
plotter.AddMarker(pango.Marker.Horizontal, 10, pango.Marker.Equal, pango.Colour.Green().WithAlpha(0.2))
plotter.SetBounds(pango.Attach(0), pango.Attach(1),
pango.Attach(0), pango.Attach(1))
pango.DisplayBase().AddDisplay(plotter)
while not pango.ShouldQuit():
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
log.Log(math.sin(t), math.cos(t), math.sin(t)+math.cos(t))
t+=tinc
pango.FinishFrame()
def main():
# Create OpenGL window in single line
pangolin.CreateWindowAndBind('Main', 640, 480)
gl.glEnable(gl.GL_DEPTH_TEST)
# Define Projection and initial ModelView matrix
scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2, 100),
pangolin.ModelViewLookAt(-2, 2, -2, 0, 0, 0,
pangolin.AxisDirection.AxisY))
handler = pangolin.Handler3D(scam)
# Create Interactive View in window
dcam = pangolin.CreateDisplay()
dcam.SetBounds(pangolin.Attach(0.0), pangolin.Attach(1.0),
pangolin.Attach(0.0), pangolin.Attach(1.0), -640.0 / 480.0)
dcam.SetHandler(handler)
# Data logger object
log = pangolin.DataLog()
# Optionally add named labels
labels = ['sin(t)', 'cos(t)', 'sin(t)+cos(t)']
log.SetLabels(labels)
# OpenGL 'view' of data. We might have many views of the same data.
tinc = 0.03
plotter = pangolin.Plotter(log, 0.0, 6.0 * np.pi / tinc, -2.0, 2.0,
np.pi / (6 * tinc), 0.5)
plotter.SetBounds(pangolin.Attach(0.05), pangolin.Attach(0.3),
pangolin.Attach(0.0), pangolin.Attach(0.4))
plotter.Track('$i')
# Add some sample annotations to the plot
plotter.AddMarker(pangolin.Marker.Vertical, -1000,
pangolin.Marker.LessThan,
pangolin.Colour.Blue().WithAlpha(0.2))
plotter.AddMarker(pangolin.Marker.Horizontal, 100,
pangolin.Marker.GreaterThan,
pangolin.Colour.Red().WithAlpha(0.2))
plotter.AddMarker(pangolin.Marker.Horizontal, 10, pangolin.Marker.Equal,
pangolin.Colour.Green().WithAlpha(0.2))
pangolin.DisplayBase().AddDisplay(plotter)
t = 0
while not pangolin.ShouldQuit():
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
# Plot line
log.Log(np.sin(t), np.cos(t), np.sin(t) + np.cos(t))
t += tinc
gl.glClearColor(1.0, 1.0, 1.0, 1.0)
dcam.Activate(scam)
# Render OpenGL 3D Cube
pangolin.glDrawColouredCube()
pangolin.FinishFrame()
def DrawTrajectory(poses):
pango.CreateWindowAndBind("trajectory viewer", 1024, 768)
glEnable(GL_DEPTH_TEST)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
pm = pango.ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000)
mv = pango.ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
s_cam = pango.OpenGlRenderState(pm, mv)
handler = pango.Handler3D(s_cam)
d_cam = pango.CreateDisplay()
d_cam.SetBounds(pango.Attach(0.0), pango.Attach(1.0), pango.Attach(0.0),
pango.Attach(1.0), -1024 / 768)
d_cam.SetHandler(handler)
poses.append(poses[0])
while not pango.ShouldQuit():
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
d_cam.Activate(s_cam)
glClearColor(1.0, 1.0, 1.0, 1.0)
glLineWidth(2)
for i, pose in enumerate(poses[:-1]):
_, t = mat.T2Rt(pose)
Ow = t
Xw = mat.transform_homogeneous(
pose, list(map(lambda x: x * 0.1, [1, 0, 0])))
Yw = mat.transform_homogeneous(
pose, list(map(lambda x: x * 0.1, [0, 1, 0])))
Zw = mat.transform_homogeneous(
pose, list(map(lambda x: x * 0.1, [0, 0, 1])))
glBegin(GL_LINES)
glColor3f(1.0, 0.0, 0.0)
glVertex3d(Ow[0], Ow[1], Ow[2])
glVertex3d(Xw[0], Xw[1], Xw[2])
glColor3f(0.0, 1.0, 0.0)
glVertex3d(Ow[0], Ow[1], Ow[2])
glVertex3d(Yw[0], Yw[1], Yw[2])
glColor3f(0.0, 0.0, 1.0)
glVertex3d(Ow[0], Ow[1], Ow[2])
glVertex3d(Zw[0], Zw[1], Zw[2])
p1 = poses[i]
p2 = poses[i + 1]
_, t1 = mat.T2Rt(p1)
_, t2 = mat.T2Rt(p2)
glColor3f(0.0, 0.0, 0.0)
glVertex3d(t1[0], t1[1], t1[2])
glVertex3d(t2[0], t2[1], t2[2])
glEnd()
pango.FinishFrame()
time.sleep(0.005)
def __init__(self,worker_dir,map_img_path,map_yaml_path,waypoints,show_laser=False):
with open(map_yaml_path,'r')as yaml_stream:
try:
map_metadata=yaml.safe_load(yaml_stream)
map_resolution=map_metadata['resolution']
origin=map_metadata['origin']
map_origin_x=origin[0]
map_origin_y=origin[1]
except yaml.YAMLError as ex:
print(ex)
with open(worker_dir+'config.yaml','r')as yaml_stream:
try:
config=yaml.safe_load(yaml_stream)
speed_lut_name=config['speed_lut_name']
zoom=config['zoom']
except yaml.YAMLError as ex:
print(ex)
self.speed_lut=msgpack.unpack(open(worker_dir+speed_lut_name,'rb'),use_list=False)
self.waypoints=waypoints
self.waypoints_plot=np.copy(waypoints[:,0:3])
self.waypoints_plot[:,2]*=0.
self.show_laser=show_laser
self.map_img=np.array(Image.open(map_img_path).transpose(Image.FLIP_TOP_BOTTOM)).astype(np.float64)
if len(self.map_img.shape)>2:
print('map image not grayscale')
self.map_img=np.dot(self.map_img[...,:3],[0.29,0.57,0.14])
self.map_img=np.floor(self.map_img)
map_height=self.map_img.shape[0]
map_width=self.map_img.shape[1]
range_x=np.arange(self.map_img.shape[1])
range_y=np.arange(self.map_img.shape[0])
map_x,map_y=np.meshgrid(range_x,range_y)
map_x=(map_x*map_resolution+map_origin_x).flatten()
map_y=(map_y*map_resolution+map_origin_y).flatten()
map_z=np.zeros(map_y.shape)
map_coords=np.vstack((map_x,map_y,map_z))
map_mask=self.map_img==0.0
map_mask_flat=map_mask.flatten()
self.map_points=map_coords[:,map_mask_flat].T
pangolin.CreateWindowAndBind('sim',930,1080)
gl.glEnable(gl.GL_DEPTH_TEST)
self.scam=pangolin.OpenGlRenderState(pangolin.ProjectionMatrix(640,480,120,120,320,280,0.2,200),pangolin.ModelViewLookAt(-0.1,0,zoom,0,0,0,pangolin.AxisDirection.AxisZ))
self.handler=pangolin.Handler3D(self.scam)
self.dcam=pangolin.CreateDisplay()
self.dcam.SetBounds(pangolin.Attach(0.0),pangolin.Attach(1.0),pangolin.Attach(0.0),pangolin.Attach(1.0),-1920.0/1080)
self.dcam.SetHandler(self.handler)
angle_min=-4.7/2
angle_max=4.7/2
num_beams=1080
self.scan_angles=np.linspace(angle_min,angle_max,num_beams)
def main():
# Create OpenGL window in single line
pangolin.CreateWindowAndBind('Main', 640, 480)
# 3D Mouse handler requires depth testing to be enabled
gl.glEnable(gl.GL_DEPTH_TEST)
scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.1, 1000),
pangolin.ModelViewLookAt(-1, 1, -1, 0, 0, 0,
pangolin.AxisDirection.AxisY))
# Aspect ratio allows us to constrain width and height whilst fitting within specified
# bounds. A positive aspect ratio makes a view 'shrink to fit' (introducing empty bars),
# whilst a negative ratio makes the view 'grow to fit' (cropping the view).
dcam = pangolin.CreateDisplay()
dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0)
dcam.SetHandler(pangolin.Handler3D(scam))
# This view will take up no more than a third of the windows width or height, and it
# will have a fixed aspect ratio to match the image that it will display. When fitting
# within the specified bounds, push to the top-left (as specified by SetLock).
dimg = pangolin.Display('image')
dimg.SetBounds(2. / 3, 1.0, 0.0, 1. / 3, 640. / 480)
dimg.SetLock(pangolin.Lock.LockLeft, pangolin.Lock.LockTop)
w, h = 64, 48
texture = pangolin.GlTexture(w, h, gl.GL_RGB, False, 0, gl.GL_RGB,
gl.GL_UNSIGNED_BYTE)
# Default hooks for exiting (Esc) and fullscreen (tab).
while not pangolin.ShouldQuit():
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.95, 0.95, 0.95, 1.0)
dcam.Activate(scam)
gl.glColor3f(1.0, 1.0, 1.0)
pangolin.glDrawColouredCube()
# Set some random image data and upload to GPU
image = random_image(w, h)
texture.Upload(image, gl.GL_RGB, gl.GL_UNSIGNED_BYTE)
# display the image
dimg.Activate()
gl.glColor3f(1.0, 1.0, 1.0)
texture.RenderToViewport()
pangolin.FinishFrame()
def main():
win = pango.CreateWindowAndBind("pySimpleDisplay", 640, 480)
glEnable(GL_DEPTH_TEST)
pm = pango.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.1, 1000)
mv = pango.ModelViewLookAt(-0, 0.5, -3, 0, 0, 0, pango.AxisY)
s_cam = pango.OpenGlRenderState(pm, mv)
ui_width = 180
handler = pango.Handler3D(s_cam)
d_cam = (
pango.CreateDisplay()
.SetBounds(
pango.Attach(0),
pango.Attach(1),
pango.Attach.Pix(ui_width),
pango.Attach(1),
-640.0 / 480.0,
)
.SetHandler(handler)
)
pango.CreatePanel("ui").SetBounds(
pango.Attach(0), pango.Attach(1), pango.Attach(0), pango.Attach.Pix(ui_width)
)
var_ui = pango.Var("ui")
var_ui.a_Button = False
var_ui.a_double = (0.0, pango.VarMeta(0, 5))
var_ui.an_int = (2, pango.VarMeta(0, 5))
var_ui.a_double_log = (3.0, pango.VarMeta(1, 1e4, logscale=True))
var_ui.a_checkbox = (False, pango.VarMeta(toggle=True))
var_ui.an_int_no_input = 2
var_ui.a_str = "sss"
ctrl = -96
pango.RegisterKeyPressCallback(ctrl + ord("a"), a_callback)
while not pango.ShouldQuit():
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
if var_ui.a_checkbox:
var_ui.an_int = var_ui.a_double
var_ui.an_int_no_input = var_ui.an_int
d_cam.Activate(s_cam)
pango.glDrawColouredCube()
pango.FinishFrame()
def viewer_init(self, w, h):
pangolin.CreateWindowAndBind('Main', w, h)
gl.glEnable(gl.GL_DEPTH_TEST)
self.scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(w, h, 420, 420, w // 2, h // 2, 0.2,
1000),
pangolin.ModelViewLookAt(0, -10, -8, 0, 0, 0, 0, -1, 0))
self.handler = pangolin.Handler3D(self.scam)
# Create Interactive View in window
self.dcam = pangolin.CreateDisplay()
self.dcam.SetBounds(pangolin.Attach(0), pangolin.Attach(1),
pangolin.Attach(0), pangolin.Attach(1), -w / h)
self.dcam.SetHandler(self.handler)
def viewer_init(self):
pangolin.CreateWindowAndBind("main", 640, 480)
gl.glEnable(gl.GL_DEPTH_TEST)
self.scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2, 100),
pangolin.ModelViewLookAt(-2, 2, -2, 0, 0, 0, pangolin.AxisDirection.AxisY))
self.handler = pangolin.Handler3D(self.scam)
self.dcam = pangolin.CreateDisplay()
self.dcam.SetBounds(
pangolin.Attach(0.0), pangolin.Attach(1.0), pangolin.Attach(0.0), pangolin.Attach(1.0), -640.0 / 480.0)
self.dcam.SetHandler(self.handler)
self.dcam.Activate()
'''
def viewer_init(self, w, h):
pango.CreateWindowAndBind('Main', w, h)
gl.glEnable(gl.GL_DEPTH_TEST)
pm = pango.ProjectionMatrix(w, h, 420, 420, w // 2, h // 2, 0.2, 1000)
mv = pango.ModelViewLookAt(0, -25, -50, 0, 0, 0, 0, -1, 0)
self.scam = pango.OpenGlRenderState(pm, mv)
ui_width = 180
self.handler = pango.Handler3D(self.scam)
self.d_cam = pango.CreateDisplay().SetBounds(
pango.Attach(0), pango.Attach(1), pango.Attach.Pix(ui_width),
pango.Attach(1), -w / h).SetHandler(self.handler)
# hack to avoid small Pangolin, no idea why it's *2
self.d_cam.Resize(pango.Viewport(0, 0, w * 2, h * 2))
self.d_cam.Activate(self.scam)
def DrawTrajectory(groundtruth, estimated):
pango.CreateWindowAndBind("trajectory viewer", 1024, 768)
glEnable(GL_DEPTH_TEST)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
pm = pango.ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000)
mv = pango.ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
s_cam = pango.OpenGlRenderState(pm, mv)
handler = pango.Handler3D(s_cam)
d_cam = pango.CreateDisplay()
d_cam.SetBounds(pango.Attach(0.0), pango.Attach(1.0),
pango.Attach(0.0), pango.Attach(1.0),
-1024/768)
d_cam.SetHandler(handler)
while not pango.ShouldQuit():
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT)
d_cam.Activate(s_cam)
glClearColor(1.0, 1.0, 1.0, 1.0)
glLineWidth(2)
for i in range(len(groundtruth)-1):
glColor3f(0.0, 1.0, 0.0)
glBegin(GL_LINES)
SE3_p1 = groundtruth[i]
SE3_p2 = groundtruth[i+1]
glVertex3d(SE3_p1[0], SE3_p1[1], SE3_p1[2])
glVertex3d(SE3_p2[0], SE3_p2[1], SE3_p2[2])
glEnd()
for i in range(len(estimated)-1):
glColor3f(1.0, 0.0, 0.0)
glBegin(GL_LINES)
SE3_p1 = estimated[i]
SE3_p2 = estimated[i + 1]
glVertex3d(SE3_p1[0], SE3_p1[1], SE3_p1[2])
glVertex3d(SE3_p2[0], SE3_p2[1], SE3_p2[2])
glEnd()
pango.FinishFrame()
time.sleep(0.005)
def main():
# Create OpenGL window in single line
pangolin.CreateWindowAndBind('Main', 640, 480)
# Data logger object
log = pangolin.DataLog()
# Optionally add named labels
labels = ['sin(t)', 'cos(t)', 'sin(t)+cos(t)']
log.SetLabels(labels)
# OpenGL 'view' of data. We might have many views of the same data.
tinc = 0.03
plotter = pangolin.Plotter(log, 0.0, 4.0 * np.pi / tinc, -2.0, 2.0,
np.pi / (4 * tinc), 0.5)
plotter.SetBounds(0.0, 1.0, 0.0, 1.0)
plotter.Track('$i')
# Add some sample annotations to the plot
plotter.AddMarker(pangolin.Marker.Vertical, -1000,
pangolin.Marker.LessThan,
pangolin.Colour.Blue().WithAlpha(0.2))
plotter.AddMarker(pangolin.Marker.Horizontal, 100,
pangolin.Marker.GreaterThan,
pangolin.Colour.Red().WithAlpha(0.2))
plotter.AddMarker(pangolin.Marker.Horizontal, 10, pangolin.Marker.Equal,
pangolin.Colour.Green().WithAlpha(0.2))
pangolin.DisplayBase().AddDisplay(plotter)
t = 0
while not pangolin.ShouldQuit():
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
log.Log(np.sin(t), np.cos(t), np.sin(t) + np.cos(t))
t += tinc
pangolin.FinishFrame()
def main():
pangolin.CreateWindowAndBind('Main', 640, 480)
gl.glEnable(gl.GL_DEPTH_TEST)
# Define Projection and initial ModelView matrix
scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2, 100),
pangolin.ModelViewLookAt(-2, 2, -2, 0, 0, 0,
pangolin.AxisDirection.AxisY))
handler = pangolin.Handler3D(scam)
# Create Interactive View in window
dcam = pangolin.CreateDisplay()
dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0)
dcam.SetHandler(handler)
while not pangolin.ShouldQuit():
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(1.0, 1.0, 1.0, 1.0)
dcam.Activate(scam)
# Render OpenGL Cube
pangolin.glDrawColouredCube()
# Draw Point Cloud
points = np.random.random((100000, 3)) * 10
colors = np.zeros((len(points), 3))
colors[:, 1] = 1 - points[:, 0] / 10.
colors[:, 2] = 1 - points[:, 1] / 10.
colors[:, 0] = 1 - points[:, 2] / 10.
gl.glPointSize(2)
gl.glColor3f(1.0, 0.0, 0.0)
# access numpy array directly(without copying data), array should be contiguous.
pangolin.DrawPoints(points, colors)
pangolin.FinishFrame()
def main():
pangolin.ParseVarsFile('app.cfg')
pangolin.CreateWindowAndBind('Main', 640, 480)
gl.glEnable(gl.GL_DEPTH_TEST)
scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.1, 1000),
pangolin.ModelViewLookAt(0, 0.5, -3, 0, 0, 0,
pangolin.AxisDirection.AxisY))
handler3d = pangolin.Handler3D(scam)
dcam = pangolin.CreateDisplay()
dcam.SetBounds(0.0, 1.0, 180 / 640., 1.0, -640.0 / 480.0)
# dcam.SetBounds(pangolin.Attach(0.0), pangolin.Attach(1.0),
# pangolin.Attach.Pix(180), pangolin.Attach(1.0), -640.0/480.0)
dcam.SetHandler(pangolin.Handler3D(scam))
panel = pangolin.CreatePanel('ui')
panel.SetBounds(0.0, 1.0, 0.0, 180 / 640.)
button = pangolin.VarBool('ui.Button', value=False, toggle=False)
checkbox = pangolin.VarBool('ui.Checkbox', value=False, toggle=True)
float_slider = pangolin.VarFloat('ui.Float', value=3, min=0, max=5)
float_log_slider = pangolin.VarFloat('ui.Log_scale var',
value=3,
min=1,
max=1e4,
logscale=True)
int_slider = pangolin.VarInt('ui.Int', value=2, min=0, max=5)
int_slave_slider = pangolin.VarInt('ui.Int_slave', value=2, toggle=False)
save_window = pangolin.VarBool('ui.Save_Window', value=False, toggle=False)
save_cube = pangolin.VarBool('ui.Save_Cube', value=False, toggle=False)
record_cube = pangolin.VarBool('ui.Record_Cube', value=False, toggle=False)
def reset():
#float_slider.SetVal(0.5)
print('You typed ctrl-r or pushed reset')
# Reset = SetVarFunctor(float_slider, 0.5)
# reset = pangolin.VarFunc('ui.Reset', reset)
# pangolin.RegisterKeyPressCallback(int(pangolin.PANGO_CTRL) + ord('r'), reset) # segfault
# pangolin.RegisterKeyPressCallback(int(pangolin.PANGO_CTRL) + ord('b'), pangolin.SetVarFunctorFloat('ui.Float', 4.5)) # segfault
# pangolin.RegisterKeyPressCallback(int(pangolin.PANGO_CTRL) + ord('b'), SetVarFunctor(float_slider, 4.5)) # segfault
while not pangolin.ShouldQuit():
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
if pangolin.Pushed(button):
print('You Pushed a button!')
if checkbox.Get():
int_slider.SetVal(int(float_slider))
int_slave_slider.SetVal(int_slider)
if pangolin.Pushed(save_window):
pangolin.SaveWindowOnRender("window")
if pangolin.Pushed(save_cube):
pangolin.SaveWindowOnRender("cube")
if pangolin.Pushed(record_cube):
pangolin.DisplayBase().RecordOnRender(
"ffmpeg:[fps=50,bps=8388608,unique_filename]//screencap.avi")
dcam.Activate(scam)
gl.glColor3f(1.0, 1.0, 1.0)
pangolin.glDrawColouredCube()
pangolin.FinishFrame()
xyz_neg = data[1][:, 0:3].numpy().astype(ctypes.c_float)
sdf_neg = data[1][:, 3].numpy().astype(ctypes.c_float)
rgb_neg = np.zeros_like(xyz_neg).astype(ctypes.c_float)
xyz_pos = data[0][:, 0:3].numpy().astype(ctypes.c_float)
sdf_pos = data[0][:, 3].numpy().astype(ctypes.c_float)
rgb_pos = np.zeros_like(xyz_pos).astype(ctypes.c_float)
s = 10.
rgb_neg[:,0] = np.minimum(s*sdf_neg / sdf_neg.min(), np.ones_like(sdf_neg))
rgb_neg[:,1] = 1. - rgb_neg[:,0]
rgb_pos[:,2] = np.minimum( s*sdf_pos / sdf_pos.max(), np.ones_like(sdf_pos))
rgb_pos[:,1] = 1. - rgb_pos[:,2]
win = pango.CreateWindowAndBind("Interior Samples | " + npz_filename, 640, 480)
gl.glEnable(gl.GL_DEPTH_TEST)
pm = pango.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.1, 1000)
mv = pango.ModelViewLookAt(-0, 0.5, -3, 0, 0, 0, pango.AxisY)
s_cam = pango.OpenGlRenderState(pm, mv)
handler = pango.Handler3D(s_cam)
d_cam = (
pango.CreateDisplay()
.SetBounds(
pango.Attach(0),
pango.Attach(1),
pango.Attach(0),
pango.Attach(1),
-640.0 / 480.0,
def main():
# Create OpenGL window in single line
pangolin.CreateWindowAndBind('Main', 640, 480)
# 3D Mouse handler requires depth testing to be enabled
gl.glEnable(gl.GL_DEPTH_TEST)
# Issue specific OpenGl we might need
gl.glEnable(gl.GL_BLEND)
gl.glBlendFunc(gl.GL_SRC_ALPHA, gl.GL_ONE_MINUS_SRC_ALPHA)
# Define Camera Render Object (for view / scene browsing)
proj = pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.1, 1000)
scam = pangolin.OpenGlRenderState(
proj, pangolin.ModelViewLookAt(1, 0.5, -2, 0, 0, 0, pangolin.AxisY))
scam2 = pangolin.OpenGlRenderState(
proj, pangolin.ModelViewLookAt(0, 0, -2, 0, 0, 0, pangolin.AxisY))
# Add named OpenGL viewport to window and provide 3D Handler
dcam1 = pangolin.Display('cam1')
dcam1.SetAspect(640 / 480.)
dcam1.SetHandler(pangolin.Handler3D(scam))
dcam2 = pangolin.Display('cam2')
dcam2.SetAspect(640 / 480.)
dcam2.SetHandler(pangolin.Handler3D(scam2))
dcam3 = pangolin.Display('cam3')
dcam3.SetAspect(640 / 480.)
dcam3.SetHandler(pangolin.Handler3D(scam))
dcam4 = pangolin.Display('cam4')
dcam4.SetAspect(640 / 480.)
dcam4.SetHandler(pangolin.Handler3D(scam2))
dimg1 = pangolin.Display('img1')
dimg1.SetAspect(640 / 480.)
dimg2 = pangolin.Display('img2')
dimg2.SetAspect(640 / 480.)
# LayoutEqual is an EXPERIMENTAL feature - it requires that all sub-displays
# share the same aspect ratio, placing them in a raster fasion in the
# viewport so as to maximise display size.
view = pangolin.Display('multi')
view.SetBounds(0.0, 1.0, 0.0, 1.0)
view.SetLayout(pangolin.LayoutEqual)
view.AddDisplay(dcam1)
view.AddDisplay(dimg1)
view.AddDisplay(dcam2)
view.AddDisplay(dimg2)
view.AddDisplay(dcam3)
view.AddDisplay(dcam4)
w, h = 64, 48
image_texture = pangolin.GlTexture(w, h, gl.GL_RGB, False, 0, gl.GL_RGB,
gl.GL_UNSIGNED_BYTE)
# Default hooks for exiting (Esc) and fullscreen (tab)
while not pangolin.ShouldQuit():
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glColor3f(1.0, 1.0, 1.0)
dcam1.Activate(scam)
pangolin.glDrawColouredCube()
dcam2.Activate(scam2)
pangolin.glDrawColouredCube()
dcam3.Activate(scam)
pangolin.glDrawColouredCube()
dcam4.Activate(scam2)
pangolin.glDrawColouredCube()
dimg1.Activate()
gl.glColor4f(1.0, 1.0, 1.0, 1.0)
image_texture.Upload(random_image(w, h), gl.GL_RGB,
gl.GL_UNSIGNED_BYTE)
image_texture.RenderToViewport()
dimg2.Activate()
gl.glColor4f(1.0, 1.0, 1.0, 1.0)
# image_texture.Upload(random_image(w, h), gl.GL_RGB, gl.GL_UNSIGNED_BYTE)
image_texture.RenderToViewport()
pangolin.FinishFrame()
import sys
sys.path.append('../pangolin/build/src')
import OpenGL.GL as gl
import pypangolin as pango
if __name__ == "__main__":
npz_filename = sys.argv[1]
data = deep_sdf.data.read_sdf_samples_into_ram(npz_filename)
xyz_neg = data[1][:, 0:3].numpy().astype(ctypes.c_float)
xyz_pos = data[0][:, 0:3].numpy().astype(ctypes.c_float)
win = pango.CreateWindowAndBind("Samples | " + npz_filename, 800, 600)
gl.glEnable(gl.GL_DEPTH_TEST)
pm = pango.ProjectionMatrix(800, 600, 420, 420, 400, 300, 0.1, 1000)
mv = pango.ModelViewLookAt(-0, 0.5, -3, 0, 0, 0, pango.AxisY)
s_cam = pango.OpenGlRenderState(pm, mv)
handler = pango.Handler3D(s_cam)
d_cam = (pango.CreateDisplay().SetBounds(
pango.Attach(0),
pango.Attach(1),
pango.Attach(0),
pango.Attach(1),
-800.0 / 600.0,
).SetHandler(handler))
def viewer_init(self, w, h):
# pangolin.ParseVarsFile('app.cfg')
pangolin.CreateWindowAndBind('Map Viewer', w, h)
gl.glEnable(gl.GL_DEPTH_TEST)
viewpoint_x = 0
viewpoint_y = -40
viewpoint_z = -80
viewpoint_f = 1000
self.proj = pangolin.ProjectionMatrix(w, h, viewpoint_f, viewpoint_f,
w // 2, h // 2, 0.1, 5000)
self.look_view = pangolin.ModelViewLookAt(viewpoint_x, viewpoint_y,
viewpoint_z, 0, 0, 0, 0, -1,
0)
self.scam = pangolin.OpenGlRenderState(self.proj, self.look_view)
self.handler = pangolin.Handler3D(self.scam)
# Create Interactive View in window
self.dcam = pangolin.CreateDisplay()
self.dcam.SetBounds(0.0, 1.0, kUiWidth / w, 1.0, -w / h)
self.dcam.SetHandler(pangolin.Handler3D(self.scam))
self.panel = pangolin.CreatePanel('ui')
self.panel.SetBounds(0.0, 1.0, 0.0, kUiWidth / w)
self.do_follow = True
self.is_following = True
self.draw_cameras = True
self.draw_covisibility = True
self.draw_spanning_tree = True
self.draw_loops = True
#self.button = pangolin.VarBool('ui.Button', value=False, toggle=False)
self.checkboxFollow = pangolin.VarBool('ui.Follow',
value=True,
toggle=True)
self.checkboxCams = pangolin.VarBool('ui.Draw Cameras',
value=True,
toggle=True)
self.checkboxCovisibility = pangolin.VarBool('ui.Draw Covisibility',
value=True,
toggle=True)
self.checkboxSpanningTree = pangolin.VarBool('ui.Draw Tree',
value=True,
toggle=True)
self.checkboxGrid = pangolin.VarBool('ui.Grid',
value=True,
toggle=True)
self.checkboxPause = pangolin.VarBool('ui.Pause',
value=False,
toggle=True)
#self.float_slider = pangolin.VarFloat('ui.Float', value=3, min=0, max=5)
#self.float_log_slider = pangolin.VarFloat('ui.Log_scale var', value=3, min=1, max=1e4, logscale=True)
self.int_slider = pangolin.VarInt('ui.Point Size',
value=kDefaultPointSize,
min=1,
max=10)
self.pointSize = self.int_slider.Get()
self.Twc = pangolin.OpenGlMatrix()
self.Twc.SetIdentity()
def view(self):
pangolin.CreateWindowAndBind('Viewer', 1024, 768)
gl.glEnable(gl.GL_DEPTH_TEST)
gl.glEnable(gl.GL_BLEND)
gl.glBlendFunc(gl.GL_SRC_ALPHA, gl.GL_ONE_MINUS_SRC_ALPHA)
viewpoint_x = 0
viewpoint_y = -7
viewpoint_z = -18
viewpoint_f = 1000
proj = pangolin.ProjectionMatrix(1024, 768, viewpoint_f, viewpoint_f,
512, 389, 0.1, 300)
look_view = pangolin.ModelViewLookAt(viewpoint_x, viewpoint_y,
viewpoint_z, 0, 0, 0, 0, -1, 0)
# Camera Render Object (for view / scene browsing)
scam = pangolin.OpenGlRenderState(proj, look_view)
# Add named OpenGL viewport to window and provide 3D Handler
dcam = pangolin.CreateDisplay()
# dcam.SetBounds(0, 1, 0, 1, 1024 / 768.)
dcam.SetHandler(pangolin.Handler3D(scam))
# image
width, height = 376, 240
dimg = pangolin.Display('image')
# dimg.SetBounds(0, height / 768., 0.0, width / 1024., 1024 / 768.)
dimg.SetLock(pangolin.Lock.LockLeft, pangolin.Lock.LockTop)
texture = pangolin.GlTexture(width, height, gl.GL_RGB, False, 0,
gl.GL_RGB, gl.GL_UNSIGNED_BYTE)
image = np.ones((height, width, 3), 'uint8')
# axis
axis = pangolin.Renderable()
axis.Add(pangolin.Axis())
trajectory = DynamicArray()
camera = None
image = None
while not pangolin.ShouldQuit():
if not self.pose_queue.empty():
while not self.pose_queue.empty():
pose = self.pose_queue.get()
trajectory.append(pose[:3, 3])
camera = pose
if not self.image_queue.empty():
while not self.image_queue.empty():
img = self.image_queue.get()
img = img[::-1, :, ::-1]
img = cv2.resize(img, (width, height))
image = img.copy()
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(1.0, 1.0, 1.0, 1.0)
dcam.Activate(scam)
# draw axis
axis.Render()
# draw current camera
if camera is not None:
gl.glLineWidth(1)
gl.glColor3f(0.0, 0.0, 1.0)
pangolin.DrawCameras(np.array([camera]), 0.5)
# show trajectory
if len(trajectory) > 0:
gl.glPointSize(2)
gl.glColor3f(0.0, 0.0, 0.0)
pangolin.DrawPoints(trajectory.array())
# show image
if image is not None:
texture.Upload(image, gl.GL_RGB, gl.GL_UNSIGNED_BYTE)
dimg.Activate()
gl.glColor3f(1.0, 1.0, 1.0)
texture.RenderToViewport()
pangolin.FinishFrame()
def main():
pangolin.CreateWindowAndBind('Main', 640, 480)
gl.glEnable(gl.GL_DEPTH_TEST)
# Define Projection and initial ModelView matrix
scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2, 200),
pangolin.ModelViewLookAt(-2, 2, -2, 0, 0, 0, pangolin.AxisDirection.AxisY))
handler = pangolin.Handler3D(scam)
# Create Interactive View in window
dcam = pangolin.CreateDisplay()
dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0/480.0)
dcam.SetHandler(handler)
trajectory = [[0, -6, 6]]
for i in range(300):
trajectory.append(trajectory[-1] + np.random.random(3)-0.5)
trajectory = np.array(trajectory)
print(trajectory.shape)
while not pangolin.ShouldQuit():
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(1.0, 1.0, 1.0, 1.0)
dcam.Activate(scam)
drawPlane()
# Render OpenGL Cube
pangolin.glDrawColouredCube(0.1)
# Draw Point Cloud
points = np.random.random((10000, 3)) * 3 - 4
gl.glPointSize(1)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(points)
# Draw Point Cloud
points = np.random.random((10000, 3))
colors = np.zeros((len(points), 3))
colors[:, 1] = 1 -points[:, 0]
colors[:, 2] = 1 - points[:, 1]
colors[:, 0] = 1 - points[:, 2]
points = points * 3 + 1
gl.glPointSize(1)
pangolin.DrawPoints(points, colors)
# Draw lines
gl.glLineWidth(1)
gl.glColor3f(0.0, 0.0, 0.0)
pangolin.DrawLine(trajectory) # consecutive
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawLines(
trajectory,
trajectory + np.random.randn(len(trajectory), 3),
point_size=5) # separate
# Draw camera
pose = np.identity(4)
pose[:3, 3] = np.random.randn(3)
gl.glLineWidth(1)
gl.glColor3f(0.0, 0.0, 1.0)
pangolin.DrawCamera(pose, 0.5, 0.75, 0.8)
# Draw boxes
poses = [np.identity(4) for i in range(10)]
for pose in poses:
pose[:3, 3] = np.random.randn(3) + np.array([5,-3,0])
sizes = np.random.random((len(poses), 3))
gl.glLineWidth(1)
gl.glColor3f(1.0, 0.0, 1.0)
pangolin.DrawBoxes(poses, sizes)
pangolin.FinishFrame()
def __init__(self,
zoom,
map_img_path,
map_yaml_path,
waypoints,
show_laser=False):
# load map params
with open(map_yaml_path, 'r') as yaml_stream:
try:
map_metadata = yaml.safe_load(yaml_stream)
map_resolution = map_metadata['resolution']
origin = map_metadata['origin']
map_origin_x = origin[0]
map_origin_y = origin[1]
except yaml.YAMLError as ex:
print(ex)
self.waypoints = waypoints
self.waypoints_plot = np.copy(waypoints[:, 0:3])
self.waypoints_plot[:, 2] *= 0.
# toggle for laser viz
self.show_laser = show_laser
# load map
self.map_img = np.array(
Image.open(map_img_path).transpose(Image.FLIP_TOP_BOTTOM)).astype(
np.float64)
if len(self.map_img.shape) > 2:
print('map image not grayscale')
self.map_img = np.dot(self.map_img[..., :3], [0.29, 0.57, 0.14])
self.map_img = np.floor(self.map_img)
range_x = np.arange(self.map_img.shape[1])
range_y = np.arange(self.map_img.shape[0])
map_x, map_y = np.meshgrid(range_x, range_y)
map_x = (map_x * map_resolution + map_origin_x).flatten()
map_y = (map_y * map_resolution + map_origin_y).flatten()
map_z = np.zeros(map_y.shape)
map_coords = np.vstack((map_x, map_y, map_z))
map_mask = self.map_img == 0.0
map_mask_flat = map_mask.flatten()
self.map_points = map_coords[:, map_mask_flat].T
# init pangolin
pangolin.CreateWindowAndBind('sim', 930, 1080)
gl.glEnable(gl.GL_DEPTH_TEST)
# define projection and initial modelview matrix
self.scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(640, 480, 120, 120, 320, 280, 0.2, 200),
pangolin.ModelViewLookAt(-0.1, 0, zoom, 0, 0, 0,
pangolin.AxisDirection.AxisZ))
self.handler = pangolin.Handler3D(self.scam)
# create interactive view in window
self.dcam = pangolin.CreateDisplay()
self.dcam.SetBounds(pangolin.Attach(0.0), pangolin.Attach(1.0),
pangolin.Attach(0.0), pangolin.Attach(1.0),
-1920.0 / 1080)
self.dcam.SetHandler(self.handler)
# scan params
angle_min = -4.7 / 2
angle_max = 4.7 / 2
num_beams = 1080
self.scan_angles = np.linspace(angle_min, angle_max, num_beams)
