def rgbd2cloud(color,
depth,
pinhole_camera_intrinsic,
input_img_format="open3d"):
assert input_img_format in ["open3d", "cv", "cv2"]
'''
test case{
images_folder = "/home/feiyu/baxterws/src/winter_prj/mask_objects_from_rgbd/data/mydata"
cam_param_file="/home/feiyu/baxterws/src/winter_prj/mask_objects_from_rgbd/config/cam_params.json"
color, depth = read_color_and_depth_image(images_folder, 1)
pinhole_camera_intrinsic = open3d.io.read_pinhole_camera_intrinsic(cam_param_file)
cloud = rgbd2cloud(color, depth, pinhole_camera_intrinsic)
}
'''
if input_img_format == "cv" or input_img_format == "cv2":
rgbd_image = open3d.create_rgbd_image_from_color_and_depth(
open3d.Image(cv2.cvtColor(color, cv2.COLOR_BGR2RGB)),
open3d.Image(depth),
convert_rgb_to_intensity=False)
else:
rgbd_image = open3d.create_rgbd_image_from_color_and_depth(
color, depth, convert_rgb_to_intensity=False)
cloud = open3d.create_point_cloud_from_rgbd_image(
rgbd_image, pinhole_camera_intrinsic)
return cloud
def compute_point_cloud(self, color_img, depth_img):
rgbd_image = open3d.create_rgbd_image_from_color_and_depth(
open3d.Image(cv2.cvtColor(color_img, cv2.COLOR_BGR2RGB)),
open3d.Image(depth_img),
convert_rgb_to_intensity=False)
cloud = open3d.create_point_cloud_from_rgbd_image(
rgbd_image, self.camera_intrinsics)
return cloud
def visualise_rgbd_cloud(key_list, resolution_height, resolution_width,
device_manager, coordinate_dimentions,
transformation_devices):
# enable visualiser
align = rs.align(rs.stream.depth)
vis = o3d.Visualizer()
vis.create_window('PCD', width=1280, height=720)
pointcloud = o3d.PointCloud()
geom_added = False
voxel_size = 0.005
while True:
cloud_pcd = o3d.PointCloud()
for (serial, device) in device_manager._enabled_devices.items():
frames = device.pipeline.wait_for_frames()
frames = align.process(frames)
profile = frames.get_profile()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if not depth_frame or not color_frame:
continue
# Convert images to numpy arrays
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
img_depth = o3d.Image(depth_image)
img_color = o3d.Image(color_image)
rgbd = o3d.create_rgbd_image_from_color_and_depth(
img_color, img_depth, convert_rgb_to_intensity=False)
intrinsics = profile.as_video_stream_profile().get_intrinsics()
pinhole_camera_intrinsic = o3d.PinholeCameraIntrinsic(
intrinsics.width, intrinsics.height, intrinsics.fx,
intrinsics.fy, intrinsics.ppx, intrinsics.ppy)
pcd = o3d.create_point_cloud_from_rgbd_image(
rgbd, pinhole_camera_intrinsic)
pcd.transform(transformation_devices[serial].pose_mat)
pointcloud.points = pcd.points
pointcloud.colors = pcd.colors
cloud_pcd = cloud_pcd + pointcloud
#pcd = o3d.geometry.voxel_down_sample(cloud_pcd,
# voxel_size=voxel_size)
while True:
if geom_added == False:
vis.add_geometry(cloud_pcd)
geom_added = True
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
def pcl_from_open3d(color_image, depth_image, camera_matrix):
color_o3d = o3d.Image(color_image.astype(np.uint8))
depth_o3d = o3d.Image(depth_image.astype(np.uint8))
intrinsics = o3d.camera.PinholeCameraIntrinsic()
intrinsics.intrinsic_matrix = camera_matrix
rgbd = o3d.create_rgbd_image_from_color_and_depth(
color_o3d, depth_o3d, convert_rgb_to_intensity=False)
pcl = o3d.create_point_cloud_from_rgbd_image(image=rgbd,
intrinsic=intrinsics)
pcl.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
return pcl
def get_rgbd_image(align_color_img, depth_img, depth_scale, clipping_distance_in_meters):
align_color_img = align_color_img[:, :, 0:3] # Only get the first three channel
align_color_img = align_color_img[..., ::-1] # Convert opencv BGR to RGB
rgbd_image = open3d.create_rgbd_image_from_color_and_depth(
open3d.Image(align_color_img.copy()),
open3d.Image(depth_img),
depth_scale=1.0 / depth_scale,
depth_trunc=clipping_distance_in_meters,
convert_rgb_to_intensity=False)
# rgbd_image = open3d.geometry.create_rgbd_image_from_color_and_depth(open3d.Image(align_color_img.copy()),
# open3d.Image(depth_img),)
return rgbd_image
def get_camera_pcd(self, i):
img = self._get_image(i)
depth = self._get_depth(i)
_, H, W = img.shape
fx, fy, cx, cy = self._get_intrinsic(i)
intrinsic = PinholeCameraIntrinsic(W, H, fx, fy, cx, cy)
img = open3d.Image(img.transpose((1, 2, 0)).astype(np.uint8))
depth = open3d.Image(depth)
rgbd = create_rgbd_image_from_color_and_depth(
img, depth, depth_scale=1.0, convert_rgb_to_intensity=False)
pcd = create_point_cloud_from_rgbd_image(rgbd, intrinsic)
return pcd
def point_cloud_open3d(index, camera_matrix):
depth_image = load_depth_image(index)
depth_o3d = o3d.Image(depth_image.astype(np.uint16))
intrinsics = o3d.camera.PinholeCameraIntrinsic()
intrinsics.intrinsic_matrix = camera_matrix
pcl = o3d.create_point_cloud_from_depth_image(depth=depth_o3d,
intrinsic=intrinsics,
depth_scale=5000.0)
# pcl.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
return pcl
def create_open3d_point_cloud_from_rgbd(color_img,
depth_img,
cam_info,
depth_unit=0.001,
depth_trunc=3.0):
''' Create pointcreate_open3dpoint_cloud_from_rgbd cloud of open3d format, given opencv rgbd images and camera info.
Arguments:
color_img {np.ndarry, np.uint8}:
3 channels of BGR. Undistorted.
depth_img {np.ndarry, np.uint16}:
Undistorted depth image that matches color_img.
cam_info {CameraInfo}
depth_unit {float}:
if depth_img[i, j] is x, then the real depth is x*depth_unit meters.
depth_trunc {float}:
Depth value larger than ${depth_trunc} meters
gets truncated to 0.
Output:
open3d_point_cloud {open3d.geometry.PointCloud}
See: http://www.open3d.org/docs/release/python_api/open3d.geometry.PointCloud.html
Reference:
'''
# Create `open3d.geometry.RGBDImage` from color_img and depth_img.
# http://www.open3d.org/docs/0.7.0/python_api/open3d.geometry.create_rgbd_image_from_color_and_depth.html#open3d.geometry.create_rgbd_image_from_color_and_depth
rgbd_image = open3d.create_rgbd_image_from_color_and_depth(
color=open3d.Image(cv2.cvtColor(color_img, cv2.COLOR_BGR2RGB)),
depth=open3d.Image(depth_img),
depth_scale=1.0 / depth_unit,
convert_rgb_to_intensity=False)
# Convert camera info to `class open3d.camera.PinholeCameraIntrinsic`.
# http://www.open3d.org/docs/release/python_api/open3d.camera.PinholeCameraIntrinsic.html
pinhole_camera_intrinsic = cam_info.to_open3d_format()
# Project image pixels into 3D world points.
# Output type: `class open3d.geometry.PointCloud`.
# http://www.open3d.org/docs/0.6.0/python_api/open3d.geometry.create_point_cloud_from_rgbd_image.html#open3d.geometry.create_point_cloud_from_rgbd_image
open3d_point_cloud = open3d.create_point_cloud_from_rgbd_image(
image=rgbd_image, intrinsic=pinhole_camera_intrinsic)
return open3d_point_cloud
def pcl_from_depth_image(depth_image, camera_matrix):
far = 3.0
depth_image = depth_image[:, :, 0] / 255.0
depth_image = lerp(depth_image, 0.0, 1.0, 0.0, far)
depth_o3d = o3d.Image(depth_image.astype(np.uint8))
intrinsics = o3d.camera.PinholeCameraIntrinsic()
intrinsics.intrinsic_matrix = camera_matrix
pcl = o3d.create_point_cloud_from_depth_image(depth=depth_o3d,
intrinsic=intrinsics)
pcl.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
return pcl
def _create_point_cloud(self, color_img_resized, depth_img_resized):
''' Create point cloud from color and depth image.
Return:
pcd {open3d.geometry.PointCloud}
'''
rgbd_image = open3d.create_rgbd_image_from_color_and_depth(
color=open3d.Image(
cv2.cvtColor(color_img_resized, cv2.COLOR_BGR2RGB)),
depth=open3d.Image(depth_img_resized),
depth_scale=1.0 / self._cfg.depth_unit,
depth_trunc=self._cfg.depth_trunc,
convert_rgb_to_intensity=False)
cam_intrin = self._cam_intrin_resized.to_open3d_format()
pcd = open3d.create_point_cloud_from_rgbd_image(rgbd_image, cam_intrin)
if self._cfg.cloud_downsample_voxel_size > 0:
pcd = open3d.geometry.voxel_down_sample(
pcd, voxel_size=self._cfg.cloud_downsample_voxel_size)
return pcd
def map_texture(object_name, session_name, base_dir, models_dir,
debug_mode=False, show_textured_mesh=False,
depth_thresh_for_visibility=1e-2, depth_thresh_for_discontinuity=0.035,
max_vertex_normal_angle=70, real_depth_maps=False):
data_dir = osp.join(base_dir, session_name, object_name)
try:
with open(osp.join(data_dir, 'object_name.txt'), 'r') as f:
object_name = f.readline().rstrip()
except IOError:
print('{:s} does not have object_name.txt, skipping'.format(data_dir))
return
# read mesh file
mesh_filename = osp.join(models_dir, '{:s}.ply'.format(object_name))
mesh = open3d.read_triangle_mesh(mesh_filename)
if not mesh.has_vertex_normals():
mesh.compute_vertex_normals()
# names of views
rgb_im_dir = osp.join(data_dir, 'thermal_images')
pose_dir = osp.join(data_dir, 'poses')
names = []
for filename in os.listdir(pose_dir):
if '.txt' not in filename:
continue
if 'camera_pose' not in filename:
continue
name = '_'.join(filename.split('.')[0].split('_')[2:])
names.append(name)
names = sorted(names)
# camera extrinsics
Ts = []
for name in names:
filename = osp.join(pose_dir, 'camera_pose_{:s}.txt'.format(name))
T = np.eye(4)
with open(filename, 'r') as f:
f.readline()
T[0, 3] = float(f.readline().strip())
T[1, 3] = float(f.readline().strip())
T[2, 3] = float(f.readline().strip())
f.readline()
f.readline()
T[0, :3] = [float(v) for v in f.readline().strip().split()]
T[1, :3] = [float(v) for v in f.readline().strip().split()]
T[2, :3] = [float(v) for v in f.readline().strip().split()]
Ts.append(np.linalg.inv(T))
# RGB images
rgb_ims = []
im_intensities = []
for name in names:
rgb_im = open3d.read_image(osp.join(rgb_im_dir, '{:s}.png'.format(name)))
rgb_im = np.asarray(rgb_im)
rgb_ims.append(rgb_im)
im_shape = rgb_im.shape
intensity = rgb_im[:200, :200, 0].mean()
im_intensities.append(intensity)
target_intensity = np.mean(im_intensities)
for i, rgb_im in enumerate(rgb_ims):
rgb_im = rgb_im * target_intensity / im_intensities[i]
rgb_im = np.clip(rgb_im, a_min=0, a_max=255)
rgb_ims[i] = open3d.Image(rgb_im.astype(np.uint8))
# camera intrinsic
cinfo_filename = cinfo_manager.getPackageFileName(
'package://contactdb_utils/calibrations/boson.yaml')
cinfo = cinfo_manager.loadCalibrationFile(cinfo_filename, 'boson')
h_scaling = float(im_shape[0]) / cinfo.height
w_scaling = float(im_shape[1]) / cinfo.width
K = np.asarray(cinfo.K)
K[0] *= w_scaling
K[2] *= w_scaling
K[4] *= h_scaling
K[5] *= h_scaling
K = K.reshape((3,3))
intrinsic = open3d.PinholeCameraIntrinsic(im_shape[1], im_shape[0], K[0,0],
K[1,1], K[0,2], K[1,2])
if real_depth_maps: # use registered Kinect depthmaps
depth_im_dir = osp.join(data_dir, 'depth_images')
depth_ims = []
for name in names:
depth_im_filename = osp.join(depth_im_dir, '{:s}.png'.format(name))
depth_im = open3d.read_image(depth_im_filename)
depth_im = np.uint16(fill_holes(depth_im))
depth_ims.append(depth_im)
else: # create depth maps by rendering
depth_ims = render_depth_maps(mesh_filename, intrinsic, Ts)
# create RGB-D images
rgbds = []
for depth_im, rgb_im, T in zip(depth_ims, rgb_ims, Ts):
depth_im = open3d.Image(depth_im)
rgbds.append(open3d.create_rgbd_image_from_color_and_depth(rgb_im,
depth_im, convert_rgb_to_intensity=False))
if debug_mode:
pc = open3d.create_point_cloud_from_rgbd_image(rgbds[-1], intrinsic)
tmesh = copy(mesh)
tmesh.transform(T)
geoms = [pc]
if show_textured_mesh:
geoms.append(tmesh)
open3d.draw_geometries(geoms)
# create trajectory for texture mapping
traj = open3d.PinholeCameraTrajectory()
traj.extrinsic = open3d.Matrix4dVector(np.asarray(Ts))
traj.intrinsic = intrinsic
# do texture mapping!
option = open3d.ColorMapOptmizationOption()
option.maximum_iteration = 300
option.depth_threshold_for_visiblity_check = depth_thresh_for_visibility
option.depth_threshold_for_discontinuity_check = \
depth_thresh_for_discontinuity
option.half_dilation_kernel_size_for_discontinuity_map = 0
option.max_angle_vertex_normal_camera_ray = max_vertex_normal_angle
open3d.color_map_optimization(mesh, rgbds, traj, option)
if not debug_mode: # write result as a PLY file
mesh_filename = osp.join(data_dir, 'thermal_images',
'{:s}_textured.ply'.format(object_name))
open3d.write_triangle_mesh(mesh_filename, mesh)
print('Written {:s}'.format(mesh_filename))
if show_textured_mesh:
show_object_mesh(data_dir)
color_image1 = np.asanyarray(color_frame1.get_data())
depth_image1 = np.asanyarray(depth_frame1.get_data())
color_frame2 = aligned_frames2.get_color_frame()
depth_frame2 = aligned_frames2.get_depth_frame()
color_image2 = np.asanyarray(color_frame2.get_data())
depth_image2 = np.asanyarray(depth_frame2.get_data())
depth_image11 = np.where(
(depth_image1 > 1000) | (depth_image1 < 0), 0, depth_image1)
depth_image22 = np.where(
(depth_image2 > 1000) | (depth_image2 < 0), 0, depth_image2)
depth1 = o3d.Image(depth_image11)
color1 = o3d.Image(cv2.cvtColor(color_image1, cv2.COLOR_BGR2RGB))
depth2 = o3d.Image(depth_image22)
color2 = o3d.Image(cv2.cvtColor(color_image2, cv2.COLOR_BGR2RGB))
rgbd1 = o3d.create_rgbd_image_from_color_and_depth(
color1, depth1, convert_rgb_to_intensity=False)
pcd1 = o3d.create_point_cloud_from_rgbd_image(
rgbd1, pinhole_camera1_intrinsic)
rgbd2 = o3d.create_rgbd_image_from_color_and_depth(
color2, depth2, convert_rgb_to_intensity=False)
pcd2 = o3d.create_point_cloud_from_rgbd_image(
rgbd2, pinhole_camera2_intrinsic)
def track_view(vis):
global frame, poses
global num_frames, last_T
global parameter
ctr = vis.get_view_control()
if frame == 0:
params = ctr.convert_to_pinhole_camera_parameters()
intrinsics = params.intrinsic
extrinsics = params.extrinsic
pose = np.array(
[[-0.8188861922, 0.3889273405, -0.4220911372, -14.6068376600],
[-0.1157361687, -0.8321937190, -0.5422718444, 23.0477832143],
[-0.5621659395, -0.3952077147, 0.7264849060, 4.1193224787],
[0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000]])
params = o3d.camera.PinholeCameraParameters()
params.extrinsic = pose
params.intrinsic = intrinsics
ctr.convert_from_pinhole_camera_parameters(params)
fid = frame % num_frames
keyindex1 = 500
keyindex2 = 600
if True:
intrinsics = o3d.read_pinhole_camera_intrinsic(
"stream/%d.intrinsics.json" % fid)
T = np.loadtxt('stream/%d.pose' % fid)
params = o3d.camera.PinholeCameraParameters()
params.extrinsic = T
params.intrinsic = intrinsics
ctr.convert_from_pinhole_camera_parameters(params) #很重要，不能删除
if (fid == keyindex1) | (fid == keyindex2):
""" Generate Point Cloud """
if (last_T is None) or ((fid in frame_trace) or
(AngularDistance(T, last_T))):
print('%d/%d' % (fid, num_frames))
depth = vis.capture_depth_float_buffer(False)
depth = np.array(depth)
idx = np.where(depth > 30)
depth[idx] = 0
depth = o3d.Image(depth)
image = vis.capture_screen_float_buffer(False)
image = o3d.Image(
np.array(np.array(image) * 255).astype(np.uint8))
rgbd = o3d.create_rgbd_image_from_color_and_depth(
image, depth, convert_rgb_to_intensity=False)
rgbd.depth = o3d.Image(np.array(rgbd.depth) * 1000)
pcd = o3d.create_point_cloud_from_rgbd_image(
rgbd, intrinsics)
pcd.transform(np.linalg.inv(T))
o3d.write_point_cloud("stream/%d.ply" % fid,
pcd,
write_ascii=True)
cv2.imwrite("stream/%d.png" % fid, np.array(image))
depth = np.array(depth)
#depth = np.array(depth)*1000
#depth.astype(np.uint16)
#cv2.imwrite("stream/%d_depth.png" % fid, depth)##
last_T = T
if (fid == keyindex1):
intrinsic_matrix = intrinsics.intrinsic_matrix
pcd0, image, indexlist = see2pcd(
fid, depth, np.array(image), pcd,
intrinsics.intrinsic_matrix)
pcd0.transform(np.linalg.inv(T))
#print(T)
parameter['0'] = [pcd0, image, indexlist]
if (fid == keyindex2):
intrinsic_matrix = intrinsics.intrinsic_matrix
pcd1, image, indexlist = see2pcd(
fid, depth, np.array(image), pcd,
intrinsics.intrinsic_matrix)
pcd1.transform(np.linalg.inv(T))
#print(T)
parameter['1'] = [pcd1, image, indexlist]
vis.destroy_window()
#o3d.draw_geometries([parameter['0'][0],pcd1])
compare(parameter)
os.system("pause")
if fid == num_frames - 1:
exit()
frame += 1
depth_image[depth_image > max_value] = max_value
return depth_image, intrin
data_path = '/home/spc/Desktop/DepthCodec/sample_data/*'
files = glob.glob(data_path)
assert files.__len__(), "no test data in " + data_path
for example in files:
depth, intrin = loadSampleFromFile(example, 20)
plt.figure()
plt.imshow(depth)
plt.pause(0.1)
plt.waitforbuttonpress()
open3d_depth = open3d.Image(depth)
open3d_intrin = open3d.PinholeCameraIntrinsic(intrin.width, intrin.height,
intrin.fx, intrin.fy,
intrin.ppx, intrin.ppy)
open3d_pc1 = open3d.create_point_cloud_from_depth_image(
open3d_depth, open3d_intrin)
open3d.draw_geometries([open3d_pc1])
tc = DepthImageCodec()
tc.compress(depth)
bits = BitStream()
bits = tc.encode(bits)
print(depth.size * 16 / len(bits))
uncompressed = tc.uncompress()
open3d_uncompressed = open3d.Image(uncompressed)
def to_rgbd(color, depth, depth_scale, depth_trunc):
if color.shape[:2] != depth.shape[:2]:
color = cv2.resize(color, (depth.shape[1], depth.shape[0]))
rgbd = o3d.create_rgbd_image_from_color_and_depth(o3d.Image(color), o3d.Image(depth),
convert_rgb_to_intensity=False, depth_scale=depth_scale, depth_trunc=depth_trunc)
return rgbd
def track_view(vis):
global frame, poses
global num_frames, last_T
#print('frame=%d' % frame)
#import ipdb; ipdb.set_trace()
ctr = vis.get_view_control()
######intrinsics = o3d.read_pinhole_camera_intrinsic('intrinsics.json')
#intrinsics = o3d.PinholeCameraIntrinsic(o3d.PinholeCameraIntrinsicParameters.Kinect2ColorCameraDefault)
######intrinsics = o3d.PinholeCameraIntrinsic(
###### o3d.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
######intrinsics_mat=np.array([[935.30743608719376, 0.0, 0.0],
###### [0.0, 935.30743608719376, 0.0],
###### [959.5, 539.5, 1.0]])
######intrinsics.intrinsic_matrix = intrinsics_mat
#import ipdb; ipdb.set_trace()
######print(intrinsics)
#pcd = o3d.create_point_cloud_from_rgbd_image(rgbd, intrinsics)
#o3d.write_point_cloud("stream/%d.ply" % frame, pcd)
if frame == 0:
#vis.get_render_option().load_from_json("render.json")
intrinsics, extrinsics = ctr.convert_to_pinhole_camera_parameters()
pose = [[-0.8188861922, 0.3889273405, -0.4220911372, -14.6068376600],
[-0.1157361687, -0.8321937190, -0.5422718444, 23.0477832143],
[-0.5621659395, -0.3952077147, 0.7264849060, 4.1193224787],
[0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000]]
ctr.convert_from_pinhole_camera_parameters(intrinsics, pose)
fid = frame % num_frames
if not args.load:
intrinsics, extrinsics = ctr.convert_to_pinhole_camera_parameters()
o3d.write_pinhole_camera_intrinsic("stream/%d.intrinsics.json" % frame, intrinsics)
with open('stream/%d.pose' % frame, 'w') as fout:
for i in range(4):
for j in range(4):
if j > 0:
fout.write(' ')
fout.write('%.10f' % extrinsics[i, j])
fout.write('\n')
#o3d.write_pinhole_camera_intrinsics(intrinsics, "stream/%d.intrinsics.json" % frame)
#pose_dict['intrinsics'].append(intrinsics)
#pose_dict['extrinsics'].append(extrinsics)
#if frame == 100:
# import pickle
# with open('stream/pose.p', 'wb') as fout:
# pickle.dump(pose_dict, fout, protocol=pickle.HIGHEST_PROTOCOL)
# exit()
else:
intrinsics = o3d.read_pinhole_camera_intrinsic("stream/%d.intrinsics.json" % fid)
T = np.loadtxt('stream/%d.pose' % fid)
ctr.convert_from_pinhole_camera_parameters(intrinsics, T)
if args.generate:
""" Generate Point Cloud """
if (last_T is None) or (fid in frame_trace) or (AngularDistance(T, last_T)):
print('%d/%d' % (fid, num_frames))
#vis.update_geometry()
#vis.poll_events()
#vis.update_renderer()
depth = vis.capture_depth_float_buffer(False)
depth = np.array(depth)
#print(depth.max(), depth.min())
idx = np.where(depth > 30)
depth[idx] = 0
depth = o3d.Image(depth)
image = vis.capture_screen_float_buffer(False)
image = o3d.Image(np.array(np.array(image)*255).astype(np.uint8))
rgbd = o3d.create_rgbd_image_from_color_and_depth(
image, depth, convert_rgb_to_intensity = False)
rgbd.depth = o3d.Image(np.array(rgbd.depth)*1000)
pcd = o3d.create_point_cloud_from_rgbd_image(rgbd, intrinsics)
#pcd = o3d.voxel_down_sample(pcd, voxel_size=0.05)
#pcd.transform(np.linalg.inv(T))
o3d.write_point_cloud("stream/%d.ply" % fid, pcd, write_ascii=True)
cv2.imwrite("stream/%d.png" % fid, np.array(image))
last_T = T
if fid == num_frames - 1:
exit()
frame += 1
depth_val = 255 * (norm) / 120
depth_val = 32 * np.log2(depth_val + 1)
print('%s Norm %d' % (ply_file_path, max(norm[:])))
if np.isclose(max(norm[:]), 0):
'Error With File'
continue
thetaz = np.rad2deg(np.arcsin(xyz_load[:, 2] / norm))
thetax = (180 * (np.int64(xyz_load[:, 0] < 0))) + np.rad2deg(
np.arctan(xyz_load[:, 1] / xyz_load[:, 0]))
theta_x = thetax + 90 - 180 % 360
theta_z = thetaz + 90
img_cy = np.zeros((180 * res_scale, 360 * res_scale, 1))
img_cy[np.int64(theta_z * res_scale),
np.int64(theta_x * res_scale)] = np.expand_dims(
255 - depth_val, 1)
img_od_cyl = od.Image(img_cy.astype(np.uint8))
xyz_tmp = np.zeros_like(xyz_load)
xyz_tmp[:, 1] = xyz_load[:, 0]
xyz_tmp[:, 2] = xyz_load[:, 1]
xyz_tmp[:, 0] = xyz_load[:, 2]
ext = get_extrinsic(roll=0 * np.pi / 4,
yaw=4 * np.pi / 4,
pitch=2 * np.pi / 4,
x=0,
y=0,
z=0)
xyz_load = xyz_tmp
K = np.identity(3)
K[0, 2] = WINDOW_WIDTH / 2.0
K[1, 2] = WINDOW_HEIGHT / 2.0
K[0, 0] = K[1,
