def draw_frame(ax, frame, db, cfg):
map_frame = db.keyframe[0]
img = frame['image'].copy()
obs = db.observation[
db.observation['src_idx'] == frame['index']
]
cld = db.landmark[obs['lmk_idx']]
xyz = cld['pos']
col = cld['col']
draw_points(img, frame['feat'].pt, color=(0,255,0))
if len(xyz) > 0:
# red = projected points
pt2 = project_to_frame(xyz, map_frame, frame,
cfg['K'], cfg['D'])
img = draw_matches(img, img, pt2, obs['point'],
single=True)
#draw_points(img, pt2, color=(0,0,255) )
#draw_points(img, obs['point'], color=(255,0,0))
ax_img = sub_axis(ax, [0.0, 0.0, 0.5, 1.0])
ax_img.imshow(img[..., ::-1])
# local cloud
ax_cld = sub_axis(ax, [0.5, 0.0, 0.5, 1.0], projection='3d')
xyz = transform_cloud(xyz, map_frame, frame)
xyz = vm.tx3( vm.tx.euler_matrix(-np.pi/2, 0, -np.pi/2), xyz)
ax_cld.scatter(xyz[:,0], xyz[:,1], xyz[:,2],
c = (col[...,::-1] / 255.0))
ax_cld.view_init(elev=0, azim=180)
def main():
#src = './scan_20190212-233625.h264'
#reader = CVCameraReader(src, K=CFG['K'])
root = '/media/ssd/datasets/ADVIO'
reader = AdvioReader(root, idx=2)
# update configuration based on input
cfg = dict(CFG)
cfg['K'] = reader.meta_['K']
cfg.update(reader.meta_)
reader.set_pos(1000)
auto = True
pl = Pipeline(cfg=cfg)
cv2.namedWindow('viz', cv2.WINDOW_NORMAL)
while True:
suc, idx, stamp, img = reader.read()
#cv2.imshow('img', img)
if not suc:
break
img = cv2.resize(img, None, fx=CFG['scale'], fy=CFG['scale'])
data = {}
pl.process(img, stamp, data)
# try:
# pl.process(img, stamp, data)
# except Exception as e:
# print('Exception while processing: {}'.format(e))
# break
if 'viz' in data:
cv2.imshow('viz', data['viz'])
k = cv2.waitKey(1 if auto else 0)
if k in [27, ord('q')]:
break
if k in [ord(' ')]:
auto = (not auto)
if ('col_viz' in data) and ('cld_viz' in data):
cld = data['cld_viz']
col = data['col_viz']
# optical -> base
cld = vm.tx3(vm.tx.euler_matrix(-np.pi / 2, 0, -np.pi / 2), cld)
ax = plt.gca(projection='3d')
ax.cla()
ax.scatter(cld[:, 0],
cld[:, 1],
cld[:, 2],
c=(col[..., ::-1] / 255.0))
ax.view_init(elev=0, azim=180)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
# set_axes_equal(ax)
plt.show()
pass
pl.save('/tmp/db')
def draw_map(ax, frame, db, cfg):
# global - top-down view
# extract data
map_frame = db.keyframe[0]
xyz = db.landmark['pos']
col = db.landmark['col']
#idx = np.random.choice(len(xyz), size=2048, replace=False)
#xyz = xyz[idx]
#col = col[idx]
# draw (3d)
ax3 = sub_axis(ax, [0.0, 0.0, 1.0, 0.5], projection='3d')
ax3.scatter(xyz[:,0], xyz[:,1], xyz[:,2],
s = 0.1,
c = (col[...,::-1] / 255.0),
)
for fr in db.frame:
xfm_pose = pose_to_xfm(fr['pose'])
txn = tx.translation_from_matrix(xfm_pose)
rxn = tx.euler_from_matrix(xfm_pose)
draw_pose(ax3, txn, rxn, alpha=0.02)
draw_pose(ax3, frame['pose'][0:3], frame['pose'][9:12])
set_axes_equal(ax3)
# draw (2d)
T_R = vm.tx.euler_matrix(-np.pi/2, 0, -np.pi/2)
ax2 = sub_axis(ax, [0.0, 0.5, 1.0, 0.5])
xyz = vm.tx3(T_R, xyz)
ax2.scatter(xyz[:,0], xyz[:,1],
s = 0.1,
c = (col[...,::-1] / 255.0)
)
for fr in db.frame:
xfm_pose = pose_to_xfm(fr['pose'])
r_xfm_pose = T_R.dot(xfm_pose)
txn = tx.translation_from_matrix(r_xfm_pose)
rxn = tx.euler_from_matrix(r_xfm_pose)
draw_pose(ax2, txn, rxn, alpha=0.02)
fr = frame
xfm_pose = pose_to_xfm(fr['pose'])
r_xfm_pose = T_R.dot(xfm_pose)
txn = tx.translation_from_matrix(r_xfm_pose)
rxn = tx.euler_from_matrix(r_xfm_pose)
draw_pose(ax2, txn, rxn, alpha=1.0)
ax2.set_aspect('equal')
def cld0(_):
""" Triangulated PointCloud (view 2) """
return vm.tx3(_['P0'], _['cld1'])
def main():
root = '/media/ssd/datasets/ADVIO'
reader = AdvioReader(root, idx=15)
reader.set_pos(0)
rec = DenseRec(K=reader.meta_['K'])
imgs = []
cv2.namedWindow('viz', cv2.WINDOW_NORMAL)
h, w = reader.meta_['h'], reader.meta_['w']
pi, pj = np.random.uniform(low=(0, 0), high=(h, w),
size=(256, 2)).T.astype(np.int32)
while True:
suc, idx, stamp, img = reader.read()
if not suc:
print('Read failure : likely finished.')
break
imgs.append(img)
if len(imgs) < 2:
continue
i0 = max(0, len(imgs) - 8)
i1 = -1
data = {}
res = rec.compute(imgs[i0], imgs[i1], data=data)
viz = img
if 'viz' in data:
viz = data['viz']
# draw pointcloud
if res is not None:
cld, col = res
plt.clf()
plt.subplots_adjust(0, 0, 1, 1, 0, 0)
plt.margins(0.0)
ax = plt.subplot2grid((2, 4), (0, 0), 2, 1)
ax.imshow(imgs[i0][..., ::-1])
ax = plt.subplot2grid((2, 4), (0, 3), 1, 1)
ax.imshow(data['viz'][..., ::-1])
ax = plt.subplot2grid((2, 4), (1, 3), 1, 1)
ax.imshow(data['fimg'][..., ::-1])
ax = plt.subplot2grid((2, 4), (0, 1), 2, 2, projection='3d')
ax.cla()
cld = vm.tx3(vm.tx.euler_matrix(-np.pi / 2, 0, -np.pi / 2), cld)
ax.scatter(cld[:, 0],
cld[:, 1],
cld[:, 2],
c=(col[..., ::-1] / 255.0))
ax.view_init(elev=0, azim=180)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
set_axes_equal(ax)
plt.gcf().canvas.draw()
buf = plt.gcf().canvas.tostring_rgb()
nc, nr = plt.gcf().canvas.get_width_height()
# override 'viz'
#viz = np.fromstring(buf, dtype=np.uint8).reshape(nr,nc,3)
if viz is not None:
if res is None:
cv2.imshow('viz', viz)
else:
plt.show()
# handle key
k = cv2.waitKey(1)
if k in [27, ord('q')]:
break