def init(self):
w, h = (1024, 768)
f = 2000 #420
pango.CreateWindowAndBind("g2o_stuff", w, h)
gl.glEnable(gl.GL_DEPTH_TEST)
# Projection and ModelView Matrices
self.scam = pango.OpenGlRenderState(
pango.ProjectionMatrix(w, h, f, f, w // 2, h // 2, 0.1, 100000),
pango.ModelViewLookAt(0, -50.0, -10.0, 0.0, 0.0, 0.0, 0.0, -1.0,
0.0)) #pango.AxisDirection.AxisY))
self.handler = pango.Handler3D(self.scam)
# Interactive View in Window
self.dcam = pango.CreateDisplay()
self.dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -w / h)
self.dcam.SetHandler(self.handler)
self.dcam.Activate()
pango.RegisterKeyPressCallback(ord('r'), self.optimize_callback)
pango.RegisterKeyPressCallback(ord('t'), self.switch_callback)
def setup():
global dcam, scam
pangolin.CreateWindowAndBind('Main', 640, 480)
gl.glEnable(gl.GL_DEPTH_TEST)
# Define Projection and initial ModelView matrix
width = 640
height = 480
z_near = 0.2
z_far = 1000
scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(width, height, 420, 420, 320, 240, z_near,
z_far),
# Negative Y direction is up since top left corner of images is (0,0)
pangolin.ModelViewLookAt(-2, -2, -4, 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, -640.0 / 480.0)
dcam.SetHandler(handler)
def init(self, width, height):
"""Initialize the 3D display (pangolin)
:width: TODO
:height: TODO
:returns: TODO
"""
pangolin.CreateWindowAndBind('Map viewer', width, height)
gl.glEnable(gl.GL_DEPTH_TEST)
# Define Projection and initial ModelView matrix
self.scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(width, height, 420, 420, width // 2,
height // 2, 0.2, 10000),
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(0.0, 1.0, 0.0, 1.0, -width / height)
self.dcam.SetHandler(self.handler)
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)
dcam.Resize(pangolin.Viewport(0, 0, 640 * 2, 480 * 2))
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 __init__(self, name):
super().__init__()
self.name = name
# Window
pango.CreateWindowAndBind(self.name, int(640*(3/2)), int(480*(3/2)))
gl.glEnable(gl.GL_DEPTH_TEST)
# Define Projection and initial ModelView matrix
self.scam = pango.OpenGlRenderState(
pango.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2, 200),
pango.ModelViewLookAt(0, -0.5, -0.5, 0, 0, 1, 0, -1, 0))
self.handler = pango.Handler3D(self.scam)
# Create Interactive View in window
self.dcam = pango.CreateDisplay()
self.dcam.SetBounds(
pango.Attach(0),
pango.Attach(1),
pango.Attach(0),
pango.Attach(1), -640.0/480.0)
self.dcam.SetHandler(self.handler)
def main():
# Parse arguments
args = options.get_args(description="Debug a network",
options=[
"batch",
"workers",
"device",
"load",
"net",
"loss",
])
if args.load == "":
print("No model file specified to load!")
exit()
# Construct datasets
random.seed(1337)
_, _, test_loader = utils.get_kitti_split(args.batch, args.workers)
# The model
model = networks.architectures.get_net(args)
loss_fn = sfm_loss.get_loss_fn(args)
# Load
checkpoint = torch.load(args.load, map_location=torch.device(args.device))
model.load_state_dict(checkpoint["model"])
# Window
pango.CreateWindowAndBind('Main', int(640 * (3 / 2)), int(480 * (3 / 2)))
gl.glEnable(gl.GL_DEPTH_TEST)
# Define Projection and initial ModelView matrix
scam = pango.OpenGlRenderState(
pango.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2, 200),
pango.ModelViewLookAt(0, -0.5, -0.5, 0, 0, 1, 0, -1, 0))
handler = pango.Handler3D(scam)
# Create Interactive View in window
dcam = pango.CreateDisplay()
dcam.SetBounds(pango.Attach(0), pango.Attach(1), pango.Attach(0),
pango.Attach(1), -640.0 / 480.0)
dcam.SetHandler(handler)
# Test
model.train()
model = model.to(args.device)
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()
utils.iterate_loader(args, test_loader, step_fn)
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()
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)
panel = pangolin.CreatePanel('menu')
panel.SetBounds(0.5, 1.0, 0.0, 175 / 1024.)
# checkbox
m_follow_camera = pangolin.VarBool('menu.Follow Camera',
value=True,
toggle=True)
m_show_points = pangolin.VarBool('menu.Show Points', True, True)
m_show_lines = pangolin.VarBool('menu.Show Lines', True, True)
m_show_keyframes = pangolin.VarBool('menu.Show KeyFrames', True, True)
m_show_graph = pangolin.VarBool('menu.Show Graph', True, True)
m_show_image = pangolin.VarBool('menu.Show Image', True, True)
# button
m_replay = pangolin.VarBool('menu.Replay', value=False, toggle=False)
m_refresh = pangolin.VarBool('menu.Refresh', False, False)
m_reset = pangolin.VarBool('menu.Reset', False, False)
if self.config is None:
width, height = 400, 250
viewpoint_x = 0
viewpoint_y = -500 # -10
viewpoint_z = -100 # -0.1
viewpoint_f = 2000
camera_width = 1.
else:
width = self.config.view_image_width
height = self.config.view_image_height
viewpoint_x = self.config.view_viewpoint_x
viewpoint_y = self.config.view_viewpoint_y
viewpoint_z = self.config.view_viewpoint_z
viewpoint_f = self.config.view_viewpoint_f
camera_width = self.config.view_camera_width
proj = pangolin.ProjectionMatrix(1024, 768, viewpoint_f, viewpoint_f,
512, 389, 0.1, 5000)
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.0, 1.0, 175 / 1024., 1.0, -1024 / 768.)
dcam.SetHandler(pangolin.Handler3D(scam))
# image
# width, height = 400, 130
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')
pose = pangolin.OpenGlMatrix() # identity matrix
following = True
active = []
replays = []
graph = []
loops = []
mappoints = DynamicArray(shape=(3, ))
maplines = DynamicArray(shape=(6, ))
colors = DynamicArray(shape=(3, ))
cameras = DynamicArray(shape=(4, 4))
active_lines = []
line_colors = []
gnd_mesh = None
while not pangolin.ShouldQuit():
if not self.q_pose.empty():
pose.m = self.q_pose.get()
follow = m_follow_camera.Get()
if follow and following:
scam.Follow(pose, True)
elif follow and not following:
scam.SetModelViewMatrix(look_view)
scam.Follow(pose, True)
following = True
elif not follow and following:
following = False
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)
# show graph
if not self.q_graph.empty():
graph = self.q_graph.get()
loops = np.array([_[0] for _ in graph if _[1] == 2])
graph = np.array([_[0] for _ in graph if _[1] < 2])
if m_show_graph.Get():
if len(graph) > 0:
gl.glLineWidth(1)
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawLines(graph, 3)
if len(loops) > 0:
gl.glLineWidth(2)
gl.glColor3f(0.0, 0.0, 0.0)
pangolin.DrawLines(loops, 4)
gl.glPointSize(4)
gl.glColor3f(1.0, 0.0, 0.0)
gl.glBegin(gl.GL_POINTS)
gl.glVertex3d(pose[0, 3], pose[1, 3], pose[2, 3])
gl.glEnd()
# if not self.q_ground.empty():
# gnd_mesh = self.q_ground.get()
if gnd_mesh is not None:
gl.glLineWidth(2)
gl.glColor3f(0.5, 0.25, 0.5)
pangolin.DrawLines(gnd_mesh, 2)
# Show mappoints
if not self.q_points.empty():
pts, code = self.q_points.get()
cls, code = self.q_colors.get()
if code == 1: # append new points
mappoints.extend(pts)
colors.extend(cls)
elif code == 0: # refresh all points
mappoints.clear()
mappoints.extend(pts)
colors.clear()
colors.extend(cls)
if m_show_points.Get():
gl.glPointSize(4)
# easily draw millions of points
pangolin.DrawPoints(mappoints.array(), colors.array())
if not self.q_active.empty():
active = self.q_active.get()
gl.glPointSize(5)
gl.glBegin(gl.GL_POINTS)
gl.glColor3f(1.0, 0.0, 0.0)
for point in active:
gl.glVertex3f(*point)
gl.glEnd()
if not self.q_lines.empty():
lines, code = self.q_lines.get()
maplines.extend(lines)
if m_show_lines.Get():
gl.glLineWidth(1)
gl.glColor3f(1.0, 0.0, 0.5)
pangolin.DrawLines(maplines.array(), 2)
if not self.q_active_lines.empty():
active_lines, line_colors = np.array(
self.q_active_lines.get())
if active_lines is not None:
for act_line, color in zip(active_lines, line_colors):
c = (color[2] / 255, color[1] / 255, color[0] / 255)
act_line = act_line.reshape((1, 6))
gl.glLineWidth(5)
gl.glColor3f(*c)
pangolin.DrawLines(act_line, 2)
if len(replays) > 0:
n = 300
gl.glPointSize(4)
gl.glColor3f(1.0, 0.0, 0.0)
gl.glBegin(gl.GL_POINTS)
for point in replays[:n]:
gl.glVertex3f(*point)
gl.glEnd()
replays = replays[n:]
# show cameras
if not self.q_camera.empty():
cams = self.q_camera.get()
if len(cams) > 20:
cameras.clear()
cameras.extend(cams)
if m_show_keyframes.Get():
gl.glLineWidth(1)
gl.glColor3f(0.0, 0.0, 1.0)
pangolin.DrawCameras(cameras.array(), camera_width)
# show image
if not self.q_image.empty():
image = self.q_image.get()
if image.ndim == 3:
image = image[::-1, :, ::-1]
else:
image = np.repeat(image[::-1, :, np.newaxis], 3, axis=2)
image = cv2.resize(image, (width, height))
if m_show_image.Get():
texture.Upload(image, gl.GL_RGB, gl.GL_UNSIGNED_BYTE)
dimg.Activate()
gl.glColor3f(1.0, 1.0, 1.0)
texture.RenderToViewport()
if pangolin.Pushed(m_replay):
replays = mappoints.array()
if pangolin.Pushed(m_reset):
m_show_graph.SetVal(True)
m_show_keyframes.SetVal(True)
m_show_points.SetVal(True)
m_show_image.SetVal(True)
m_follow_camera.SetVal(True)
follow_camera = True
if pangolin.Pushed(m_refresh):
self.q_refresh.put(True)
pangolin.FinishFrame()
def visualize(R1, t1, R2, t2, R3, t3, R4, t4, 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, 100),
# pangolin.ModelViewLookAt(0, -10, 0.1, 0, 0, 0, pangolin.AxisNegY))
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)
w = 0.3
h = w * 0.75
z = w * 0.6
t1r = np.matmul(R1, t1)
t2r = np.matmul(R2, t2)
t3r = np.matmul(R3, t3)
t4r = np.matmul(R4, t4)
bottom = np.array([[0, 0, 0, 1.0]])
T1 = np.concatenate([R1, t1], axis=1)
T1 = np.concatenate([T1, bottom], axis=0)
T2 = np.concatenate([R2, t2], axis=1)
T2 = np.concatenate([T2, bottom], axis=0)
T3 = np.concatenate([R3, t3], axis=1)
T3 = np.concatenate([T3, bottom], axis=0)
T4 = np.concatenate([R4, t4], axis=1)
T4 = np.concatenate([T4, bottom], axis=0)
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.glLineWidth(2)
gl.glBegin(gl.GL_LINES)
gl.glColor3f(0.0, 0.0, 0.0)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(w, h, z)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(w, -h, z)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(-w, -h, z)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(-w, h, z)
gl.glVertex3f(w, h, z)
gl.glVertex3f(w, -h, z)
gl.glVertex3f(-w, h, z)
gl.glVertex3f(-w, -h, z)
gl.glVertex3f(-w, h, z)
gl.glVertex3f(w, h, z)
gl.glVertex3f(-w, -h, z)
gl.glVertex3f(w, -h, z)
gl.glEnd()
gl.glPushMatrix()
gl.glMultMatrixd(T1.T)
gl.glLineWidth(2)
gl.glBegin(gl.GL_LINES)
gl.glColor3f(0., 1.0, 1.0)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(w, h, z)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(w, -h, z)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(-w, -h, z)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(-w, h, z)
gl.glVertex3f(w, h, z)
gl.glVertex3f(w, -h, z)
gl.glVertex3f(-w, h, z)
gl.glVertex3f(-w, -h, z)
gl.glVertex3f(-w, h, z)
gl.glVertex3f(w, h, z)
gl.glVertex3f(-w, -h, z)
gl.glVertex3f(w, -h, z)
gl.glEnd()
gl.glPopMatrix()
gl.glPushMatrix()
gl.glMultMatrixd(T2.T)
gl.glLineWidth(2)
gl.glBegin(gl.GL_LINES)
gl.glColor3f(1.0, 0.7, 1.0)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(w, h, z)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(w, -h, z)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(-w, -h, z)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(-w, h, z)
gl.glVertex3f(w, h, z)
gl.glVertex3f(w, -h, z)
gl.glVertex3f(-w, h, z)
gl.glVertex3f(-w, -h, z)
gl.glVertex3f(-w, h, z)
gl.glVertex3f(w, h, z)
gl.glVertex3f(-w, -h, z)
gl.glVertex3f(w, -h, z)
gl.glEnd()
gl.glPopMatrix()
gl.glPushMatrix()
gl.glMultMatrixd(T3.T)
gl.glLineWidth(2)
gl.glBegin(gl.GL_LINES)
gl.glColor3f(0.8, 0.8, 0.)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(w, h, z)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(w, -h, z)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(-w, -h, z)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(-w, h, z)
gl.glVertex3f(w, h, z)
gl.glVertex3f(w, -h, z)
gl.glVertex3f(-w, h, z)
gl.glVertex3f(-w, -h, z)
gl.glVertex3f(-w, h, z)
gl.glVertex3f(w, h, z)
gl.glVertex3f(-w, -h, z)
gl.glVertex3f(w, -h, z)
gl.glEnd()
gl.glPopMatrix()
gl.glPushMatrix()
gl.glMultMatrixd(T4.T)
gl.glLineWidth(2)
gl.glBegin(gl.GL_LINES)
gl.glColor3f(0.7, 0.7, 0.7)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(w, h, z)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(w, -h, z)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(-w, -h, z)
gl.glVertex3f(0, 0, 0)
gl.glVertex3f(-w, h, z)
gl.glVertex3f(w, h, z)
gl.glVertex3f(w, -h, z)
gl.glVertex3f(-w, h, z)
gl.glVertex3f(-w, -h, z)
gl.glVertex3f(-w, h, z)
gl.glVertex3f(w, h, z)
gl.glVertex3f(-w, -h, z)
gl.glVertex3f(w, -h, z)
gl.glEnd()
gl.glPopMatrix()
# gl.glLineWidth(2)
# gl.glBegin(gl.GL_LINES)
# gl.glColor3f(0., 1., 0.)
# gl.glVertex3f(0, 0, 0)
#
# gl.glVertex3f(t1r[0], t1r[1], t1r[2])
# gl.glVertex3f(t1r[0], t1r[1], t1r[2])
# gl.glVertex3f(t2r[0], t2r[1], t3r[2])
# gl.glVertex3f(t2r[0], t2r[1], t3r[2])
# gl.glVertex3f(t3r[0], t3r[1], t4r[2])
# gl.glVertex3f(t3r[0], t3r[1], t4r[2])
# gl.glVertex3f(t4r[0], t4r[1], t4r[2])
#
# gl.glEnd()
#
pangolin.FinishFrame()
def UpdateProjectionMat(self):
self.projectionMat = pangolin.ProjectionMatrix(1024, 768, 420, 420,
320, 240, self.near, self.far)
return self.projectionMat
def glMainLoop(self):
self.logger.info("Running gl viewer ...")
flag = False
pangolin.CreateWindowAndBind('Main', 640, 480)
gl.glEnable(gl.GL_DEPTH_TEST)
gl.glEnable(gl.GL_BLEND)
gl.glBlendFunc(gl.GL_SRC_ALPHA, gl.GL_ONE_MINUS_SRC_ALPHA)
panel = pangolin.CreatePanel('menu')
panel.SetBounds(1.0, 1.0, 0.0, 100 / 640.)
# Does not work with initialization of window
# self.Init()
camera_pose = pangolin.OpenGlMatrix()
# create scene graph
# scene = pangolin.Renderable()
# # x : R
# # y : G
# # z : B
# scene.Add(pangolin.Axis())
#
rendered_cam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.01, 2000),
pangolin.ModelViewLookAt(1, 0, 1, 0, 0, 0, 0, 0, 1)
)
handler = pangolin.Handler3D(rendered_cam)
# handler = pangolin.SceneHandler(scene, rendered_cam)
# add drawing callback
#
viewport = pangolin.CreateDisplay()
viewport.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0)
viewport.SetHandler(handler)
image_viewport = pangolin.Display('image')
w = 160
h = 120
image_viewport.SetBounds(0, h / 480., 0.0, w / 640., 640. / 480.)
image_viewport.SetLock(pangolin.Lock.LockLeft, pangolin.Lock.LockTop)
texture = pangolin.GlTexture(w, h, gl.GL_RGB, False, 0, gl.GL_RGB, gl.GL_UNSIGNED_BYTE)
# geometries
self.lines = []
self.points = {}
self.colors = {}
self.poses = []
self.redundant_points = []
self.redundant_colors = []
self.landmarks = {}
#
img = np.ones((h, w, 3), 'uint8')
while not pangolin.ShouldQuit():
datum = None
if not self.attributes.empty():
self.logger.info("[GL Process] attributes is not empty, fetching datum ...")
datum = self.attributes.get()
self.logger.info("[GL Process] get a datum from the main thread of the parent process")
if datum is not None:
# dispatch instructions
if datum.attrs.get('pose', None) is not None:
pose = datum.attrs['pose']
# self.camera.pose.m = pose
# set Twc
camera_pose.m = pose # np.linalg.inv(pose)
# camera_pose.m = pose
rendered_cam.Follow(camera_pose, True)
self.logger.info("[GL Process] update camera pose matrix got from datum: \n%s" % pose)
pass
pass
# self.Clear()
# self.control.Update(self.camera.pose)
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(1.0, 1.0, 1.0, 1.0)
viewport.Activate(rendered_cam)
# scene.Render()
# Just for test
# pangolin.glDrawColouredCube(0.1)
# render graph
if datum is not None:
# dispatch data
# update graph
if datum.attrs.get('lines', None) is not None:
lines = datum.attrs['lines']
self.lines.extend(lines)
self.logger.info("[GL Process] drawing %d lines ..." % len(lines))
if datum.attrs.get('pose', None) is not None:
pose = datum.attrs['pose']
self.poses.append(pose)
self.logger.info("[GL Process] drawing a camera with new pose matrix ...")
pass
# update image
if datum.attrs.get('img', None) is not None:
img = datum.attrs['img']
# see pangolin issue #180
# change cv BGR channels to norm one
# img = img[::-1, : ,::-1].astype(np.uint8)
img = img.astype(np.uint8)
img = cv2.resize(img, (w, h))
self.logger.info("[GL Process] drawing image to image viewport ...")
# show mappoints
if datum.attrs.get('points', None) is not None:
points = datum.attrs['points']
if len(points) > 0:
# self.points.extend(points)
for point in points:
self.points[point.seq] = point
# colors = np.array([p.color if p.color is not None else np.array([1.0, 0.0, 0.0]) for p in points]).astype(np.float64)
colors = [p.color / 255. if p.color is not None else np.array([1.0, 1.0, 0.0]) for p in points]
# print("colors: \n%s" % np.array(colors))
# colors = [ [1., 1., 0.] for p in points]
# self.colors.extend(colors)
for i, color in enumerate(colors):
point = points[i]
self.colors[point.seq] = color
self.logger.info("[GL Process] drawing %d points" % len(points))
# print("new mappoints: \n%s" % np.array([ p.data for p in points]).astype(np.float64))
# print("new colors (default): \n%s" % np.array(colors))
else:
self.logger.info("[GL Process] no points to be drawn.")
# redundant points
if datum.attrs.get('points-redundant', None) is not None:
points = datum.attrs['points-redundant']
colors = datum.attrs['colors-redundant']
for i, p in enumerate(points):
self.redundant_points.append(p)
self.redundant_colors.append(colors[i] / 255.)
# show landmarks
if datum.attrs.get('landmarks', None) is not None:
landmarks = datum.attrs['landmarks']
for landmark in landmarks:
self.landmarks[landmark.seq] = landmark
self.logger.info("[GL Process] drawing %d landmarks" % len(landmarks))
self.attributes.task_done()
self.logger.info("[GL Process] datum has been processed.")
############
# draw graph
############
line_geometries = np.array([
[*line[0], *line[1]] for line in self.lines
])
if len(line_geometries) > 0:
gl.glLineWidth(1)
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawLines(line_geometries, 3)
# pose = self.camera.pose
pose = camera_pose
# GL 2.0 API
gl.glPointSize(4)
gl.glColor3f(1.0, 0.0, 0.0)
gl.glBegin(gl.GL_POINTS)
gl.glVertex3d(pose[0, 1], pose[1, 3], pose[2, 3])
gl.glEnd()
############
# draw poses
############
if len(self.poses) > 0:
gl.glLineWidth(1)
gl.glColor3f(0.0, 0.0, 1.0)
# poses: numpy.ndarray[float64], w: float=1.0, h_ratio: float=0.75, z_ratio: float=0.6
pangolin.DrawCameras(np.array(self.poses))
gl.glPointSize(4)
gl.glColor3f(0.0, 0.0, 1.0)
gl.glBegin(gl.GL_POINTS)
for pose in self.poses:
gl.glVertex3d(pose[0, 1], pose[1, 3], pose[2, 3])
gl.glEnd()
################
# draw mappoints
################
if len(self.points) > 0:
# points_geometries = np.array([p.data for p in self.points]).astype(np.float64)
points_geometries = []
colors = []
for point_key, p in self.points.items():
points_geometries.append(p.data)
colors.append(self.colors[point_key])
points_geometries = np.array(points_geometries).astype(np.float64)
colors = np.array(colors).astype(np.float64)
gl.glPointSize(6)
# pangolin.DrawPoints(points_geometries, np.array(self.colors).astype(np.float64) )
pangolin.DrawPoints(points_geometries, colors)
# gl.glPointSize(4)
# gl.glColor3f(1.0, 0.0, 0.0)
#
# gl.glBegin(gl.GL_POINTS)
# for point in points_geometries:
# gl.glVertex3d(point[0], point[1], point[2])
# gl.glEnd()
####################
# redundant points #
####################
if len(self.redundant_points) > 0:
points_geometries = []
colors = []
for i, p in enumerate(self.redundant_points):
points_geometries.append(p.data)
colors.append(self.redundant_colors[i])
points_geometries = np.array(points_geometries).astype(np.float64)
colors = np.array(colors).astype(np.float64)
gl.glPointSize(3)
pangolin.DrawPoints(points_geometries, colors)
################
# draw landmarks
################
for key, landmarkDatum in self.landmarks.items():
# abox = landmark.computeAABB()
# drawABox(abox.toArray())
drawABox(landmarkDatum.abox.toArray(), color=landmarkDatum.color)
pass
#############
# draw images
#############
# self.camera.RenderImg(img)
texture.Upload(img, gl.GL_RGB, gl.GL_UNSIGNED_BYTE)
image_viewport.Activate()
gl.glColor3f(1.0, 1.0, 1.0)
texture.RenderToViewport()
pangolin.FinishFrame()
print("gl program loop stopped")
with self._state_locker:
self._stopped = True
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)
panel = pangolin.CreatePanel('menu')
panel.SetBounds(0.5, 1.0, 0.0, 175 / 1024.)
# checkbox
if self.view_point_cloud:
m_show_points = pangolin.VarBool('menu.Show Points', True, True)
m_show_keyframes = pangolin.VarBool('menu.Show KeyFrames', True, True)
m_show_graph = pangolin.VarBool('menu.Show Graph', True, True)
m_show_image = pangolin.VarBool('menu.Show Image', True, True)
# button
m_next_frame = pangolin.VarBool('menu.Next', False, False)
if self.config is None:
viewpoint_x = 0
viewpoint_y = -500 # -10
viewpoint_z = -100 # -0.1
viewpoint_f = 2000
camera_width = 1.
width, height = 350, 250
else:
viewpoint_x = self.config.view_viewpoint_x
viewpoint_y = self.config.view_viewpoint_y
viewpoint_z = self.config.view_viewpoint_z
viewpoint_f = self.config.view_viewpoint_f
camera_width = self.config.view_camera_width
width = self.config.view_image_width * 2
height = self.config.view_image_height
proj = pangolin.ProjectionMatrix(1024, 768, viewpoint_f, viewpoint_f,
512, 389, 0.1, 5000)
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.0, 1.0, 175 / 1024., 1.0, -1024 / 768.)
dcam.SetHandler(pangolin.Handler3D(scam))
# Dilay image
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')
pose = pangolin.OpenGlMatrix() # identity matrix
active = []
graph = []
loops = []
loops_local = []
mappoints = DynamicArray(shape=(3, ))
colors = DynamicArray(shape=(3, ))
cameras = DynamicArray(shape=(4, 4))
while not pangolin.ShouldQuit():
if not self.q_pose.empty():
pose.m = self.q_pose.get()
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)
# Show graph
if not self.q_graph.empty():
graph = self.q_graph.get()
loops = np.array([_[0] for _ in graph if _[1] == 2])
loops_local = np.array([_[0] for _ in graph if _[1] == 1])
graph = np.array([_[0] for _ in graph if _[1] == 0])
if m_show_graph.Get():
if len(graph) > 0:
gl.glLineWidth(1)
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawLines(graph, 3)
if len(loops) > 0:
gl.glLineWidth(2)
gl.glColor3f(1.0, 0.0, 1.0)
pangolin.DrawLines(loops, 4)
if len(loops_local) > 0:
gl.glLineWidth(2)
gl.glColor3f(1.0, 1.0, 0.0)
pangolin.DrawLines(loops_local, 4)
if self.view_point_cloud:
# Show mappoints
if not self.q_points.empty():
pts, code = self.q_points.get()
cls, code = self.q_colors.get()
if code == 1: # append new points
mappoints.extend(pts)
colors.extend(cls)
elif code == 0: # refresh all points
mappoints.clear()
mappoints.extend(pts)
colors.clear()
colors.extend(cls)
if m_show_points.Get():
gl.glPointSize(2)
# easily draw millions of points
pangolin.DrawPoints(mappoints.array(), colors.array())
if not self.q_active.empty():
active = self.q_active.get()
gl.glPointSize(3)
gl.glBegin(gl.GL_POINTS)
gl.glColor3f(1.0, 0.0, 0.0)
for point in active:
gl.glVertex3f(*point)
gl.glEnd()
# Show cameras
if not self.q_camera.empty():
cams = self.q_camera.get()
if len(cams) > 20:
cameras.clear()
cameras.extend(cams)
if m_show_keyframes.Get():
if cameras.array().shape[0] > 0:
gl.glLineWidth(1)
gl.glColor3f(0.0, 0.0, 1.0)
pangolin.DrawCameras(cameras.array()[:-1], camera_width)
gl.glLineWidth(1)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawCameras(
np.expand_dims(cameras.array()[-1], axis=0),
camera_width)
# Show image
if not self.q_image.empty():
image = self.q_image.get()
if image.ndim == 3:
image = image[::-1, :, ::-1]
else:
image = np.repeat(image[::-1, :, np.newaxis], 3, axis=2)
image = cv2.resize(image, (width, height))
if m_show_image.Get():
texture.Upload(image, gl.GL_RGB, gl.GL_UNSIGNED_BYTE)
dimg.Activate()
gl.glColor3f(1.0, 1.0, 1.0)
texture.RenderToViewport()
if pangolin.Pushed(m_next_frame):
self.q_next.put(True)
pangolin.FinishFrame()
# print("******** : ")
# print(np.linalg.inv(pose))
# print(pose)
# Ow = pose[:3, 3]
# Xw = 0.1*pose[:3, :3].dot(np.array([1, 0, 0]).T) + Ow
# Yw = 0.1*pose[:3, :3].dot(np.array([0, 1, 0]).T) + Ow
# Zw = 0.1*pose[:3, :3].dot(np.array([0, 0, 1]).T) + Ow
# print(Ow, Xw, Yw, Zw)
pangolin.CreateWindowAndBind("Trajectory 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)
scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
pangolin.ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0))
handler = pangolin.Handler3D(scam)
dcam = pangolin.CreateDisplay()
dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0 / 768.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.glLineWidth(2)
Ow = np.array([0.0, 0.0, 0.0]).T
for pose in poses:
Ow = pose[:3, 3]
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)
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(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
print(points.shape)
print(colors.shape)
print(points[0:3])
print(colors[0:3])
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.ones(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 display(self):
points = None
W = 990
H = 540
pangolin.CreateWindowAndBind('JSLAM', W, H)
gl.glEnable(gl.GL_DEPTH_TEST)
# Define Projection and initial ModelView matrix
scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(W, H, 420, 420, 320, 240, 0.2, 100),
pangolin.ModelViewLookAt(0, 2, 15, 2, -3, -5,
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)
x = 0
y = 0
z = 0
# Perspective coordinates
xc = 0
yc = 0
zc = 0
animation_counter = 0
while not pangolin.ShouldQuit():
failed_to_load = False
try:
points = pickle.load(open("data/points.p", "rb"))
except Exception:
failed_to_load = True
pass
if not failed_to_load:
self.realPoints = []
for i, (xp, yp) in enumerate(points):
self.realPoints.append([xp, yp, z])
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0, 0, 0, 0)
dcam.Activate(scam)
# Draw Point Cloud
if not failed_to_load:
points = np.random.random((100000, 3)) * 10
gl.glPointSize(2)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(points)
# Load the camera
print("Animation counter: {}".format(animation_counter))
pose = np.identity(4)
pose[:3, 3] = [x, y, z]
gl.glLineWidth(2)
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCamera(pose, 0.5, 0.75, 0.8)
if not self.creative_mode or animation_counter > 100:
zc += 0.1
scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(W, H, 420, 420, 320, 240, 0.2,
100),
pangolin.ModelViewLookAt(0, 2, 15 + zc, 2, -3, -5,
pangolin.AxisDirection.AxisY))
handler = pangolin.Handler3D(scam)
dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0)
dcam.SetHandler(handler)
z += 0.1
animation_counter += 1
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()
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()
import numpy as np
import OpenGL.GL as gl
import pangolin
state = np.load("traj_full.npy", allow_pickle=True)
w = 1024
h = 768
pangolin.CreateWindowAndBind('Map Viewer', w, h)
gl.glEnable(gl.GL_DEPTH_TEST)
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()
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)
if state is not None:
if state[0].shape[0] >= 2:
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)
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 = -5 # -10
viewpoint_z = -10 # -0.1
viewpoint_f = 200
camera_width = 0.5
proj = pangolin.ProjectionMatrix(1024, 768, viewpoint_f, viewpoint_f,
512, 389, 0.1, 5000)
look_view = pangolin.ModelViewLookAt(viewpoint_x, viewpoint_y,
viewpoint_z, 0, 0, 0, 0, -1, 0)
scam = pangolin.OpenGlRenderState(proj, look_view)
dcam = pangolin.CreateDisplay()
dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -1024. / 768.)
dcam.SetHandler(pangolin.Handler3D(scam))
dimg = pangolin.Display('image')
dimg.SetBounds(0.0, self.h / 768., 0.0, self.w / 1024.,
float(self.w) / self.h)
dimg.SetLock(pangolin.Lock.LockLeft, pangolin.Lock.LockTop)
texture = pangolin.GlTexture(self.w, self.h, gl.GL_RGB, False, 0,
gl.GL_RGB, gl.GL_UNSIGNED_BYTE)
ddepth = pangolin.Display('depth')
ddepth.SetBounds(self.h / 768., self.h / 768. * 2.0, 0.0,
self.w / 1024.,
float(self.w) / float(self.h))
ddepth.SetLock(pangolin.Lock.LockLeft, pangolin.Lock.LockTop)
if self.use_custom_depth_image:
print("Use RGB depth buffer")
texture_depth = pangolin.GlTexture(self.w, self.h, gl.GL_RGB,
False, 0, gl.GL_RGB,
gl.GL_UNSIGNED_BYTE)
else:
texture_depth = pangolin.GlTexture(self.w, self.h, gl.GL_LUMINANCE,
False, 0, gl.GL_LUMINANCE,
gl.GL_UNSIGNED_BYTE)
cameras = []
trajectory = []
pose = pangolin.OpenGlMatrix()
points = np.empty(shape=(0, 3))
colors = np.empty(shape=(0, 3))
# image = random_image(self.w, self.h)
image = 255 * np.ones((self.h, self.w, 3), 'uint8')
if self.use_custom_depth_image:
depth = 255 * np.ones((self.h, self.w, 3), 'uint8')
else:
depth = 255 * np.ones((self.h, self.w), 'uint8')
gl.glPointSize(3)
gl.glLineWidth(2)
while not pangolin.ShouldQuit():
if not self.q_camera.empty():
cameras = self.q_camera.get()
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.75, 0.75, 0.75, 1.0)
if not self.q_pose.empty():
pose.m = self.q_pose.get()
# scam.Follow(pose, True)
dcam.Activate(scam)
gl.glColor3f(0.0, 0.0, 1.0)
if len(cameras) > 0:
pangolin.DrawCameras(cameras, camera_width)
if not self.q_trajectory.empty():
trajectory = self.q_trajectory.get()
if len(trajectory) > 1:
gl.glColor3f(0.0, 0.0, 0.0)
pangolin.DrawLine(trajectory)
if not self.q_point.empty():
points = self.q_point.get()
if not self.q_color.empty():
colors = self.q_color.get()
# if len(points) > 0:
# pangolin.DrawPoints(points, colors)
if not self.q_image.empty():
image = self.q_image.get()
# print(image.shape, image.dtype)
texture.Upload(image, gl.GL_RGB, gl.GL_UNSIGNED_BYTE)
dimg.Activate()
gl.glColor3f(1.0, 1.0, 1.0)
texture.RenderToViewport()
if not self.q_depth.empty():
depth = self.q_depth.get()
print('^^^')
print(depth.shape, depth.dtype)
if self.use_custom_depth_image:
texture_depth.Upload(depth, gl.GL_RGB, gl.GL_UNSIGNED_BYTE)
else:
texture_depth.Upload(depth, gl.GL_LUMINANCE,
gl.GL_UNSIGNED_BYTE)
ddepth.Activate()
gl.glColor3f(1.0, 1.0, 1.0)
texture_depth.RenderToViewport()
pangolin.FinishFrame()
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.0, 1.0, 175 / 1024., 1.0, -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 UpdatePointsThread(self, PointsQueue, PoseQueue):
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))
scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2,
10000),
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)
points = None
cameras = None
while not pangolin.ShouldQuit():
if points is None:
try:
points = PointsQueue.get()
except:
continue
else:
points = np.vstack((points, PointsQueue.get()))
if cameras is None:
try:
cameras = [PoseQueue.get()]
except:
continue
else:
cameras.append(PoseQueue.get())
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()
gl.glPointSize(2)
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawPoints(points)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawCameras(cameras)
# gl.glPointSize(5)
# gl.glColor3f(1.0, 0.0, 0.0)
# pangolin.DrawPoints(self.state[1], self.state[2])
#assert 1==0
pangolin.FinishFrame()
