def create_pointcloud_from_depth(
depth_path, # type: str,
focal_x, # type: float,
focal_y, # type: float,
principal_x, # type: float,
principal_y, # type: float
):
depth_image = open3d.read_image(depth_path)
intrinsic = open3d.camera.PinholeCameraIntrinsic()
intrinsic.set_intrinsics(640, 480, focal_x, focal_y, principal_x,
principal_y)
return open3d.geometry.create_point_cloud_from_depth_image(
depth_image, intrinsic)
def getPcRGBD(path, idx):
frame = 'frame-000000'
frame = frame[:-len(idx)] + idx
depth = frame + '.depth.png'
depth = os.path.join(path, depth)
rgb = frame + '.color.png'
rgb = os.path.join(path, rgb)
im_depth = o3d.read_image(depth)
im_color = o3d.read_image(rgb)
# rgbd_image = o3d.create_rgbd_image_from_color_and_depth(im_color, im_depth)
pcd = o3d.create_point_cloud_from_depth_image(
im_depth,
o3d.PinholeCameraIntrinsic(
# pcd = o3d.create_point_cloud_from_rgbd_image(rgbd_image, o3d.PinholeCameraIntrinsic(
o3d.PinholeCameraIntrinsicParameters.PrimeSenseDefault))
# o3d.draw_geometries([pcd])
return pcd
def read_color_and_depth_image(
folder,
ith_image,
output_img_format="open3d",
index_len=5,
image_folder="image", # folder/image/image_ith_image
image_name="image_",
depth_doler="depth", # folder/depth/depth_ith_image
depth_name="depth_",
):
assert output_img_format in ["open3d", "cv2", "cv"]
'''
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)
cloud = filtCloudByRange(cloud, zmin=0.2, zmax=0.8)
open3d.io.draw_geometries([cloud])
}
'''
cfolder = folder + "/" + image_folder + "/" + image_name
dfolder = folder + "/" + depth_doler + "/" + depth_name
str_idx = ("{:0" + str(index_len) + "d}").format(ith_image)
suffix = str_idx + ".png"
if output_img_format == "cv" or output_img_format == "cv2":
color_image = cv2.imread(cfolder + suffix, cv2.IMREAD_UNCHANGED)
depth_image = cv2.imread(dfolder + suffix, cv2.IMREAD_UNCHANGED)
else:
color_image = open3d.read_image(cfolder + suffix)
depth_image = open3d.read_image(dfolder + suffix)
return color_image, depth_image
def __init__(self,
root,
traj,
subsample_rate=20,
depth_scale=2000,
trans_by_pose=None):
self.root = root
self.traj = traj
self._trans_by_pose = trans_by_pose
self.depth_scale = depth_scale
traj_file = os.path.join(root, 'local_point_cloud', traj)
depth_files = [line.rstrip('\n') for line in open(traj_file)]
self.n_pc = len(depth_files)
# load point cloud and gt
image_structs = sio.loadmat(os.path.join(root, 'image_structs.mat'))
image_structs = image_structs['image_structs'][0]
image_files = [i[0][0] for i in image_structs]
point_clouds = []
self.gt = np.zeros((self.n_pc, 6))
for index, depth_file in enumerate(depth_files):
depth_file_full = os.path.join(root, 'high_res_depth', depth_file)
depth_map = np.asarray(o3d.read_image(depth_file_full))
current_point_cloud = utils.convert_depth_map_to_pc(
depth_map,
self.focal_len,
self.principal_pt,
depth_scale=self.depth_scale)
current_point_cloud = current_point_cloud[::subsample_rate, ::
subsample_rate, :]
point_clouds.append(current_point_cloud)
image_file = depth_file[:14] + '1.jpg'
idx = image_files.index(image_file)
current_image_struct = image_structs[idx]
current_pos = current_image_struct[6]
current_direction = current_image_struct[4]
current_gt = np.concatenate((current_pos, current_direction)).T
self.gt[index, :] = current_gt
point_clouds = np.asarray(point_clouds)
self.point_clouds = torch.from_numpy(point_clouds) # <NxHxWx3>
self.valid_points = find_valid_points(self.point_clouds)
def generate_pcd(path):
setting = list(
csv.reader(open(path + '/img.cfg', 'r'),
delimiter=';',
skipinitialspace=True))[1]
h_start_angle = math.radians(float(setting[2]))
h_end_angle = math.radians(float(setting[3]))
v_angles = numpy.radians(numpy.float32(setting[4:]))
image_files = sorted(
[path + '/' + x for x in os.listdir(path) if '.png' in x])
vecs = []
for row in range(int(setting[1])):
for col in range(int(setting[0])):
yaw_step = (h_end_angle - h_start_angle) / int(setting[0])
yaw = h_start_angle + col * yaw_step
pitch = v_angles[row]
vecs.append(yawpitch2_vec(pitch, yaw))
vecs = numpy.float32(vecs)
for image_file in image_files:
print image_file
image = numpy.asarray(open3d.read_image(image_file)).flatten()
points = (vecs.T * (image / 500.0).flatten()).T
cloud = open3d.geometry.PointCloud()
cloud.points = open3d.Vector3dVector(points)
open3d.estimate_normals(cloud,
search_param=open3d.KDTreeSearchParamHybrid(
radius=0.2, max_nn=30))
dst_filename = image_file[:-4] + '.pcd'
open3d.write_point_cloud(dst_filename, cloud)
def generateImage(mapping, im_color):
global CLOUD_ROT
img_m = mapping.Cloud2Image(CLOUD_ROT)
img_mapped = cv2.addWeighted(img_m, 0.5, im_color, 0.5, 0)
cv2.imshow(window_name, img_mapped)
if __name__ == "__main__":
args = get_argumets()
"""Data loading"""
print(":: Load two point clouds to be matched.")
color_raw = o3.read_image(args.cimg)
depth_raw = o3.read_image(args.dimg)
camera_intrinsic = o3.read_pinhole_camera_intrinsic(args.intrin)
im_color = np.asarray(color_raw)
im_color = cv2.cvtColor(im_color, cv2.COLOR_BGR2RGB)
im_depth = np.asarray(depth_raw)
rgbd_image = o3.create_rgbd_image_from_color_and_depth(
color_raw, depth_raw, 1000.0, 2.0)
pcd = o3.create_point_cloud_from_rgbd_image(rgbd_image, camera_intrinsic)
o3.write_point_cloud("cloud_in.ply", pcd)
cloud_in_ds = o3.voxel_down_sample(pcd, 0.005)
o3.write_point_cloud("cloud_in_ds.ply", cloud_in_ds)
np_pcd = np.asarray(pcd.points)
os.path.join(path, "image/"), extension = ".jpg")
if USE_Z_BUFFERING:
depth_image_path = [osp.join('/tmp', fn.replace('jpg', 'png'))
for fn in color_image_path]
# generate depth maps
save_depth_maps(mesh, camera.intrinsic, np.asarray(camera.extrinsic),
depth_image_path)
else:
depth_filenames = get_file_list(
os.path.join(path, "depth/"), extension = ".png")
depth_image_path = []
# add artificial noise to the depth maps
for i in range(len(depth_filenames)):
depth = open3d.read_image(osp.join(path, 'depth', depth_filenames[i]))
depth = np.asarray(depth, dtype=np.float)
depth += 50*np.random.rand(*depth.shape)
tmp_filename = osp.join('/tmp', 'depth_{:s}'.format(depth_filenames[i]))
depth_image_path.append(tmp_filename)
assert(len(depth_image_path) == len(color_image_path))
# Read RGBD images
rgbd_images = []
for i in range(len(depth_image_path)):
depth = open3d.read_image(depth_image_path[i])
color = open3d.read_image(os.path.join(path, 'image', color_image_path[i]))
rgbd_image = open3d.create_rgbd_image_from_color_and_depth(color, depth,
convert_rgb_to_intensity = False)
if debug_mode:
args = get_argumets()
green = np.array([0, 255, 0])
blue = np.array([255, 0, 0])
white = np.array([255, 255, 255])
img_list = glob.glob(args.img_folder + '/*rgb.png')
img_list.sort()
model_list = glob.glob(args.model_dir + '/*.pcd')
for i, cimg in enumerate(img_list):
"""Data loading"""
print(":: Load two point clouds to be matched.")
color_raw = o3.read_image(cimg)
dimg = cimg[:-7] + 'depth.png'
depth_raw = o3.read_image(dimg)
camera_intrinsic = o3.read_pinhole_camera_intrinsic(args.intrin)
im_color = np.asarray(color_raw)
im_color = cv2.cvtColor(im_color, cv2.COLOR_BGR2RGB)
im_depth = np.asarray(depth_raw)
rgbd_image = o3.create_rgbd_image_from_color_and_depth(
color_raw, depth_raw, 1000.0, 2.0)
pcd = o3.create_point_cloud_from_rgbd_image(rgbd_image,
camera_intrinsic)
o3.write_point_cloud("cloud_in.ply", pcd)
cloud_in_ds = o3.voxel_down_sample(pcd, 0.005)
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)
def reconstruct(room_name, cam_poses):
# prepare the result file
result_file = os.path.join(
proj_path, "result", "{}_{}_{}.json".format(dataset_name, room_name,
vox_num_option))
print(result_file)
if ft.file_exist(result_file):
# load data
print("load result json file ...")
data_result = io.load_json(result_file)
else:
print("No result file, please check your folder again!")
return
if VIS_ALL or VIS_FINAL:
vis = o3d.Visualizer()
vis.create_window()
print("created a window")
for start_index_key in start_key_list:
print("start index", start_index_key)
result_pcd_folder = os.path.join(proj_path, "result", "pcd",
start_index_key)
if not os.path.exists(result_pcd_folder):
os.makedirs(result_pcd_folder)
# regarding the view point for the visualiser
viewpoint_file = os.path.join(result_pcd_folder, "view_point.json")
for method in NBV_strategy:
print("method", method)
result_pcd_file = os.path.join(
result_pcd_folder,
"{}_{}_{}.ply".format(dataset_name, room_name, method))
result_pcd_folder_method = os.path.join(proj_path, "result",
start_index_key, method)
if (not os.path.exists(result_pcd_file)) or (
VIS_ALL and
(not os.path.exists(result_pcd_folder_method))):
if VIS_ALL:
trajectory = o3d.read_pinhole_camera_trajectory(
viewpoint_file)
print("created a window")
if VIS_ALL and (not os.path.exists(result_pcd_folder_method)):
os.makedirs(os.path.join(result_pcd_folder_method, "pcd"))
os.makedirs(os.path.join(result_pcd_folder_method,
"depth"))
os.makedirs(os.path.join(result_pcd_folder_method, "rgb"))
print("created folders")
nbv_ids = data_result[start_index_key][method]["NBV_ids"]
volume = o3d.ScalableTSDFVolume(
voxel_length=0.01,
sdf_trunc=0.03,
color_type=o3d.TSDFVolumeColorType.RGB8) # 4.0 / 512.0
count = 0
for pose_id in range(len(nbv_ids)):
print("Integrate frame at {:d}".format(pose_id))
current_pose = cam_poses[nbv_ids[pose_id]]
rgb_path = os.path.join(
proj_path, config["dataset_folder"], dataset_name,
room_name, "image",
"rgb{:d}.png".format(nbv_ids[pose_id]))
depth_path = os.path.join(
proj_path, config["dataset_folder"], dataset_name,
room_name, "depth",
"depth{:d}.png".format(nbv_ids[pose_id]))
color = o3d.read_image(rgb_path)
depth = o3d.read_image(depth_path)
rgbd = o3d.create_rgbd_image_from_color_and_depth(
color,
depth,
depth_trunc=4.5,
convert_rgb_to_intensity=False)
volume.integrate(rgbd, intrinsic,
np.linalg.inv(current_pose))
pcd_scene = volume.extract_point_cloud(
) # note the points are not isometric
if VIS_ALL:
print("Visualise each frame")
result_pcd_file_temp = os.path.join(
result_pcd_folder_method, "pcd",
"{}_{}_{:03d}.png".format(dataset_name, room_name,
count))
result_depth_file_temp = os.path.join(
result_pcd_folder_method, "depth",
"{}_{}_{:03d}.png".format(dataset_name, room_name,
count))
result_rgb_file_temp = os.path.join(
result_pcd_folder_method, "rgb",
"{}_{}_{:03d}.png".format(dataset_name, room_name,
count))
copyfile(depth_path, result_depth_file_temp)
copyfile(rgb_path, result_rgb_file_temp)
vis.add_geometry(pcd_scene)
mesh_frame = local_plt.o3d_coordinate(current_pose)
vis.add_geometry(mesh_frame)
ctr = vis.get_view_control()
#if count == 0:
ctr.convert_from_pinhole_camera_parameters(
trajectory.intrinsic, trajectory.extrinsic[0])
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
vis.capture_screen_image(result_pcd_file_temp)
# crop border
img = improc.crop_border(result_pcd_file_temp, 0.08,
0.4)
# img = improc.imscale(img, 1.5)
improc.imwrite(result_pcd_file_temp, img)
time.sleep(0.5)
pcd_scene.points = o3d.Vector3dVector([])
pcd_scene.colors = o3d.Vector3dVector([])
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
count = count + 1
if not os.path.exists(result_pcd_file):
pcd_scene = volume.extract_point_cloud(
) #note the points are not isometric
o3d.write_point_cloud(result_pcd_file, pcd_scene)
else:
if VIS_FINAL:
print("created a visualiser")
pcd_scene = o3d.read_point_cloud(result_pcd_file)
# only trigger the view point set when there is no tarjectory file
if not os.path.exists(viewpoint_file):
print(
"Please manipulate the visualiser for a better view point, close to save!"
)
save_view_point(vis, pcd_scene, viewpoint_file)
return
image_name = os.path.join(result_pcd_folder,
"{}_pcd.png".format(method))
trajectory = o3d.read_pinhole_camera_trajectory(
viewpoint_file)
print("read the view point")
vis.add_geometry(pcd_scene)
print("Added the view point")
ctr = vis.get_view_control()
print("obtained view control")
ctr.convert_from_pinhole_camera_parameters(
trajectory.intrinsic, trajectory.extrinsic[0])
print("changed the view point")
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
print("updated the rendering")
vis.capture_screen_image(image_name)
print("captured the screenshot")
# crop border
img = improc.crop_border(image_name, 0.08, 0.4)
#img = improc.imscale(img, 1.5)
improc.imwrite(image_name, img)
time.sleep(2)
pcd_scene.points = o3d.Vector3dVector([])
pcd_scene.colors = o3d.Vector3dVector([])
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
print("reset the point to empty")
if VIS_ALL or VIS_FINAL:
vis.destroy_window()
print("closed window and visualiser")
import transforms3d as t3d # pip install transform3d
import open3d as o3d # pip install open3d-python==0.5
import numpy as np
import math
# load point cloud
i = 4
depth_image_path = "../output/bop_data/lmo/train_pbr/000000/depth/000000.png"
depth_raw = o3d.read_image(depth_image_path)
image_width = 671
image_height = 502
fx = 1122.375
fy = 1122.375
cx = 334.4445
cy = 264.443075
intrinsic = o3d.PinholeCameraIntrinsic(image_width, image_height, fx, fy, cx,
cy)
extrinsic = np.identity(4)
print(extrinsic)
depth_scale = 10
depth_trunc = 1500.0
np_depth = np.array(depth_raw)
depth_raw = o3d.geometry.Image(np_depth)
print(depth_raw)
print(type(depth_raw))
print(type(intrinsic))
color = True
def read_image(self, path):
return o3.read_image(path)
import open3d as o3d
import matplotlib.pyplot as plt
if __name__ == "__main__":
print("Read Redwood dataset")
color_raw = o3d.read_image("img3.png")
depth_raw = o3d.io.read_image("imgDepth3.png")
rgbd_image = o3d.geometry.create_rgbd_image_from_color_and_depth(
color_raw, depth_raw)
print(rgbd_image)
plt.subplot(1, 2, 1)
plt.title('Redwood grayscale image')
plt.imshow(rgbd_image.color)
plt.subplot(1, 2, 2)
plt.title('Redwood depth image')
plt.imshow(rgbd_image.depth)
plt.show()
pcd = o3d.geometry.create_point_cloud_from_rgbd_image(
rgbd_image,
o3d.camera.PinholeCameraIntrinsic(
o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault))
# Flip it, otherwise the pointcloud will be upside down
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
o3d.visualization.draw_geometries([pcd])
