def tsdf_voxel_volume(fx, fy, cx, cy):
print("Estimating voxel volume bounds....")
cam_intrinsics = cam.camera_intrinsics(fx, fy, cx, cy)
vol_bnds = np.zeros((3, 2))
depth_image = np.load(
"/home/aditya/PycharmProjects/OpenCV-python/Project_2/TDCV-Project-2/trial1.npz"
)['depth']
camera_pose = np.identity(4)
view_frust_pts = fusion.get_view_frustum(depth_image, cam_intrinsics,
camera_pose)
vol_bnds[:, 0] = np.minimum(vol_bnds[:, 0], np.amin(view_frust_pts,
axis=1))
vol_bnds[:, 1] = np.maximum(vol_bnds[:, 1], np.amax(view_frust_pts,
axis=1))
print("Initializing voxel volume...")
tsdf_vol = fusion.TSDFVolume(vol_bnds, voxel_size=0.01)
tsdf_vol.integrate(depth_image,
depth_image,
cam_intrinsics,
camera_pose,
obs_weight=1.0)
print("Saving mesh to mesh.ply...")
verts, faces, norms, colors = tsdf_vol.get_mesh()
fusion.meshwrite("mesh.ply", verts, faces, norms, colors)
"""
def _generate_tsdf(self, camera, voxel_bounds, voxel_resolution):
# Initialize voxel volume
with HiddenPrints():
tsdf_vol = fusion.TSDFVolume(voxel_bounds,
voxel_size=voxel_resolution,
use_gpu=False)
# Read depth image
depth_name = osp.join(self.root_dir, camera['house_id'], 'invdepth',
'{:04}.png'.format(camera['idx']))
invdepth_img = cv2.imread(depth_name, cv2.IMREAD_COLOR)
invdepth_img = invdepth_img[:, :, ::-1].astype(np.uint16)
idepth = invdepth_img[:, :, 0] * 256 + invdepth_img[:, :, 1]
PIXEL_MAX = np.iinfo(np.uint16).max
minDepth = 0.3
depth = minDepth * PIXEL_MAX / idepth.astype(np.float)
# Dont trust the extreme values
valid_depth = (idepth != PIXEL_MAX) & (idepth != 0)
#Cap at 20 meters
valid_depth &= depth < 20
#Set invalid depth to 0
depth[~valid_depth] = 0
#Read RGB image
rgb_name = osp.join(self.root_dir, camera['house_id'], 'rgb',
'{:04}.png'.format(camera['idx']))
bgr_img = cv2.imread(rgb_name, cv2.IMREAD_COLOR)
rgb_img = bgr_img[:, :, ::-1]
# Integrate observation into voxel volume (assume color aligned with depth)
P_4x4 = np.vstack([camera['P'], np.array([0, 0, 0, 1])])
tsdf_vol.integrate(rgb_img, depth, camera['K'], np.linalg.inv(P_4x4))
result = {}
result['tsdf'], _ = tsdf_vol.get_volume()
result['flipped_tsdf'] = np.sign(
result['tsdf']) - result['tsdf'] #dmax = 1
result['frustum_mask'] = tsdf_vol.get_frustum_mask()
result['visible_free'] = result['frustum_mask'] & (
result['tsdf'] > voxel_resolution / tsdf_vol._trunc_margin)
result['occluded_mask'] = result['frustum_mask'] & (
result['tsdf'] <= voxel_resolution / tsdf_vol._trunc_margin)
result['tsdf_trunc_margin'] = tsdf_vol._trunc_margin
return result
view_frust_pts = fusion.get_view_frustum(depth_im, cam_intr, cam_pose)
vol_bnds[:, 0] = np.minimum(vol_bnds[:, 0],
np.amin(view_frust_pts, axis=1))
vol_bnds[:, 1] = np.maximum(vol_bnds[:, 1],
np.amax(view_frust_pts, axis=1))
break
print "Volume Bounds:", vol_bnds
file.close()
# ======================================================================================================== #
# Integrate
# ======================================================================================================== #
# Initialize voxel volume
print("Initializing voxel volume...")
tsdf_vol = fusion.TSDFVolume(vol_bnds, voxel_size=0.005)
t0_elapse = time.time()
class_id = 29
file = open('data_table/associate.txt')
lines = file.read().split("\n")
for i in range(len(lines) - 1):
rgb_file = base_dir + '/' + lines[i].split(" ")[1]
depth_file = base_dir + '/' + lines[i].split(" ")[2]
print("Fusing frame %d/%d" % (i + 1, n_imgs))
img_no = lines[i].split(" ")[1].split("_")[1][:4]
flag = False
final_mask = np.zeros((480, 640, 3), dtype=np.bool)
mask_index_file = open(mask_dir + img_no + '.txt')
mask_index_file_lines = mask_index_file.read().split("\n")
for j in range(len(mask_index_file_lines) - 1):
vol_bnds[:, 1] = np.maximum(vol_bnds[:, 1],
np.amax(view_frust_pts, axis=1))
else:
vol_bnds = np.array((
(-0.256, 0.256),
(-0.256, 0.256),
(-0.001, 0.255),
))
print('vol_bnds: ', vol_bnds)
# Initialize voxel volume
print("Initializing voxel volume...")
use_gpu = False if args.disable_gpu else True
tsdf_vol = fusion.TSDFVolume(vol_bnds,
voxel_size=voxel_size,
init_tsdf_vol_value=init_tsdf_vol_value,
use_gpu=use_gpu)
# Integrate voxels: Loop through RGB-D images and fuse them together
t0_elapse = time.time()
vol_dim = np.ceil((vol_bnds[:, 1] - vol_bnds[:, 0]) /
voxel_size).copy(order='C').astype(int)
mask = np.zeros(vol_dim, dtype=bool)
for idx_img in range(imgs_per_scene):
print("Fusing frame %d/%d" % (idx_img + 1, imgs_per_scene))
img_num = idx_img
# Read RGB-D image
f_color = files_color[img_num]
print('filename_rgb: ', f_color)
color_image = cv2.imread(str(f_color))
voxel_size = float(sys.argv[2])
block_length = float(sys.argv[3])
grid_bounds = np.array([[grid[0], grid[0] + block_length],
[grid[1], grid[1] + block_length],
[grid[2], grid[2] + block_length]])
plydata = PlyData.read(sys.argv[1])
print('getting vertices')
vertices = np.array(plydata.elements[0].data)
print('initializing tsdf with grid_bounds: ', grid_bounds)
mesh_name = "meshes/mesh_constructed_" + str(grid[0]) + "_" + str(
grid[1]) + "_" + str(grid[2]) + ".ply"
print(mesh_name)
if os.path.exists(mesh_name):
print("already done")
exit()
tsdf_vol = fusion.TSDFVolume(grid_bounds, voxel_size=voxel_size)
tsdf_vol.fill_grid(vertices)
verts, faces, norms, colors = tsdf_vol.get_mesh()
fusion.meshwrite(mesh_name, verts, faces, norms, colors)
def generate_mesh_blocks():
plydata = PlyData.read(sys.argv[1])
print('getting vertices')
vertices = np.array(plydata.elements[0].data)
if precise:
print('creating output directory')
if os.path.isdir("meshes"):
print("meshes already exists")
else:
os.mkdir("meshes", mode=0o777)
if os.path.isdir("blocks"):
print("blocks already exists")
else:
os.mkdir("blocks", mode=0o777)
print('generating blocks')
block_dict = dict()
for v in vertices:
x = int(math.floor(
(v[0] - half_len) /
block_side_length)) * block_side_length + half_len
y = int(math.floor(
(v[1] - half_len) /
block_side_length)) * block_side_length + half_len
z = int(math.floor(
(v[2] - half_len) /
block_side_length)) * block_side_length + half_len
pos = (x, y, z)
if pos in block_dict:
block_dict.get(pos).append(v)
else:
block_dict.update({pos: [v]})
else:
grid_bounds = np.array([[-2, 2], [-2, 2], [-2, 2]])
if precise:
already_done_blocks = set()
for file in os.listdir("meshes"):
already_done_blocks.add("meshes/" + file)
print(already_done_blocks)
for key in block_dict.keys():
verts = block_dict.get(key)
file_name = "blocks/" + str(key[0]) + "_" + str(
key[1]) + "_" + str(key[2]) + ".ply"
ply_file = open(file_name, "w")
ply_file.write("ply\n")
ply_file.write("format ascii 1.0\n")
ply_file.write("element vertex %d\n" % (len(verts)))
ply_file.write("property float x\n")
ply_file.write("property float y\n")
ply_file.write("property float z\n")
ply_file.write("property uchar red\n")
ply_file.write("property uchar green\n")
ply_file.write("property uchar blue\n")
ply_file.write("end_header\n")
for i in range(len(verts)):
ply_file.write("%f %f %f %d %d %d\n" %
(verts[i][0], verts[i][1], verts[i][2],
verts[i][3], verts[i][4], verts[i][5]))
ply_file.close()
bad_parallel()
else:
print('initializing tsdf for what reason idk')
print(grid_bounds)
tsdf_vol = fusion.TSDFVolume(grid_bounds, voxel_size=voxel_size)
tsdf_vol.fill_grid(vertices)
verts, faces, norms, colors = tsdf_vol.get_mesh()
fusion.meshwrite("mesh_constructed.ply", verts, faces, norms, colors)
def main(args):
if args.example == 'cpp':
print("Using PyTorch CPP.")
from cpp.integrate import integrate
elif args.example == 'jit':
print("Using PyTorch JIT.")
from jit.integrate import integrate
elif args.example == 'py':
print("Using vanilla PyTorch.")
from python.integrate import integrate
else:
pass
# ======================================================================================================== #
# (Optional) This is an example of how to compute the 3D bounds
# in world coordinates of the convex hull of all camera view
# frustums in the dataset
# ======================================================================================================== #
print("Estimating voxel volume bounds...")
n_imgs = 15
cam_intr = np.loadtxt("data/camera-intrinsics.txt", delimiter=' ')
vol_bnds = np.zeros((3, 2))
for i in range(n_imgs):
# Read depth image and camera pose
depth_im = cv2.imread("data/frame-%06d.depth.png" % (i),
-1).astype(float)
depth_im /= 1000. # depth is saved in 16-bit PNG in millimeters
depth_im[
depth_im ==
65.535] = 0 # set invalid depth to 0 (specific to 7-scenes dataset)
cam_pose = np.loadtxt("data/frame-%06d.pose.txt" %
(i)) # 4x4 rigid transformation matrix
# Compute camera view frustum and extend convex hull
view_frust_pts = fusion.get_view_frustum(depth_im, cam_intr, cam_pose)
vol_bnds[:, 0] = np.minimum(vol_bnds[:, 0],
np.amin(view_frust_pts, axis=1))
vol_bnds[:, 1] = np.maximum(vol_bnds[:, 1],
np.amax(view_frust_pts, axis=1))
# ======================================================================================================== #
# Initialize voxel volume
print("Initializing voxel volume...")
tsdf_vol = fusion.TSDFVolume(vol_bnds, 0.02, integrate)
# ======================================================================================================== #
# Integrate
# ======================================================================================================== #
# Loop through RGB-D images and fuse them together
t0_elapse = time.time()
times = []
for i in range(n_imgs):
print("Fusing frame %d/%d" % (i + 1, n_imgs))
# Read RGB-D image and camera pose
color_image = cv2.cvtColor(
cv2.imread("data/frame-%06d.color.jpg" % (i)), cv2.COLOR_BGR2RGB)
depth_im = cv2.imread("data/frame-%06d.depth.png" % (i),
-1).astype(float)
depth_im /= 1000.
depth_im[depth_im == 65.535] = 0
cam_pose = np.loadtxt("data/frame-%06d.pose.txt" % (i))
# Integrate observation into voxel volume (assume color aligned with depth)
tic = time.time()
tsdf_vol.integrate(color_image,
depth_im,
cam_intr,
cam_pose,
obs_weight=1.)
toc = time.time()
times.append(toc - tic)
fps = n_imgs / (time.time() - t0_elapse)
print("Average FPS: {:.2f}".format(fps))
times = [t * TIME_SCALES[args.scale] for t in times]
print("Average integration time: {:.3f} {}".format(np.mean(times),
args.scale))
# Extract pointcloud
point_cloud = tsdf_vol.extract_point_cloud()
fusion.pcwrite("pc.ply", point_cloud)
# Get mesh from voxel volume and save to disk (can be viewed with Meshlab)
print("Saving to mesh.ply...")
verts, faces, norms, colors = tsdf_vol.extract_triangle_mesh()
fusion.meshwrite("mesh.ply", verts, faces, norms, colors)