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)
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()
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 = 2000
camera_width = 0.02
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)
# texture_depth = pangolin.GlTexture(self.w, self.h, gl.GL_LUMINANCE , False, 0, gl.GL_LUMINANCE, gl.GL_UNSIGNED_BYTE)
cameras = []
old_cameras = []
trajectory = []
old_trajectory = []
connection_edge = []
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')
# depth = 255 * np.ones((self.h, self.w), 'uint8')
gl.glPointSize(3)
gl.glLineWidth(2)
while not pangolin.ShouldQuit():
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.glLineWidth(2)
if not self.q_camera.empty():
cameras = self.q_camera.get()
gl.glColor3f(0.0, 0.0, 1.0)
if len(cameras) > 0:
pangolin.DrawCameras(cameras, camera_width)
if not self.q_old_camera.empty():
old_cameras = self.q_old_camera.get()
if len(old_cameras) > 0:
pangolin.DrawCameras(old_cameras, camera_width)
gl.glLineWidth(4)
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_connection_edge.empty():
connection_edge = self.q_connection_edge.get()
if len(connection_edge) > 1:
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawLine(connection_edge)
if not self.q_old_trajectory.empty():
old_trajectory = self.q_old_trajectory.get()
if len(old_trajectory) > 1:
gl.glColor3f(1.0, 1.0, 0.0)
pangolin.DrawLine(old_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(depth.shape, depth.dtype)
# 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()
