def main(self, pts=[], colors=None, camera=None):
if True:
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(1.0, 0.5, 0.5, 1.0)
self.dcam.Activate(self.scam)
# Render OpenGL Cube
# pangolin.glDrawColouredCube(0.1)
# Draw Point Cloud
gl.glColor3f(1.0, 1.0, 0.0)
# pangolin.DrawPoints(points)
if len(pts) > 0:
gl.glPointSize(3)
if isinstance(colors, np.ndarray):
pangolin.DrawPoints(pts, colors)
else:
pangolin.DrawPoints(pts)
if isinstance(camera, list):
self.draw_cameras(camera)
# gl.glColor3f(0.0, 0.0, 0.0)
# gl.glPointSize(1)
# pangolin.DrawPoints(pts2)
pangolin.FinishFrame()
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 draw(self):
q = self.q
self.draw_init()
while not pangolin.ShouldQuit():
try:
while not q.empty():
state = q.get()
if state is not None:
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.0, 0.0, 0.0, 0.0)
self.dcam.Activate(self.scam)
# Draw Point Cloud
gl.glPointSize(2)
gl.glColor3f(0.0, 1.0, 0.0)
if self.history:
self.points.append(state[0])
pangolin.DrawPoints(
np.reshape(np.array(self.points), (-1, 3)))
else:
pangolin.DrawPoints(state[0])
pangolin.FinishFrame()
except:
continue
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)
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)
# 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(5)
pangolin.DrawPoints(points, colors)
pangolin.FinishFrame()
def refresh(self, q):
"""Refresh the display if there is new data
:q: TODO
:returns: TODO
"""
while not q.empty():
self.message = q.get()
if self.message is not None:
map_points, poses, colors = self.message
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.0, 0.0, 0.0, 1.0)
self.dcam.Activate(self.scam)
if poses is not None:
if poses.shape[0] >= 2:
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(poses[0:-1, :])
gl.glColor3f(1.0, 1.0, 0.0)
pangolin.DrawCameras(poses[-1:, :])
if map_points is not None and colors is not None:
if map_points.shape[0] != 0:
gl.glPointSize(3)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(map_points, colors)
pangolin.FinishFrame()
def viewer_refresh(self, q):
while not q.empty():
self.points_to_draw, self.point_colors, self.poses_to_draw = q.get(
)
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.0, 0.0, 0.0, 1.0)
self.dcam.Activate(self.scam)
#pangolin.glDrawColouredCube(1)
# Draw feature points with red dots.
gl.glPointSize(4)
gl.glColor3f(1.0, 0.0, 0.0)
if self.points_to_draw is not None:
pangolin.DrawPoints(self.points_to_draw, self.point_colors)
# Test of drawing manual points.
#gl.glColor3f(1.0, 1.0, 1.0)
#pangolin.DrawPoints(np.array([[0, 0, 0], [10, 0, 0], [0, 20, 0], [0, 0, 40]]))
gl.glColor3f(1.0, 1.0, 1.0)
if self.poses_to_draw is not None:
for pose in self.poses_to_draw:
pangolin.DrawCamera(pose, 1, 0.5, 0.8)
pangolin.FinishFrame()
return
def viewer_refresh(self, q):
while not q.empty():
self.state = q.get()
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.0, 0.0, 0.0, 1.0)
self.dcam.Activate(self.scam)
if self.state is not None:
if self.state[0].shape[0] >= 2:
# draw poses
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(self.state[0][:-1])
if self.state[0].shape[0] >= 1:
# draw current pose as yellow
gl.glColor3f(1.0, 1.0, 0.0)
pangolin.DrawCameras(self.state[0][-1:])
if self.state[1].shape[0] != 0:
# draw keypoints
gl.glPointSize(5)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(self.state[1], self.state[2])
pangolin.FinishFrame()
def viewer_refresh(self, q):
while not q.empty():
self.data = q.get()
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.0, 0.0, 0.0, 1.0)
self.dcam.Activate(self.scam)
# drawing begins here
# self.pose[2, 3] = self.pose[2, 3] - 0.1
# self.poses.append(np.linalg.inv(self.pose))
if self.data is not None:
gl.glLineWidth(3)
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(self.data[0])
if len(self.data) > 1:
gl.glPointSize(5)
points = self.data[1]
colors = self.data[2]
pangolin.DrawPoints(points, colors)
pangolin.FinishFrame()
def DISPLAY():
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)
pangolin.FinishFrame()
def main(showSemSegmentation=False):
# create window
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, 50),
pangolin.ModelViewLookAt(0, 0, -.01, 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, colors, sem = getPointsAndColors("000020.png")
# If we want to show the semantic segmentations
if showSemSegmentation:
colors = colors * 0.5 + sem * 0.5
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.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 main():
pango.CreateWindowAndBind('point cloud cube render', 640, 480)
# Projection and ModelView Matrices
scam = pango.OpenGlRenderState(
pango.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2, 100),
pango.ModelViewLookAt(-1, -1, -1, 0, 0, 0, pango.AxisDirection.AxisY))
handler = pango.Handler3D(scam)
# Interactive View in Window
disp_cam = pango.CreateDisplay()
disp_cam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0)
disp_cam.SetHandler(handler)
# Create a random point cloud
pts = np.random.random((100, 3)) * 10
# Color matrix based on point location
colors = np.zeros((len(pts), 3))
colors[:, :] = 1. - pts[:, :] / 10
#colors[:] = [1.0, 0.0, 0.0]
while not pango.ShouldQuit():
# Clear screen
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.15, 0.15, 0.15, 0.0)
disp_cam.Activate(scam)
# Draw Points
gl.glPointSize(5)
gl.glColor3f(0.0, 1.0, 0.0)
pango.DrawPoints(pts, colors)
# Finish Drawing
pango.FinishFrame()
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()
def run(self):
pangolin.CreateWindowAndBind(self.win_name, 640, 480)
gl.glEnable(gl.GL_DEPTH_TEST)
self.scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2, 1000),
pangolin.ModelViewLookAt(2, -10, -30, 0, 0, 0, pangolin.AxisNegY))
self.tree = pangolin.Renderable()
self.tree.Add(pangolin.Axis())
self.handler = pangolin.SceneHandler(self.tree, self.scam)
self.dcam = pangolin.CreateDisplay()
self.dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0)
self.dcam.SetHandler(self.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)
self.dcam.Activate(self.scam)
self.tree.Render()
self.data_lock.acquire()
if self.poses:
gl.glPointSize(5)
gl.glColor3f(*self.cam_color)
for pose in self.poses:
pangolin.DrawPoints(pose[1].T)
if self.map_pts is not None:
gl.glPointSize(2)
gl.glColor3f(*self.map_pts_color)
pangolin.DrawPoints(self.map_pts)
if self.new_pts_obs is not None:
cur_pos = self.poses[-1]
cam_centers = np.repeat(cur_pos[1].T,
self.new_pts_obs.shape[0],
axis=0)
gl.glLineWidth(1)
gl.glColor3f(0.0, 0.0, 1.0)
pangolin.DrawLines(cam_centers, self.new_pts_obs)
self.data_lock.release()
time.sleep(0.02)
pangolin.FinishFrame()
def drawPoints(self, pointCloud):
'''
Function to allow chosen point cloud to be drawn
'''
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
SampleToggling.dcam.Activate(SampleToggling.scam)
pangolin.DrawPoints(pointCloud)
pangolin.FinishFrame()
def handler_pts(pts, attrs):
if pts is None or len(pts) == 0:
return
if attrs:
gl.glColor3f(*attrs['Color'])
gl.glPointSize(attrs['PointSize'])
else:
gl.glColor3f(0.0, 1.0, 0.0)
gl.glPointSize(2)
pangolin.DrawPoints(pts)
def viewer_refresh(self, q):
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(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 draw_axes():
gl.glPointSize(1)
points = np.zeros((300, 3))
points[:100, 0] = np.arange(100)
points[100:200, 1] = np.arange(100)
points[200:300, 2] = np.arange(100)
colors = np.zeros((300, 3))
colors[:100, 0] = 1
colors[100:200, 1] = 1
colors[200:300, 2] = 1
pangolin.DrawPoints(points, colors)
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 viewer_refresh(self, q):
if self.state is None or not q.empty():
self.state = q.get()
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.glLineWidth(1)
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(self.state[0])
gl.glPointSize(2)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(self.state[1])
pangolin.FinishFrame()
def draw3d(self, out, cam_loc, vis_pose_check):
# object points
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(1, 1, 1, 0)
self.dcam.Activate(self.scam)
# WORLD
gl.glPointSize(3)
gl.glColor3f(1.0, 0, 0)
pangolin.DrawPoints(out)
#POSE
if vis_pose_check:
for cam_loc1 in cam_loc:
# cam pose
gl.glColor3f(0, 1.0, 0)
pose = np.identity(4)
pose[:3, 3] = cam_loc1 #loc
pangolin.DrawCamera(pose, 0.6, 0.4, 0.8)
pangolin.FinishFrame()
def viewer_refresh(self, q):
if self.state is None or not q.empty():
self.state = q.get()
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
# gl.glClearColor(1.0, 1.0, 1.0, 1.0)
gl.glClearColor(0.0, 0.0, 0.0, 1.0)
self.dcam.Activate(self.scam)
# draw poses
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(self.state[0])
# draw keypoints
gl.glPointSize(5)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(self.state[1], self.state[2])
pangolin.FinishFrame()
def render_map(self, q):
if (self.currState is None) or (not q.empty()):
self.currState = q.get()
# Extract points and poses
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 poses
#gl.glPointSize(10)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawCameras(self.currState[0])
# Draw points
gl.glPointSize(2)
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawPoints(self.currState[1])
pangolin.FinishFrame()
def cloud_for_vis(img,depth):
index_matrix_2[:,2] = np.reshape(depth,(480*640))
points_in_cam_frame = index_matrix_2
#point_in_cam_frame = np.transpose(depth*index_matrix_2) # 3x480*640
points_in_world = np.matmul(camera_matrix_inv,points_in_cam_frame.T) # 3x480*640
points_in_world = np.transpose(points_in_world) # 480*640x3
points_colours = np.reshape(img,(480*640,3)) # RGB values for each point
"""fig = plt.figure()
ax = Axes3D(fig)
#ax.scatter([2,4,5,2],[3,4,2,1],[6,3,4,1])
ax.scatter(points_in_world[:,0],points_in_world[:,1],points_in_world[:,2])
plt.show()"""
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 = points_in_world#np.random.random((100000, 3)) * 10
gl.glPointSize(2)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(points)
pangolin.FinishFrame()
def viewer_refresh(self, q):
if self.state == None or not q.empty():
self.state = q.get()
# turn state into points, np.asarray() keep all 'd' as array, not matrix
# ppts = np.array([np.asarray(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(1.0, 1.0, 1.0, 1.0)
self.dcam.Activate(self.scam)
# draw pose
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(self.state[0])
# draw key points
gl.glPointSize(2)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(self.state[1])
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)
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 viewer_refresh(self, q):
if self.state is None or not q.empty():
self.state = q.get()
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0.0, 0.0, 0.0, 1.0)
self.dcam.Activate(self.scam)
if self.state is not None:
if self.state[0].shape[0] >= 2:
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCameras(self.state[0][:-1])
if self.state[0].shape[0] >= 1:
gl.glColor3f(1.0, 1.0, 0.0)
pangolin.DrawCameras(self.state[0][-1:])
if self.state[1].shape[0] != 0:
gl.glPointSize(2)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(self.state[1], self.state[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 = -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 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_points(self, points, colors):
self.dcam.Activate(self.scam)
gl.glPointSize(5)
pango.DrawPoints(points, colors)
