def visualize_3D(p3D):
pangolin.CreateWindowAndBind('main', 640, 480)
gl.glEnable(gl.GL_DEPTH_TEST)
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)
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)
#gl.glPointSize(5);
#gl.glColor3f(0.0,0.0,1.0);
#pangolin.DrawPoints(p3D);
# Attempt to draw camera
gl.glLineWidth(2)
gl.glColor3f(1.0, 0.0, 0.0)
#pose = np.identity(4);
pose = (np.hstack((pose1.T, np.zeros((4, 1))))).T
pangolin.DrawCamera(pose, 0.5, 0.75, 0.8)
pangolin.FinishFrame()
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)
gl.glLineWidth(3)
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCamera(np.identity(4))
# 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):
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 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 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 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():
pango.CreateWindowAndBind('frustrum render', 640, 480)
# Projection and ModelView Matrices
scam = pango.OpenGlRenderState(
pango.ProjectionMatrix(640, 480, 2000, 2000, 320, 240, 0.1, 5000),
pango.ModelViewLookAt(0, -50, -10, 0, 0, 0, 0, -1,
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 and append pose matrices for cameras
pose = np.identity(4)
poses = []
for i in range(3):
poses.append(np.linalg.inv(pose))
pose[2, 3] -= 1
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)
# Render Cameras
gl.glLineWidth(2)
gl.glColor3f(1.0, 0.0, 1.0)
pango.DrawCamera(poses[0])
gl.glColor3f(0.2, 1.0, 0.2)
pango.DrawCameras(poses[1:-1])
gl.glColor3f(1.0, 1.0, 1.0)
pango.DrawCamera(poses[-1])
# End frame update
pango.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 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 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 draw_camera(pose, color=(0.0, 1.0, 0.0)):
assert pose.shape == (4, 4)
assert len(color) == 3
width = 0.2
height_ratio = 0.75
z_ratio = 0.8
gl.glLineWidth(1)
gl.glColor3f(color[0], color[1], color[2])
# convert pose to reflect camera position with respect to world coordinate origin
# TODO wtf why doesn't this work?
# new_pose = util.rotation_translation_to_pose(util.pose_to_rotation(pose).transpose(),
# util.pose_to_translation(pose)*-1)
# do this instead of the above
new_pose = np.identity(4)
new_pose[:3, :3] = pose[:3, :3].transpose()
new_pose[:3, 3] = pose[:3, 3] * -1
pangolin.DrawCamera(new_pose, width, height_ratio, z_ratio)
def t_draw(self):
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.DrawPoints(points)
for theta in np.arange(0, np.pi, np.pi / 10):
for psi in np.arange(0, np.pi * 2, np.pi / 10):
T = makeT(Y(theta).dot(Z(psi)), np.random.random((3, 1)) * 10)
pangolin.DrawCamera(T)
time.sleep(0.4)
# pangolin.DrawCamera(np.eye(4))
pangolin.FinishFrame()
def draw_cameras(self, cam):
gl.glColor3f(0.0, 1.0, 0.0)
for c in cam:
pangolin.DrawCamera(c, 0.35, 0.75, 0.8)
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[:])
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 display(self):
points = None
W = 990
H = 540
pangolin.CreateWindowAndBind('JSLAM', W, H)
gl.glEnable(gl.GL_DEPTH_TEST)
# Define Projection and initial ModelView matrix
scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(W, H, 420, 420, 320, 240, 0.2, 100),
pangolin.ModelViewLookAt(0, 2, 15, 2, -3, -5,
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)
x = 0
y = 0
z = 0
# Perspective coordinates
xc = 0
yc = 0
zc = 0
animation_counter = 0
while not pangolin.ShouldQuit():
failed_to_load = False
try:
points = pickle.load(open("data/points.p", "rb"))
except Exception:
failed_to_load = True
pass
if not failed_to_load:
self.realPoints = []
for i, (xp, yp) in enumerate(points):
self.realPoints.append([xp, yp, z])
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glClearColor(0, 0, 0, 0)
dcam.Activate(scam)
# Draw Point Cloud
if not failed_to_load:
points = np.random.random((100000, 3)) * 10
gl.glPointSize(2)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(points)
# Load the camera
print("Animation counter: {}".format(animation_counter))
pose = np.identity(4)
pose[:3, 3] = [x, y, z]
gl.glLineWidth(2)
gl.glColor3f(0.0, 1.0, 0.0)
pangolin.DrawCamera(pose, 0.5, 0.75, 0.8)
if not self.creative_mode or animation_counter > 100:
zc += 0.1
scam = pangolin.OpenGlRenderState(
pangolin.ProjectionMatrix(W, H, 420, 420, 320, 240, 0.2,
100),
pangolin.ModelViewLookAt(0, 2, 15 + zc, 2, -3, -5,
pangolin.AxisDirection.AxisY))
handler = pangolin.Handler3D(scam)
dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0)
dcam.SetHandler(handler)
z += 0.1
animation_counter += 1
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()
def showing_mapping():
"""
DESCRIPTION: Run a 3D mapping section and get live result.
* first - only position
"""
# READ ARG.
parser = argparse.ArgumentParser()
parser.add_argument("path")
parser.add_argument("x")
parser.add_argument("y")
parser.add_argument("z")
parser.add_argument("r")
parser.add_argument("p")
parser.add_argument("y")
args = parser.parse_args()
matrice = euler_to_rotation(
float(args.r), float(args.p), float(args.y),
np.array([args.x, args.y, args.z], dtype=float))
# READ OBJ OBJECT.
scene = pywavefront.Wavefront(
'/home/thomas/Documents/rane_slam/mk3slam.0.3/Final/data/{0}.obj'.
format(args.path))
points_3D = np.zeros((len(scene.vertices), 3))
for i in range(len(scene.vertices)):
points_3D[i, :] = np.array(
[scene.vertices[i][0], scene.vertices[i][1], scene.vertices[i][2]])
# PANGOLIN CONFIGURATION & INITIALISATION.
w, h = 1280, 720
scam, dcam, dimg = pangolin_init(w, h)
texture = pangolin.GlTexture(
w,
h,
gl.GL_RGB,
False,
0,
gl.GL_RGB,
gl.GL_UNSIGNED_BYTE,
)
# ZED CONFIGURATION CAMERA.
zed = sl.Camera()
init_params = sl.InitParameters()
init_params.camera_resolution = sl.RESOLUTION.HD720 # Use HD720 video mode
init_params.coordinate_units = sl.UNIT.METER # Set coordinate units
init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP
zed.open(init_params)
# ZED CALIBRATION.
zed.get_camera_information(
).camera_configuration.calibration_parameters.left_cam
# INITIALISATION OBJECT FOR MAPPING.
pymesh = sl.Mesh() # Current incremental mesh.
image = sl.Mat() # Left image from camera.
pose = sl.Pose() # Pose object.
# TRACKING PARAMETERS.
tracking_parameters = sl.PositionalTrackingParameters()
tracking_parameters.enable_area_memory = True
tracking_parameters.area_file_path = '/home/thomas/Documents/rane_slam/mk3slam.0.3/Final/data/{0}.area'.format(
args.path)
# INITIALISATION POSITION
t = sl.Transform()
t[0, 0] = matrice[0, 0]
t[1, 0] = matrice[1, 0]
t[2, 0] = matrice[2, 0]
t[0, 1] = matrice[0, 1]
t[1, 1] = matrice[1, 1]
t[2, 1] = matrice[2, 1]
t[0, 2] = matrice[0, 2]
t[1, 2] = matrice[1, 2]
t[2, 2] = matrice[2, 2]
t[0, 3] = matrice[0, 3]
t[1, 3] = matrice[1, 3]
t[2, 3] = matrice[2, 3]
tracking_parameters.set_initial_world_transform(t)
zed.enable_positional_tracking(tracking_parameters)
# TIME PARAMETERS.
last_call = time.time()
runtime = sl.RuntimeParameters()
while not pangolin.ShouldQuit():
# -clear all.
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 coordinate origine.
pangolin_draw_coordinate()
# -get image.
zed.grab(runtime)
zed.retrieve_image(image, sl.VIEW.LEFT)
# -get position and spatial mapping state.
zed.get_position(pose)
# Display the translation and timestamp
py_translation = sl.Translation()
tx = round(pose.get_translation(py_translation).get()[0], 3)
ty = round(pose.get_translation(py_translation).get()[1], 3)
tz = round(pose.get_translation(py_translation).get()[2], 3)
print("Translation: Tx: {0}, Ty: {1}, Tz {2}, Timestamp: {3}\n".format(
tx, ty, tz, pose.timestamp.get_milliseconds()))
# Display the orientation quaternion
py_orientation = sl.Orientation()
ox = round(pose.get_orientation(py_orientation).get()[0], 3)
oy = round(pose.get_orientation(py_orientation).get()[1], 3)
oz = round(pose.get_orientation(py_orientation).get()[2], 3)
ow = round(pose.get_orientation(py_orientation).get()[3], 3)
#print("Orientation: Ox: {0}, Oy: {1}, Oz {2}, Ow: {3}\n".format(ox, oy, oz, ow))
# DRAW ALL STATE.
gl.glLineWidth(1)
gl.glColor3f(0.0, 0.0, 1.0)
# -transform brute data in numpy to draw pose.
a = pose.pose_data()
pose2 = np.array([[a[0, 0], a[0, 1], a[0, 2], a[0, 3]],
[a[1, 0], a[1, 1], a[1, 2], a[1, 3]],
[a[2, 0], a[2, 1], a[2, 2], a[2, 3]],
[a[3, 0], a[3, 1], a[3, 2], a[3, 3]]])
print(pose2)
pangolin.DrawCamera(pose2, 0.5, 0.75, 0.8)
# -draw all points on the map.
gl.glPointSize(2)
gl.glColor3f(1.0, 0.0, 0.0)
pangolin.DrawPoints(points_3D)
# DISPLAY CAMERA VIDEO.
img = cv2.cvtColor(image.get_data(), cv2.COLOR_BGRA2RGB)
img = cv2.flip(img, 0)
texture.Upload(img, gl.GL_RGB, gl.GL_UNSIGNED_BYTE)
dimg.Activate()
gl.glColor3f(1.0, 1.0, 1.0)
texture.RenderToViewport()
# END OF CYCLE.
pangolin.FinishFrame()
#sl.SPATIAL_MAPPING_STATE.NOT_ENABLED
zed.disable_positional_tracking()
zed.close()
