def ten_frame_profiling():
n_imgs = 1000
cam_intr = np.loadtxt("../data/camera-intrinsics.txt", delimiter=' ')
vol_bnds = np.zeros((3, 2))
for i in range(n_imgs):
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))
view_frust_pts = grid_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))
hash_table = hash_fusion.HashTable(vol_bnds, voxel_size=0.02)
total_time = 0
# Loop through the first 10 RGB-D images and fuse them together
for i in range(10):
tic = time.perf_counter()
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))
hash_table.integrate(color_image,
depth_im,
cam_intr,
cam_pose,
obs_weight=1.)
toc = time.perf_counter()
tictoc = round(toc - tic, 2)
total_time += tictoc
avg_time = round(total_time / (i + 1), 2)
print("Integrate frame {} in {} seconds. Avg: {}s/frame".format(
i + 1, tictoc, avg_time))
"""
print("Frame {}: num occupied buckets: {}; load factor: {}; collisions: {}; collision rate: {}".format(
i + 1,
hash_table.get_num_non_empty_bucket(),
hash_table.get_load_factor(),
hash_table.get_num_collisions(),
hash_table.get_num_collisions() / hash_table.get_num_non_empty_bucket()
))
"""
verts, faces, norms, colors = hash_table.get_mesh()
grid_fusion.meshwrite("mesh_hash_demo1.ply", verts, faces, norms, colors)
# Get point cloud from voxel volume and save to disk (can be viewed with Meshlab)
print("Saving point cloud to pc.ply...")
point_cloud = hash_table.get_point_cloud()
grid_fusion.pcwrite("pc_hash_demo1.ply", point_cloud)
def one_frame_profiling():
n_imgs = 1000
cam_intr = np.loadtxt("../data/camera-intrinsics.txt", delimiter=' ')
vol_bnds = np.zeros((3, 2))
for i in range(n_imgs):
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))
view_frust_pts = grid_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))
hash_table = hash_fusion.HashTable(vol_bnds, voxel_size=0.02)
# fuse frame 0 for testing
color_image = cv2.cvtColor(cv2.imread("../data/frame-%06d.color.jpg" % 0),
cv2.COLOR_BGR2RGB)
depth_im = cv2.imread("../data/frame-%06d.depth.png" % 0, -1).astype(float)
depth_im /= 1000.
depth_im[depth_im == 65.535] = 0
cam_pose = np.loadtxt("../data/frame-%06d.pose.txt" % 0)
hash_table.integrate(color_image,
depth_im,
cam_intr,
cam_pose,
obs_weight=1.)
def test_general(self):
hash_table = hf.HashTable([[-4.22106438, 3.86798203],
[-2.6663104, 2.60146141], [0., 5.76272371]],
0.02, 10000, False)
world_coords = []
for i in range(4 * 10000):
x = np.random.randint(500)
y = np.random.randint(500)
z = i
world_coords.append([x, y, z])
entry = he.HashEntry((x, y, z), None, None)
hash_table.add_hash_entry(entry)
self.assertEqual(4 * 10000, hash_table.count_num_hash_entries(),
"test adding to maximum capacity")
for i in range(2 * 10000):
index = np.random.randint(len(world_coords) - 1)
position = world_coords[index]
world_coords.remove(position)
hash_table.remove(position)
self.assertEqual(2 * 10000, hash_table.count_num_hash_entries(),
"test remove half of entries")
for remaining_position in world_coords:
self.assertIsNotNone(
hash_table.get_hash_entry(remaining_position),
"can still find the remaining half of "
"entries")
for i in range(2 * 10000):
x = np.random.randint(500)
y = np.random.randint(500) + 501
z = i
world_coords.append([x, y, z])
entry = he.HashEntry((x, y, z), None, None)
hash_table.add_hash_entry(entry)
self.assertEqual(4 * 10000, hash_table.count_num_hash_entries(),
"adding to max capacity again")
def test_hash_function():
positions = [[333, 234, 241], [342, 234, 241], [332, 234, 242]]
hash_map = hash_fusion.HashTable(
[[-4.22106438, 3.86798203], [-2.6663104, 2.60146141], [0., 5.76272371]
], 0.02, 100000, False)
for position in positions:
hash_value = hash_map.hash_function(position)
print(hash_value)
def test_add_same_hash_value(self):
hash_map = hf.HashTable([[-4.22106438, 3.86798203],
[-2.6663104, 2.60146141], [0., 5.76272371]],
0.02, 1000, False)
for i in range(4000):
hash_entry = he.HashEntry([0, 0, 0], None, None)
hash_map.hash_function([0, 0, 0])
hash_map.add_hash_entry(hash_entry)
self.assertEqual(4000, hash_map.count_num_hash_entries(),
"4000 identical entries are added")
def test_add_until_full_size_1000(self):
hash_map = hf.HashTable([[-4.22106438, 3.86798203],
[-2.6663104, 2.60146141], [0., 5.76272371]],
0.02, 1000, False)
for i in range(4500):
x = np.random.randint(500)
y = np.random.randint(500)
z = i
position = [x, y, z]
hash_entry = he.HashEntry(position, None, None)
hash_map.hash_function(position)
hash_map.add_hash_entry(hash_entry)
self.assertEqual(4500, hash_map.count_num_hash_entries(),
"4000 entries are added")
def test_add(self):
hash_table = hf.HashTable([[-4.22106438, 3.86798203],
[-2.6663104, 2.60146141], [0., 5.76272371]],
0.02, 10, False)
world_coords = [[4, 43, 0], [48, 37, 1], [37, 33, 2], [0, 42, 3],
[44, 11, 4], [2, 42, 5], [40, 0, 6], [22, 43, 7],
[22, 15, 8], [12, 4, 9], [38, 44, 10], [24, 8, 11],
[18, 9, 12], [36, 26, 13], [11, 42, 14], [5, 17, 15],
[3, 38, 16], [13, 12, 17], [1, 43, 18], [18, 40, 19],
[22, 3, 20], [28, 40, 21], [3, 8, 22], [45, 25, 23],
[15, 21, 24], [26, 49, 25], [9, 20, 26], [13, 42, 27],
[3, 47, 28], [3, 37, 29], [33, 14, 30], [46, 44, 31],
[5, 22, 32], [33, 4, 33], [20, 41, 34], [23, 48, 35],
[35, 48, 36], [23, 25, 37], [9, 17, 38], [6, 38, 39]]
for position in world_coords:
hash_entry = he.HashEntry(position, None, None)
hash_table.add_hash_entry(hash_entry)
# self.assertEqual(10, hash_table.get_num_non_empty_buckets(), "no buckets should be empty")
self.assertEqual(40, hash_table.count_num_hash_entries())
def test_add_until_full_size_10(self):
hash_table = hf.HashTable([[-4.22106438, 3.86798203],
[-2.6663104, 2.60146141], [0., 5.76272371]],
0.02, 10, False)
world_coords = [[80, 56, 0], [66, 23, 1], [64, 5, 2], [77, 87, 3],
[22, 55, 4], [1, 62, 5], [54, 98, 6], [17, 35, 7],
[42, 86, 8], [75, 84, 9], [72, 56, 10], [68, 94, 11],
[31, 18, 12], [97, 83, 13], [21, 56, 14], [16, 38, 15],
[80, 46, 16], [22, 64, 17], [68, 79, 18], [98, 10, 19],
[26, 31, 20], [83, 53, 21], [11, 7, 22], [92, 7, 23],
[76, 81, 24], [89, 75, 25], [2, 71, 26], [82, 10, 27],
[77, 58, 28], [57, 0, 29], [80, 25, 30], [43, 92, 31],
[15, 26, 32], [33, 93, 33], [23, 42, 34], [97, 22, 35],
[6, 88, 36], [43, 51, 37], [6, 90, 38], [54, 83, 39]]
for position in world_coords:
hash_entry = he.HashEntry(position, None, None)
hash_table.hash_function(position)
hash_table.add_hash_entry(hash_entry)
self.assertEqual(40, hash_table.count_num_hash_entries(),
"40 entries are added")
def test_resize_maintain_num_entries(self):
hash_table = hf.HashTable([[-4.22106438, 3.86798203],
[-2.6663104, 2.60146141], [0., 5.76272371]],
0.02, 10, False)
world_coords = [[80, 56, 0], [66, 23, 1], [64, 5, 2], [77, 87, 3],
[22, 55, 4], [1, 62, 5], [54, 98, 6], [17, 35, 7],
[42, 86, 8], [75, 84, 9], [72, 56, 10], [68, 94, 11],
[31, 18, 12], [97, 83, 13], [21, 56, 14], [16, 38, 15],
[80, 46, 16], [22, 64, 17], [68, 79, 18], [98, 10, 19],
[26, 31, 20], [83, 53, 21], [11, 7, 22], [92, 7, 23],
[76, 81, 24], [89, 75, 25], [2, 71, 26], [82, 10, 27],
[77, 58, 28], [57, 0, 29], [80, 25, 30], [43, 92, 31],
[15, 26, 32], [33, 93, 33], [77, 25, 44], [82, 56, 45],
[9, 44, 46], [54, 34, 47], [0, 73, 48], [81, 95, 49]]
for position in world_coords:
hash_entry = he.HashEntry(position, None, None)
hash_table.add_hash_entry(hash_entry)
hash_table.double_table_size()
self.assertEqual(40, hash_table.count_num_hash_entries())
for position in world_coords:
self.assertIsNotNone(hash_table.get_hash_entry(position))
def main():
print("Estimating voxel volume bounds...")
n_imgs = 1000
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 = grid_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))
print("Initializing hash table...")
hash_table = hash_fusion.HashTable(vol_bnds,
voxel_size=0.02,
map_size=2000000)
# Loop through RGB-D images and fuse them together
t0_elapse = time.time()
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)
hash_table.integrate(color_image,
depth_im,
cam_intr,
cam_pose,
obs_weight=1.)
fps = n_imgs / (time.time() - t0_elapse)
print("Average FPS: {:.2f}".format(fps))
# Get mesh from voxel volume and save to disk (can be viewed with Meshlab)
print("Saving mesh to mesh.ply...")
verts, faces, norms, colors = hash_table.get_mesh()
grid_fusion.meshwrite("mesh_hash_demo1.ply", verts, faces, norms, colors)
# Get point cloud from voxel volume and save to disk (can be viewed with Meshlab)
print("Saving point cloud to pc.ply...")
point_cloud = hash_table.get_point_cloud()
grid_fusion.pcwrite("pc_hash_demo1.ply", point_cloud)