def viewer_refresh(self, data):
top = None
# get poses
while not data.empty():
top = data.get()
if top is None:
return
self.frames = top[0]
self.points = top[1]
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.0, 0.0, 0.0, 1.0)
self.dcam.Activate(self.scam)
# draw all poses
if self.frames is not None:
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(self.frames[:-1])
# draw world coordinates
if self.points is not None and self.points.shape[0] > 0:
colors = np.zeros((len(self.points), 3))
colors[:, 1] = 1 - self.points[:, 0] / 10.0
colors[:, 2] = 1 - self.points[:, 1] / 10.0
colors[:, 0] = 1 - self.points[:, 2] / 10.0
gl.glPointSize(2)
gl.glColor3f(0.0, 0.0, 1.0)
# # uncomment to draw points
# pangolin.DrawPoints(self.points, colors)
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)
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)
# 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(5)
pangolin.DrawPoints(points, colors)
pangolin.FinishFrame()
def viewer_refresh(self, q):
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(1.0, 1.0, 1.0, 1.0)
self.dcam.Activate(self.scam)
# Draw Camera
gl.glLineWidth(3)
gl.glColor3f(0.0, 0.0, 1.0)
pangolin.DrawCamera(self.cameras[self.cam_id], 2, 0.75, 2)
# Draw previous positions
prev_positions = []
gl.glPointSize(1)
gl.glLineWidth(1)
gl.glColor3f(0.8, 0.8, 0.7)
for i in range(0, self.cam_id + 1):
prev_positions.append(self.cameras[i][:3, 3])
pangolin.DrawPoints(np.vstack(prev_positions))
pangolin.DrawLine(np.vstack(prev_positions))
self.cam_id = (self.cam_id + 1) % len(self.cameras)
# Draw lines
gl.glLineWidth(3)
gl.glPointSize(6)
colors = [(0.7, 0, 0), (0, 0.7, 0), (0.7, 0.7, 0)]
for i in range(3):
gl.glColor3f(*colors[i])
pangolin.DrawPoints(P[i * 4:i * 4 + 4])
for j, k in [(0, 1), (2, 3), (0, 2), (1, 3)]:
pangolin.DrawLine(self.points[[i * 4 + j, i * 4 + k], :])
pangolin.FinishFrame()
def viewer_refresh(self, q):
while not q.empty():
self.data = q.get()
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.0, 0.0, 0.0, 1.0)
self.dcam.Activate(self.scam)
# drawing begins here
# self.pose[2, 3] = self.pose[2, 3] - 0.1
# self.poses.append(np.linalg.inv(self.pose))
if self.data is not None:
gl.glLineWidth(3)
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(self.data[0])
if len(self.data) > 1:
gl.glPointSize(5)
points = self.data[1]
colors = self.data[2]
pangolin.DrawPoints(points, colors)
pangolin.FinishFrame()
def main():
pango.CreateWindowAndBind('point cloud cube render', 640, 480)
# Projection and ModelView Matrices
scam = pango.OpenGlRenderState(
pango.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2, 100),
pango.ModelViewLookAt(-1, -1, -1, 0, 0, 0, pango.AxisDirection.AxisY))
handler = pango.Handler3D(scam)
# Interactive View in Window
disp_cam = pango.CreateDisplay()
disp_cam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0)
disp_cam.SetHandler(handler)
# Create a random point cloud
pts = np.random.random((100, 3)) * 10
# Color matrix based on point location
colors = np.zeros((len(pts), 3))
colors[:, :] = 1. - pts[:, :] / 10
#colors[:] = [1.0, 0.0, 0.0]
while not pango.ShouldQuit():
# Clear screen
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.15, 0.15, 0.15, 0.0)
disp_cam.Activate(scam)
# Draw Points
gl.glPointSize(5)
gl.glColor3f(0.0, 1.0, 0.0)
pango.DrawPoints(pts, colors)
# Finish Drawing
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 viewer_refresh(self, q):
while not q.empty():
self.state = q.get()
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.0, 0.0, 0.0, 1.0)
self.dcam.Activate(self.scam)
if self.state is not None:
if self.state[0].shape[0] >= 2:
# draw poses
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(self.state[0][:-1])
if self.state[0].shape[0] >= 1:
# draw current pose as yellow
gl.glColor3f(1.0, 1.0, 0.0)
pangolin.DrawCameras(self.state[0][-1:])
if self.state[1].shape[0] != 0:
# draw keypoints
gl.glPointSize(5)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(self.state[1], self.state[2])
pangolin.FinishFrame()
def visualize_3D(p3D):
pangolin.CreateWindowAndBind('main', 640, 480)
gl.glEnable(gl.GL_DEPTH_TEST)
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)
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)
#gl.glPointSize(5);
#gl.glColor3f(0.0,0.0,1.0);
#pangolin.DrawPoints(p3D);
# Attempt to draw camera
gl.glLineWidth(2)
gl.glColor3f(1.0, 0.0, 0.0)
#pose = np.identity(4);
pose = (np.hstack((pose1.T, np.zeros((4, 1))))).T
pangolin.DrawCamera(pose, 0.5, 0.75, 0.8)
pangolin.FinishFrame()
def draw(self):
q = self.q
self.draw_init()
while not pangolin.ShouldQuit():
try:
while not q.empty():
state = q.get()
if state is not None:
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.0, 0.0, 0.0, 0.0)
self.dcam.Activate(self.scam)
# Draw Point Cloud
gl.glPointSize(2)
gl.glColor3f(0.0, 1.0, 0.0)
if self.history:
self.points.append(state[0])
pangolin.DrawPoints(
np.reshape(np.array(self.points), (-1, 3)))
else:
pangolin.DrawPoints(state[0])
pangolin.FinishFrame()
except:
continue
def main(showSemSegmentation=False):
# create window
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, 50),
pangolin.ModelViewLookAt(0, 0, -.01, 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)
points, colors, sem = getPointsAndColors("000020.png")
# If we want to show the semantic segmentations
if showSemSegmentation:
colors = colors * 0.5 + sem * 0.5
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)
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 viewer_refresh(self, q):
while not q.empty():
self.points_to_draw, self.point_colors, self.poses_to_draw = q.get(
)
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.0, 0.0, 0.0, 1.0)
self.dcam.Activate(self.scam)
#pangolin.glDrawColouredCube(1)
# Draw feature points with red dots.
gl.glPointSize(4)
gl.glColor3f(1.0, 0.0, 0.0)
if self.points_to_draw is not None:
pangolin.DrawPoints(self.points_to_draw, self.point_colors)
# Test of drawing manual points.
#gl.glColor3f(1.0, 1.0, 1.0)
#pangolin.DrawPoints(np.array([[0, 0, 0], [10, 0, 0], [0, 20, 0], [0, 0, 40]]))
gl.glColor3f(1.0, 1.0, 1.0)
if self.poses_to_draw is not None:
for pose in self.poses_to_draw:
pangolin.DrawCamera(pose, 1, 0.5, 0.8)
pangolin.FinishFrame()
return
def main(self, pts=[], colors=None, camera=None):
if True:
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(1.0, 0.5, 0.5, 1.0)
self.dcam.Activate(self.scam)
# Render OpenGL Cube
# pangolin.glDrawColouredCube(0.1)
# Draw Point Cloud
gl.glColor3f(1.0, 1.0, 0.0)
# pangolin.DrawPoints(points)
if len(pts) > 0:
gl.glPointSize(3)
if isinstance(colors, np.ndarray):
pangolin.DrawPoints(pts, colors)
else:
pangolin.DrawPoints(pts)
if isinstance(camera, list):
self.draw_cameras(camera)
# gl.glColor3f(0.0, 0.0, 0.0)
# gl.glPointSize(1)
# pangolin.DrawPoints(pts2)
pangolin.FinishFrame()
def refresh(self, q):
"""Refresh the display if there is new data
:q: TODO
:returns: TODO
"""
while not q.empty():
self.message = q.get()
if self.message is not None:
map_points, poses, colors = self.message
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.0, 0.0, 0.0, 1.0)
self.dcam.Activate(self.scam)
if poses is not None:
if poses.shape[0] >= 2:
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(poses[0:-1, :])
gl.glColor3f(1.0, 1.0, 0.0)
pangolin.DrawCameras(poses[-1:, :])
if map_points is not None and colors is not None:
if map_points.shape[0] != 0:
gl.glPointSize(3)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(map_points, colors)
pangolin.FinishFrame()
def DISPLAY():
import numpy as np
import OpenGL.GL as gl
import pangolin
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
gl.glPointSize(2)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(points)
pangolin.FinishFrame()
def step_fn(step, inputs):
# Forward pass and loss
with torch.no_grad():
loss, data = utils.forward_pass(model, loss_fn, inputs)
print("loss %f" % loss.item())
print(data.keys())
print(data["pose"].shape)
for i in range(args.batch):
print(list(data["pose"][i, 0, :].cpu().detach().numpy()))
print(list(data["pose"][i, 1, :].cpu().detach().numpy()))
print("--")
depth_img = viz.tensor2depthimg(
torch.cat((*data["depth"][0][:, 0], ), dim=0))
tgt_img = viz.tensor2img(torch.cat((*data["tgt"], ), dim=1))
img = np.concatenate((tgt_img, depth_img), axis=1)
warp_imgs = []
#diff_imgs = []
for warp, diff in zip(data["warp"], data["diff"]):
warp = restack(restack(warp, 1, -1), 0, -2)
diff = restack(restack(diff, 1, -1), 0, -2)
warp_imgs.append(viz.tensor2img(warp))
#diff_imgs.append(viz.tensor2diffimg(diff))
world = reconstruction.depth_to_3d_points(data["depth"][0], data["K"])
points = world[0, :].view(3, -1).transpose(
1, 0).cpu().detach().numpy().astype(np.float64)
colors = (data["tgt"][0, :].view(3, -1).transpose(
1, 0).cpu().detach().numpy().astype(np.float64) + 1) / 2
loop = True
while loop:
key = cv2.waitKey(10)
if key == 27 or pango.ShouldQuit():
exit()
elif key != -1:
loop = False
cv2.imshow("target and depth", img)
#for i, (warp, diff) in enumerate(zip(warp_imgs, diff_imgs)):
for i, warp in enumerate(warp_imgs):
cv2.imshow("warp scale: %d" % i, warp)
#cv2.imshow("diff scale: %d" % i, diff)
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)
gl.glPointSize(5)
pango.DrawPoints(points, colors)
pose = np.identity(4)
pose[:3, 3] = 0
gl.glLineWidth(1)
gl.glColor3f(0.0, 0.0, 1.0)
pango.DrawCamera(pose, 0.5, 0.75, 0.8)
pango.FinishFrame()
def drawPoints(self, pointCloud):
'''
Function to allow chosen point cloud to be drawn
'''
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
SampleToggling.dcam.Activate(SampleToggling.scam)
pangolin.DrawPoints(pointCloud)
pangolin.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(0.0, 1.0, 0.0, 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(0.05, 0.3, 0.0, 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 t_draw2(self):
trajectory = [[0, -6, 6]]
for i in range(300):
trajectory.append(trajectory[-1] + np.random.random(3) - 0.5)
trajectory = np.array(trajectory)
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)
self._dcam.Activate(self._scam)
# 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 work(q, qclose, w=960, h=540):
pangolin.CreateWindowAndBind('pangolin', w, h)
gl.glEnable(gl.GL_DEPTH_TEST)
# Define Projection and initial ModelView matrix
scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(w, h, 420, 420, w // 2, h // 2, 0.2, 10000),
pangolin.ModelViewLookAt(-2, -2, -8, 0, 0, 0,
pangolin.AxisDirection.AxisNegY))
handler = pangolin.Handler3D(scam)
# Create Interactive View in window
dcam = pangolin.CreateDisplay()
dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -w / h)
dcam.SetHandler(handler)
pose = np.eye(4)
opath = np.zeros((0, 3))
opts = np.zeros((2, 3))
colors = np.zeros((2, 3))
while not pangolin.ShouldQuit():
if not qclose.empty():
if qclose.get():
pangolin.Quit()
if not q.empty():
pose, opath, opts, colors = q.get()
colors /= 256.0
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.0, 0.0, 0.0, 1.0)
dcam.Activate(scam)
# draw_axes()
# Draw optimized cloud
gl.glPointSize(2)
gl.glColor3f(0.5, 0.8, 0.5)
pangolin.DrawPoints(opts, colors)
# Draw camera
gl.glLineWidth(1)
gl.glColor3f(0.4, 0.4, 0.4)
pangolin.DrawCamera(pose, 10, 1, 1)
# Optimized path
if len(opath) > 2:
gl.glLineWidth(1)
gl.glColor3f(0.4, 0.4, 0.4)
pangolin.DrawLine(np.array(opath))
pangolin.FinishFrame()
def run(dq, name):
pangolin.CreateWindowAndBind(name, 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, 2000),
pangolin.ModelViewLookAt(-70, -70, 70, 0, 0, 0,
pangolin.AxisDirection.AxisZ))
# Create Interactive View in window
dcam = pangolin.CreateDisplay()
dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0)
dcam.SetHandler(pangolin.Handler3D(scam))
should_quit = False
handlers = {
'pts': VisualizerPangoV2.handler_pts,
'boxes_3d': VisualizerPangoV2.handler_bboxes3d,
'boxes_3d_center': VisualizerPangoV2.handler_bboxes3d_center,
'lines': VisualizerPangoV2.handler_lines,
'boxes_2d': VisualizerPangoV2.handler_bboxes2d
}
targets = {}
for k in handlers:
targets[k] = {}
while not (pangolin.ShouldQuit() or should_quit):
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)
pangolin.glDrawColouredCube()
while not dq.empty():
o = dq.get()
if type(o) is str:
if o == 'quit':
should_quit = True
break
if type(o) is tuple:
op, tp, k, v = o
if tp == 'cam':
VisualizerPangoV2.handler_cam(scam, v)
else:
if op == 'reset':
targets[tp][k] = [v]
elif op == 'add':
if k not in targets[tp]:
targets[tp][k] = []
targets[tp][k].append(v)
for t in handlers:
for k in targets[t]:
for v in targets[t][k]:
handlers[t](*v)
pangolin.FinishFrame()
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 viewer_refresh(self, q):
if self.state is None or not q.empty():
self.state = q.get()
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(1.0, 1.0, 1.0, 1.0)
self.dcam.Activate(self.scam)
gl.glLineWidth(1)
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(self.state[0])
gl.glPointSize(2)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(self.state[1])
pangolin.FinishFrame()
def run(self):
pangolin.CreateWindowAndBind(self.win_name, 640, 480)
gl.glEnable(gl.GL_DEPTH_TEST)
self.scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2, 1000),
pangolin.ModelViewLookAt(2, -10, -30, 0, 0, 0, pangolin.AxisNegY))
self.tree = pangolin.Renderable()
self.tree.Add(pangolin.Axis())
self.handler = pangolin.SceneHandler(self.tree, self.scam)
self.dcam = pangolin.CreateDisplay()
self.dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0)
self.dcam.SetHandler(self.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)
self.dcam.Activate(self.scam)
self.tree.Render()
self.data_lock.acquire()
if self.poses:
gl.glPointSize(5)
gl.glColor3f(*self.cam_color)
for pose in self.poses:
pangolin.DrawPoints(pose[1].T)
if self.map_pts is not None:
gl.glPointSize(2)
gl.glColor3f(*self.map_pts_color)
pangolin.DrawPoints(self.map_pts)
if self.new_pts_obs is not None:
cur_pos = self.poses[-1]
cam_centers = np.repeat(cur_pos[1].T,
self.new_pts_obs.shape[0],
axis=0)
gl.glLineWidth(1)
gl.glColor3f(0.0, 0.0, 1.0)
pangolin.DrawLines(cam_centers, self.new_pts_obs)
self.data_lock.release()
time.sleep(0.02)
pangolin.FinishFrame()
def run_viewer(q):
w, h = 1024, 768
f = 2000
pango.CreateWindowAndBind('g2o', w, h)
gl.glEnable(gl.GL_DEPTH_TEST)
cam = pango.OpenGlRenderState(
pango.ProjectionMatrix(w, h, f, f, w // 2, h // 2, 0.1, 100000),
pango.ModelViewLookAt(
1000.,
1000.,
1000.,
0.,
0.,
0.,
0.,
-1.,
0.,
))
handler = pango.Handler3D(cam)
dcam = pango.CreateDisplay()
dcam.SetBounds(0., 1., 0., 1., -w / h)
dcam.SetHandler(handler)
dcam.Activate()
# nodes = [x.estimate().matrix() for x in optimizer.vertices().values()]
# nodes = np.array(nodes)
edges = None
while not pango.ShouldQuit():
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.15, 0.15, 0.15, 0.0)
dcam.Activate(cam)
try:
edges = q.get(block=False)
except queue.Empty:
pass
if edges is not None:
gl.glLineWidth(1)
gl.glColor3f(0.2, 1.0, 0.2)
pango.DrawLines(edges[:, 0], edges[:, 1])
pango.FinishFrame()
def draw3d(self, out, cam_loc, vis_pose_check):
# object points
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(1, 1, 1, 0)
self.dcam.Activate(self.scam)
# WORLD
gl.glPointSize(3)
gl.glColor3f(1.0, 0, 0)
pangolin.DrawPoints(out)
#POSE
if vis_pose_check:
for cam_loc1 in cam_loc:
# cam pose
gl.glColor3f(0, 1.0, 0)
pose = np.identity(4)
pose[:3, 3] = cam_loc1 #loc
pangolin.DrawCamera(pose, 0.6, 0.4, 0.8)
pangolin.FinishFrame()
def main():
w, h = 640, 480
pangolin.CreateWindowAndBind('Main', 640, 480)
gl.glEnable(gl.GL_DEPTH_TEST)
# Create Interactive View in window
scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(w, h, 420, 420, w//2, h//2, 0.2, 10000),
pangolin.ModelViewLookAt(0, -10, -8,
0, 0, 0,
0, -1, 0))
handler = pangolin.Handler3D(scam)
# Create Interactive View in window
dcam = pangolin.CreateDisplay()
dcam.SetBounds(0.0, 1.0, 0.0, 1.0, w/h)
dcam.SetHandler(handler)
# hack to avoid small Pangolin, no idea why it's *2
dcam.Resize(pangolin.Viewport(0,0,w*2,h*2))
dcam.Activate()
poses = []
# pose = np.hstack((np.identity(3), np.zeros((3,1))))
pose = np.identity(4)
poses.append(np.linalg.inv(pose))
while not pangolin.ShouldQuit():
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.0, 0.0, 0.0, 1.0)
dcam.Activate(scam)
pose[2, 3] = pose[2, 3] - 1
poses.append(np.linalg.inv(pose))
print(poses[-1])
gl.glLineWidth(3)
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(poses)
# pangolin.DrawCamera(pose)
time.sleep(0.2)
# pangolin.DrawCameras(np.linalg.inv(poses[-1]))
# pangolin.DrawCameras(np.stack(poses, axis=0))
# print(np.stack(poses,axis=2).shape)
pangolin.FinishFrame()
def viewer_refresh(self, q):
if self.state is None or not q.empty():
self.state = q.get()
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
# gl.glClearColor(1.0, 1.0, 1.0, 1.0)
gl.glClearColor(0.0, 0.0, 0.0, 1.0)
self.dcam.Activate(self.scam)
# draw poses
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(self.state[0])
# draw keypoints
gl.glPointSize(5)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(self.state[1], self.state[2])
pangolin.FinishFrame()
def main():
pango.CreateWindowAndBind('frustrum render', 640, 480)
# Projection and ModelView Matrices
scam = pango.OpenGlRenderState(
pango.ProjectionMatrix(640, 480, 2000, 2000, 320, 240, 0.1, 5000),
pango.ModelViewLookAt(0, -50, -10, 0, 0, 0, 0, -1,
0)) #pango.AxisDirection.AxisY))
handler = pango.Handler3D(scam)
# Interactive View in Window
disp_cam = pango.CreateDisplay()
disp_cam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0)
disp_cam.SetHandler(handler)
# create and append pose matrices for cameras
pose = np.identity(4)
poses = []
for i in range(3):
poses.append(np.linalg.inv(pose))
pose[2, 3] -= 1
while not pango.ShouldQuit():
# Clear screen
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.15, 0.15, 0.15, 0.0)
disp_cam.Activate(scam)
# Render Cameras
gl.glLineWidth(2)
gl.glColor3f(1.0, 0.0, 1.0)
pango.DrawCamera(poses[0])
gl.glColor3f(0.2, 1.0, 0.2)
pango.DrawCameras(poses[1:-1])
gl.glColor3f(1.0, 1.0, 1.0)
pango.DrawCamera(poses[-1])
# End frame update
pango.FinishFrame()
def viewer_refresh(self, q):
if self.state is None or not q.empty():
self.state = q.get()
ppts = np.array([d[:3, 3] for d in self.state[0]])
spts = np.array(self.state[1])
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(1.0, 1.0, 1.0, 1.0)
self.dcam.Activate(self.scam)
gl.glPointSize(10)
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawPoints(ppts)
gl.glPointSize(2)
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawPoints(spts)
pangolin.FinishFrame()
def refresh(self):
#clear and activate screen
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.15, 0.15, 0.15, 0.0)
#gl.glClearColor(1.0, 1.0, 1.0, 0.0)
self.dcam.Activate(self.scam)
# render
gl.glLineWidth(1)
# render cameras
if len(self.nodes) > 1:
gl.glColor3f(1.0, 1.0, 1.0)
pango.DrawCameras(self.nodes)
# render edges
if len(self.edges) > 1:
gl.glColor3f(0.0, 0.8, 0.5)
pango.DrawLines(self.edges[:, 0, :-1, -1], self.edges[:, 1, :-1,
-1])
pango.FinishFrame()
