def drawABox(aabb, color=None):
gl.glLineWidth(3)
if color is None:
gl.glColor3f(1., 0., 1.)
else:
gl.glColor3f(color[0], color[1], color[2])
x1, y1, z1 = aabb[0]
x2, y2, z2 = aabb[1]
w = x2 - x1
h = y2 - y1
z = z2 - z1
cx = (x1 + x2) / 2.
cy = (y1 + y2) / 2.
cz = (z1 + z2) / 2.
pose = np.identity(4)
pose[:3, 3] = np.array([cx, cy, cz])
size = [w, h, z]
pangolin.DrawBoxes(np.array([pose]), np.array([size]))
pass
def handler_bboxes3d_center(boxes_3d, attrs):
if boxes_3d is None:
return
poses = []
sizes = []
for b3d in boxes_3d:
if len(b3d) != 0:
theta = b3d[-1]
width, length, height = b3d[3], b3d[4], b3d[5]
x, y, z = b3d[0], b3d[1], b3d[2]
pose = np.identity(4)
pose[0, 0] = np.cos(theta)
pose[0, 1] = -np.sin(theta)
pose[1, 0] = np.sin(theta)
pose[1, 1] = np.cos(theta)
pose[0, 3] = x
pose[1, 3] = y
pose[2, 3] = z
poses.append(pose)
sizes.append([width, length, height])
if len(poses) > 0:
if attrs:
gl.glColor3f(*attrs['Color'])
gl.glLineWidth(attrs.get('Width', 3.0))
else:
gl.glColor3f(0.0, 0.0, 1.0)
gl.glLineWidth(3.0)
pangolin.DrawBoxes(poses, sizes)
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 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 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()