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 handler_bboxes2d(boxes_2d, attrs):
if boxes_2d is None:
return
if attrs:
gl.glColor3f(*attrs['Color'])
else:
gl.glColor3f(0.0, 0.0, 1.0)
z = attrs.get('z', 0.0)
for b in boxes_2d:
b2d = b[0:8].reshape(4, 2)
b3d = []
for pt in b2d:
x, y = pt
b3d.append([x, y, z])
b3d.append([
0.5 * (b2d[0, 0] + b2d[1, 0]), 0.5 * (b2d[0, 1] + b2d[1, 1]), z
])
b3d_to_draw = []
to_draw_idx = [3, 0, 1, 2, 4, 3, 2]
for di in to_draw_idx:
b3d_to_draw.append(b3d[di])
i_cls = int(b[9]) if int(b[9]) <= 5 else 5
cls = VisualizerPangoV2.CLS2COLOR[i_cls]
gl.glColor3f(*cls)
pangolin.DrawLine(b3d_to_draw,
point_size=int(attrs.get('PointSize', 5)))
gl.glColor3f(*attrs['Color'])
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 handler_lines(lines, attrs):
if lines is None:
return
if attrs:
gl.glColor3f(*attrs['Color'])
point_size = attrs['PointSize']
else:
gl.glColor3f(0.0, 0.0, 1.0)
point_size = 5.0
pangolin.DrawLine(lines, point_size=point_size)
def draw(self, poseList_nx4x4, drawCamera=False):
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)
# pangolin.glDrawColouredCube()
# points = np.random.random((10000, 3)) *10
gl.glPointSize(2)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawLine(np.array([[0, 0, 0], [1.0, 0.0, 0.0]]))
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawLine(np.array([[0, 0, 0], [0.0, 1.0, 0.0]]))
gl.glColor3f(.0, 0.0, 1.0)
pangolin.DrawLine(np.array([[0, 0, 0], [0.0, 0.0, 1.0]]))
origin = [[0, 0, 0]]
originHomo = h**o(np.zeros((3, 1)))
originT = np.eye(4)
# pangolin.Draw
index = 0
for T in poseList_nx4x4:
originT = originT.dot(T)
if drawCamera:
gl.glColor3f(0.5, 0.75, 0.8)
pangolin.DrawCamera(originT)
pAfter = originT.dot(originHomo)
p = unHomo(pAfter)
origin.append([p[0][0], p[1][0], p[2][0]])
# line
if np.mod(index, 2) == 0:
gl.glColor3f(0.0, 0.0, 0.0)
else:
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawLine(origin)
index += 1
# time.sleep(0.01)
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 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 draw_line(self, points, color=(1.0, 0.0, 0.0)):
self.dcam.Activate(self.scam)
gl.glLineWidth(2)
gl.glColor3f(*color)
pango.DrawLine(points)
def viewer_refresh(self, qmap, qvo, is_paused):
while not qmap.empty():
self.map_state = qmap.get()
while not qvo.empty():
self.vo_state = qvo.get()
# if pangolin.Pushed(self.button):
# print('You Pushed a button!')
self.do_follow = self.checkboxFollow.Get()
self.is_grid = self.checkboxGrid.Get()
self.draw_cameras = self.checkboxCams.Get()
self.draw_covisibility = self.checkboxCovisibility.Get()
self.draw_spanning_tree = self.checkboxSpanningTree.Get()
#if pangolin.Pushed(self.checkboxPause):
if self.checkboxPause.Get():
is_paused.value = 0
else:
is_paused.value = 1
# self.int_slider.SetVal(int(self.float_slider))
self.pointSize = self.int_slider.Get()
if self.do_follow and self.is_following:
self.scam.Follow(self.Twc, True)
elif self.do_follow and not self.is_following:
self.scam.SetModelViewMatrix(self.look_view)
self.scam.Follow(self.Twc, True)
self.is_following = True
elif not self.do_follow and self.is_following:
self.is_following = False
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)
if self.is_grid:
Viewer3D.drawPlane()
# ==============================
# draw map
if self.map_state is not None:
if self.map_state.cur_pose is not None:
# draw current pose in blue
gl.glColor3f(0.0, 0.0, 1.0)
gl.glLineWidth(2)
pangolin.DrawCamera(self.map_state.cur_pose)
gl.glLineWidth(1)
self.updateTwc(self.map_state.cur_pose)
if self.map_state.predicted_pose is not None and kDrawCameraPrediction:
# draw predicted pose in red
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawCamera(self.map_state.predicted_pose)
if len(self.map_state.poses) >1:
# draw keyframe poses in green
if self.draw_cameras:
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(self.map_state.poses[:])
if len(self.map_state.points)>0:
# draw keypoints with their color
gl.glPointSize(self.pointSize)
#gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(self.map_state.points, self.map_state.colors)
if self.map_state.reference_pose is not None and kDrawReferenceCamera:
# draw predicted pose in purple
gl.glColor3f(0.5, 0.0, 0.5)
gl.glLineWidth(2)
pangolin.DrawCamera(self.map_state.reference_pose)
gl.glLineWidth(1)
if len(self.map_state.covisibility_graph)>0:
if self.draw_covisibility:
gl.glLineWidth(1)
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawLines(self.map_state.covisibility_graph,3)
if len(self.map_state.spanning_tree)>0:
if self.draw_spanning_tree:
gl.glLineWidth(1)
gl.glColor3f(0.0, 0.0, 1.0)
pangolin.DrawLines(self.map_state.spanning_tree,3)
if len(self.map_state.loops)>0:
if self.draw_spanning_tree:
gl.glLineWidth(2)
gl.glColor3f(0.5, 0.0, 0.5)
pangolin.DrawLines(self.map_state.loops,3)
gl.glLineWidth(1)
# ==============================
# draw vo
if self.vo_state is not None:
if self.vo_state.poses.shape[0] >= 2:
# draw poses in green
if self.draw_cameras:
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(self.vo_state.poses[:-1])
if self.vo_state.poses.shape[0] >= 1:
# draw current pose in blue
gl.glColor3f(0.0, 0.0, 1.0)
current_pose = self.vo_state.poses[-1:]
pangolin.DrawCameras(current_pose)
self.updateTwc(current_pose[0])
if self.vo_state.traj3d_est.shape[0] != 0:
# draw blue estimated trajectory
gl.glPointSize(self.pointSize)
gl.glColor3f(0.0, 0.0, 1.0)
pangolin.DrawLine(self.vo_state.traj3d_est)
if self.vo_state.traj3d_gt.shape[0] != 0:
# draw red ground-truth trajectory
gl.glPointSize(self.pointSize)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawLine(self.vo_state.traj3d_gt)
pangolin.FinishFrame()
def viewer_refresh(self, qmap, qvo):
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(1.0, 1.0, 1.0, 1.0)
while not qmap.empty():
self.state_map = qmap.get()
while not qvo.empty():
self.state_vo = qvo.get()
# if pangolin.Pushed(self.button):
# print('You Pushed a button!')
self.do_follow = self.checkboxFollow.Get()
self.is_grid = self.checkboxGrid.Get()
self.draw_cameras = self.checkboxCams.Get()
# self.int_slider.SetVal(int(self.float_slider))
self.pointSize = self.int_slider.Get()
self.dcam.Activate(self.scam)
if self.do_follow:
self.scam.Follow(self.Twc, True)
if self.is_grid:
Viewer3D.drawPlane()
# draw map
if self.state_map is not None:
if self.state_map[0].shape[0] >= 2:
# draw poses in green
if self.draw_cameras:
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(self.state_map[0][:-1])
if self.state_map[0].shape[0] >= 1:
# draw current pose in blue
gl.glColor3f(0.0, 0.0, 1.0)
currentPose = self.state_map[0][-1:]
pangolin.DrawCameras(currentPose)
self.updateTwc(currentPose)
if self.state_map[1].shape[0] != 0:
# draw keypoints with their color
gl.glPointSize(self.pointSize)
#gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(self.state_map[1], self.state_map[2])
# draw vo
if self.state_vo is not None:
if self.state_vo[0].shape[0] >= 2:
# draw poses in green
if self.draw_cameras:
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(self.state_vo[0][:-1])
if self.state_vo[0].shape[0] >= 1:
# draw current pose in blue
gl.glColor3f(0.0, 0.0, 1.0)
currentPose = self.state_vo[0][-1:]
pangolin.DrawCameras(currentPose)
self.updateTwc(currentPose)
if self.state_vo[1].shape[0] != 0:
# draw blue estimated trajectory
gl.glPointSize(self.pointSize)
gl.glColor3f(0.0, 0.0, 1.0)
pangolin.DrawLine(self.state_vo[1])
if self.state_vo[2].shape[0] != 0:
# draw red ground-truth trajectory
gl.glPointSize(self.pointSize)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawLine(self.state_vo[2])
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 = 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 viewer_refresh(self, qmap, qvo, is_paused):
while not qmap.empty():
self.map_state = qmap.get()
while not qvo.empty():
self.vo_state = qvo.get()
# if pangolin.Pushed(self.button):
# print('You Pushed a button!')
self.do_follow = self.checkboxFollow.Get()
self.is_grid = self.checkboxGrid.Get()
self.draw_cameras = self.checkboxCams.Get()
self.draw_covisibility = self.checkboxCovisibility.Get()
self.draw_spanning_tree = self.checkboxSpanningTree.Get()
#if pangolin.Pushed(self.checkboxPause):
if self.checkboxPause.Get():
is_paused.value = 0
else:
is_paused.value = 1
# self.int_slider.SetVal(int(self.float_slider))
self.pointSize = self.int_slider.Get()
if self.do_follow and self.is_following:
self.scam.Follow(self.Twc, True)
elif self.do_follow and not self.is_following:
self.scam.SetModelViewMatrix(self.look_view)
self.scam.Follow(self.Twc, True)
self.is_following = True
elif not self.do_follow and self.is_following:
self.is_following = False
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)
if self.is_grid:
Viewer3D.drawPlane()
# ==============================
# draw map
if self.map_state is not None:
if self.map_state.cur_pose is not None:
# draw current pose in blue
gl.glColor3f(0.0, 0.0, 1.0)
gl.glLineWidth(2)
pangolin.DrawCamera(self.map_state.cur_pose)
gl.glLineWidth(1)
self.updateTwc(self.map_state.cur_pose)
if self.map_state.predicted_pose is not None and kDrawCameraPrediction:
# draw predicted pose in red
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawCamera(self.map_state.predicted_pose)
if len(self.map_state.poses) > 1:
# draw keyframe poses in green
if self.draw_cameras:
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(self.map_state.poses[:])
# rysowaine wektora za którym powinno wyznaczać boxy
if len(self.map_state.line) > 0:
# # wyznaczam nowy wektor obrócony o -90 i 90 stopni o długości 10
gl.glLineWidth(3)
gl.glPointSize(self.pointSize)
gl.glColor3f(0.0, 0.0, 0.0)
pangolin.DrawPoints(self.map_state.line)
# # pangolin.DrawLines([self.map_state.line[0], self.map_state.line[0]])
if len(self.map_state.points) > 0:
# draw keypoints with their color
gl.glPointSize(self.pointSize)
#gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(self.map_state.points,
self.map_state.colors)
# TODO dopisać rozwiązanie drukujące na ekran zaznaczone obiekty w postaci obszarów
if len(self.map_state.dict_pose) > 0:
for k, v in self.map_state.dict_pose.items():
sizes = []
poses = [np.identity(4) for i in range(len(v))]
for pose, point in zip(poses, v):
pose[:3, 3] = np.array(point[0])
sizes.append(np.array(point[1]))
gl.glLineWidth(2)
d_color = k[1:-1].split(' ')
gl.glColor3f(float(d_color[0]), float(d_color[1]),
float(d_color[2]))
# gl.glColor3f(1.0, 0.0, 1.0)
# gl.glColor3f(self.map_state.box_color)
# print("{} {}".format(poses, sizes))
pangolin.DrawBoxes(poses, sizes)
# pangolin.DrawBoxes(self.map_state.box_left_botton, self.map_state.box_size)
if self.map_state.reference_pose is not None and kDrawReferenceCamera:
# draw predicted pose in purple
gl.glColor3f(0.5, 0.0, 0.5)
gl.glLineWidth(2)
pangolin.DrawCamera(self.map_state.reference_pose)
gl.glLineWidth(1)
if len(self.map_state.covisibility_graph) > 0:
if self.draw_covisibility:
gl.glLineWidth(1)
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawLines(self.map_state.covisibility_graph, 3)
if len(self.map_state.spanning_tree) > 0:
if self.draw_spanning_tree:
gl.glLineWidth(1)
gl.glColor3f(0.0, 0.0, 1.0)
pangolin.DrawLines(self.map_state.spanning_tree, 3)
if len(self.map_state.loops) > 0:
if self.draw_spanning_tree:
gl.glLineWidth(2)
gl.glColor3f(0.5, 0.0, 0.5)
pangolin.DrawLines(self.map_state.loops, 3)
gl.glLineWidth(1)
# ==============================
# draw vo
if self.vo_state is not None:
if self.vo_state.poses.shape[0] >= 2:
# draw poses in green
if self.draw_cameras:
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(self.vo_state.poses[:-1])
if self.vo_state.poses.shape[0] >= 1:
# draw current pose in blue
gl.glColor3f(0.0, 0.0, 1.0)
current_pose = self.vo_state.poses[-1:]
pangolin.DrawCameras(current_pose)
self.updateTwc(current_pose[0])
if self.vo_state.traj3d_est.shape[0] != 0:
# draw blue estimated trajectory
gl.glPointSize(self.pointSize)
gl.glColor3f(0.0, 0.0, 1.0)
pangolin.DrawLine(self.vo_state.traj3d_est)
if self.vo_state.traj3d_gt.shape[0] != 0:
# draw red ground-truth trajectory
gl.glPointSize(self.pointSize)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawLine(self.vo_state.traj3d_gt)
pangolin.FinishFrame()