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)