'color': io.imread(files_rgb_left[id]),
'depth': np.load(files_depth_left[id]),
'transform': poses_mat44[id]
}
### ------ VIZ ------ ###
config_viz = {
'width': 1024,
'height': 768,
'zoom': 0.5,
'front': [0, -1, -.25],
'lookat': [0, 0, 0],
'up': [0, 0, -1]
}
viz_base = [
getVisualizationBB(),
TriangleMesh.create_coordinate_frame(size=1.0, origin=[0, 0, 0])
]
viz_cam = [
getKeyframe(transform=frame0['transform'], color=[1, 0, 0]),
getKeyframe(transform=frame1['transform'], color=[1, 0, 0])
]
viz = []
viz.extend(viz_base)
viz.extend(viz_cam)
# draw_geometries(viz, **config_viz)
# for pose in poses_mat44:
# viz_cam.append(getKeyframe(transform=pose,color=[1,0,0]))
path = getDataSequences(root=rootDIR,scenario='office', level='Easy', seq_num=4)
files_rgb_left, files_rgb_right, files_depth_left, poselist = getDataLists(dir=path, skip=10)
assert (poselist.shape[1] == 7) # x-y-z qx-qy-qz-qw
poses_mat34 = pos_quats2SEs(poselist) # [R|t]
poses_mat44 = pos_quats2SE_matrices(poselist)
motions_mat = pose2motion(poses_mat34) # [R|t]
motions_quat = SEs2ses(motions_mat).astype(np.float32) # x-y-z qx-qy-qz-qw
vis = o3d.visualization.Visualizer()
vis.create_window(window_name='Trajectory', width=1024, height=768)
axis_pcd = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0,0,0])
vis.add_geometry(axis_pcd)
pose_axis = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.25, origin=[0,0,0])
line_set = getVisualizationBB()
vis.add_geometry(line_set)
pointSet = o3d.geometry.PointCloud()
vis.add_geometry(pointSet)
num_keyframes = len(files_rgb_left)
for id in range(num_keyframes):
pose = poselist[id]
pointSet.points.append([pose[0], pose[1], pose[2]])
camFrame = getKeyframe(transform=poses_mat44[id])
vis.add_geometry(camFrame)
vis.update_geometry(pointSet)
vis.update_geometry(camFrame)
# ----- get cloud -----
# img_cloud = PointCloud.create_from_rgbd_image(image=img_rgbd,
# intrinsic=cameraIntrinsic,
# extrinsic=T)
# img_cloud.transform(img_trans)
# ds_cloud = img_cloud.voxel_down_sample(voxel_size=0.01)
# o3d.visualization.draw_geometries([img_cloud])
# ----- GET SOBEL MASKS =)) -----
# img_gray = cv.cvtColor(img_color,cv.COLOR_RGB2GRAY)
# img_pcd_mask = getSobelMask(img_gray,thesh_sobel)
# io.imshow(img_mask); io.show()
# ====================================================
#3D VISUALIZATION
vizBB = getVisualizationBB()
pointSet = o3d.geometry.PointCloud()
blob3D = o3d.geometry.PointCloud()
global_pcd = o3d.geometry.PointCloud()
vis = o3d.visualization.VisualizerWithKeyCallback()
vis.create_window(window_name='Trajectory', width=1024, height=768)
axis_pcd = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0,
origin=[0, 0, 0])
vis.add_geometry(vizBB)
vis.add_geometry(axis_pcd)
vis.add_geometry(blob3D)
vis.poll_events()
vis.update_renderer()