def viewer_refresh(self, q):
#turn state into points
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(0.0, 0.0, 0.0, 1.0)
self.d_cam.Activate(self.scam)
# draw poses
# colors = np.zeros((len(ppts), 3))
# colors[:, 0] = 1
# colors[:, 1] = 1
# colors[:, 2] = 0
gl.glPointSize(10)
gl.glColor3f(0.0, 0.0, 1.0)
# pango.DrawPoints(ppts, colors)
# pango.DrawPoints(self.state[0], self.state[2])
pango.DrawCameras(self.state[0])
# # draw keypoints
# colors = np.zeros((len(spts), 3))
# colors[:, 0] = 0
# colors[:, 1] = 1
# colors[:, 2] = 0
gl.glPointSize(2)
gl.glColor3f(0.0, 1.0, 0.0)
pango.DrawPoints(self.state[1], self.state[2])
pango.FinishFrame()
def viewer_refresh(self):
if self.state == None:
return
# turn state into points
ppts = np.array([d[:3, 3] for d in self.state[0]])
spts = np.array(self.state[1])
print(ppts)
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 viewer_refresh(self, q):
if self.state is None or not q.empty():
self.state = q.get()
# turn state into points
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(0, 0, 0, 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(1.0, 1.0, 1.0)
pangolin.DrawPoints(spts)
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, 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
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 update(self,obs,ego_grid,opp_grid,op_all_states,op_picked_state,ego_all_states,ego_picked_state,is_corner,ego_xy_grid):
gl.glClear(gl.GL_COLOR_BUFFER_BIT|gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(37/255,37/255,38/255,1.0)
self.dcam.Activate(self.scam)
ego_x=obs['poses_x'][0]
ego_y=obs['poses_y'][0]
ego_theta=obs['poses_theta'][0]
op_x=obs['poses_x'][1]
op_y=obs['poses_y'][1]
op_theta=obs['poses_theta'][1]
ego_pose=transformations.rotation_matrix(ego_theta,(0,0,1))
ego_pose[0,3]=ego_x
ego_pose[1,3]=ego_y
op_pose=transformations.rotation_matrix(op_theta,(0,0,1))
op_pose[0,3]=op_x
op_pose[1,3]=op_y
ego_size=np.array([0.58,0.31,0.1])
op_size=np.array([0.58,0.31,0.1])
gl.glLineWidth(1)
gl.glColor3f(1.0,1.0,1.0)
pangolin.DrawBoxes([ego_pose],[ego_size])
gl.glColor(231/256.,34/256.,46/256.)
pangolin.DrawBoxes([op_pose],[op_size])
gl.glPointSize(2)
gl.glColor3f(0.2,0.2,0.2)
pangolin.DrawPoints(self.map_points)
gl.glPointSize(2)
gl.glColor3f(0.0,0.5,1.0)
if ego_xy_grid is None:
pangolin.FinishFrame()
return
gl.glPointSize(2)
gl.glColor3f(0.0,0.5,1.0)
rot=np.array([[np.cos(ego_theta),np.sin(ego_theta)],[-np.sin(ego_theta),np.cos(ego_theta)]])
xy_grid=np.dot(ego_xy_grid[:,:2],rot)
temp=np.hstack([xy_grid,np.zeros((xy_grid.shape[0],1))])
if self.show_laser:
rot_mat=transformations.rotation_matrix(ego_theta,(0,0,1))
ego_scan=obs['scans'][0]
ego_scan=np.asarray(ego_scan)
ego_scan_x=np.multiply(ego_scan,np.sin(self.scan_angles))
ego_scan_y=np.multiply(ego_scan,np.cos(self.scan_angles))
ego_scan_arr=np.zeros((ego_scan_x.shape[0],3))
ego_scan_arr[:,0]=ego_scan_y
ego_scan_arr[:,1]=ego_scan_x
ego_scan_arr=np.dot(rot_mat[0:3,0:3],ego_scan_arr.T)
ego_scan_arr=ego_scan_arr+np.array([[ego_x],[ego_y],[0]])
gl.glPointSize(1)
gl.glColor3f(1.0,0.0,0.0)
pangolin.DrawPoints(ego_scan_arr.T)
if ego_all_states is not None:
gl.glPointSize(2)
gl.glColor3f(0.8,0.0,0.5)
if op_picked_state is not None:
gl.glPointSize(3)
if op_all_states is None:
gl.glColor3f(231/256.,34/256.,46/256.)
else:
gl.glColor3f(231/256.,34/256.,46/256.)
pangolin.DrawPoints(np.hstack([op_picked_state[:,0:2],np.zeros((op_picked_state.shape[0],1))]))
if ego_picked_state is not None:
gl.glPointSize(5)
if ego_all_states is None:
gl.glColor3f(1.,1.,1.)
else:
gl.glColor3f(1.,1.,1.)
pangolin.DrawPoints(np.hstack([ego_picked_state[:,0:2],np.zeros((ego_picked_state.shape[0],1))]))
gl.glPointSize(2)
gl.glColor3f(22/256.,88/256.,142/256.)
pangolin.DrawPoints(self.waypoints_plot)
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, 1, 0, 1, 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 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)
print(trajectory.shape)
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)
drawPlane()
# 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 update(self, obs, ego_all_states, ego_picked_state, ego_xy_grid):
# clear buffer
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(37 / 255, 37 / 255, 38 / 255, 1.0)
self.dcam.Activate(self.scam)
# grab observations
# ignoring scans for now
# car poses
ego_x = obs['poses_x'][0]
ego_y = obs['poses_y'][0]
ego_theta = obs['poses_theta'][0]
# print('ego', ego_x, ego_y, ego_theta)
# print('opp', op_x, op_y, op_theta)
# Draw boxes for agents
# ego_pose = np.identity(4)
ego_pose = transformations.rotation_matrix(ego_theta, (0, 0, 1))
ego_pose[0, 3] = ego_x
ego_pose[1, 3] = ego_y
# ego_pose[2, 3] = 0.1
# op_pose = np.identity(4)
ego_size = np.array([0.58, 0.31, 0.1])
gl.glLineWidth(1)
# ego is blue-ish
gl.glColor3f(0.0, 0.5, 1.0)
pangolin.DrawBoxes([ego_pose], [ego_size])
# Draw map
gl.glPointSize(2)
gl.glColor3f(0.2, 0.2, 0.2)
pangolin.DrawPoints(self.map_points)
# draw waypoints
gl.glPointSize(2)
gl.glColor3f(0.3, 0.3, 0.3)
pangolin.DrawPoints(self.waypoints_plot)
# draw flow samples
# ego
gl.glPointSize(2)
gl.glColor3f(0.0, 0.5, 1.0)
# print('hello \n')
# print('st', ego_grid[0:5])
if ego_xy_grid is None:
pangolin.FinishFrame()
return
gl.glPointSize(2)
gl.glColor3f(0.0, 0.5, 1.0)
# print('xytheta', ego_xythetas[0:5])
rot = np.array([[np.cos(ego_theta),
np.sin(ego_theta)],
[-np.sin(ego_theta),
np.cos(ego_theta)]])
xy_grid = np.dot(ego_xy_grid[:, :2], rot)
temp = np.hstack([xy_grid, np.zeros((xy_grid.shape[0], 1))])
# print('xyz', ego_xythetas[0:5])
pangolin.DrawPoints(temp + np.array([ego_x, ego_y, 0.0])[None, :])
# opp
# Draw laser scans
# Red for ego, Blue for op
# Could be turned off
if self.show_laser:
rot_mat = transformations.rotation_matrix(ego_theta, (0, 0, 1))
ego_scan = obs['scans'][0]
ego_scan = np.asarray(ego_scan)
ego_scan_x = np.multiply(ego_scan, np.sin(self.scan_angles))
ego_scan_y = np.multiply(ego_scan, np.cos(self.scan_angles))
ego_scan_arr = np.zeros((ego_scan_x.shape[0], 3))
ego_scan_arr[:, 0] = ego_scan_y
ego_scan_arr[:, 1] = ego_scan_x
ego_scan_arr = np.dot(rot_mat[0:3, 0:3], ego_scan_arr.T)
ego_scan_arr = ego_scan_arr + np.array([[ego_x], [ego_y], [0]])
gl.glPointSize(1)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(ego_scan_arr.T)
# Draw splines
if ego_all_states is not None:
gl.glPointSize(1)
gl.glColor3f(0.8, 0.0, 0.5)
# print('num traj', ego_all_states.shape[0]/100)
pangolin.DrawPoints(
np.hstack([
ego_all_states[:, 0:2],
np.zeros((ego_all_states.shape[0], 1))
]))
if ego_picked_state is not None:
gl.glPointSize(3)
if ego_all_states is None:
gl.glColor3f(1., 0., 0.)
else:
gl.glColor3f(1., 1., 1.)
pangolin.DrawPoints(
np.hstack([
ego_picked_state[:, 0:2],
np.zeros((ego_picked_state.shape[0], 1))
]))
# render
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[:])
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()