def test_remove_all_entries_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 = [[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)
for position in world_coords:
hash_entry = he.HashEntry(position, None, None)
hash_table.remove_hash_entry(hash_entry)
self.assertEqual(0, hash_table.get_num_non_empty_buckets())
self.assertEqual(0, hash_table.count_num_hash_entries())
def remove_hash_entry(self, world_coord):
"""
remove the hash entry of specified world coordinate
:return: index if removed successfully, else -1
"""
temp_entry = he.HashEntry(world_coord, None, None)
for i in range(self._bucket_size):
element = self._bucket_list[i]
if element is None:
continue
if temp_entry.equals(element):
self._bucket_list[i] = None
return i
return -1
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 integrate(self, color_im, depth_im, cam_intr, cam_pose, obs_weight=1.):
"""
reference: https://github.com/andyzeng/tsdf-fusion-python
Integrate a RGB-D image to the hash table
"""
im_h, im_w = depth_im.shape
color_im = color_im.astype(np.float32)
color_im = np.floor(color_im[..., 2] * self._color_const +
color_im[..., 1] * 256 + color_im[..., 0])
cam_pts = self.vox2world(self._vol_origin, self.vox_coords,
self._voxel_size)
cam_pts = grid_fusion.rigid_transform(cam_pts, np.linalg.inv(cam_pose))
pix_z = cam_pts[:, 2]
pix = self.cam2pix(cam_pts, cam_intr)
pix_x, pix_y = pix[:, 0], pix[:, 1]
valid_pix = np.logical_and(
pix_x >= 0,
np.logical_and(
pix_x < im_w,
np.logical_and(pix_y >= 0,
np.logical_and(pix_y < im_h, pix_z > 0))))
depth_val = np.zeros(pix_x.shape)
depth_val[valid_pix] = depth_im[pix_y[valid_pix], pix_x[valid_pix]]
depth_diff = depth_val - pix_z
valid_pts = np.logical_and(depth_val > 0,
depth_diff >= -self._trunc_margin)
dist = np.minimum(1, depth_diff / self._trunc_margin)
valid_vox_x = self.vox_coords[valid_pts, 0]
valid_vox_y = self.vox_coords[valid_pts, 1]
valid_vox_z = self.vox_coords[valid_pts, 2]
valid_pix_x = pix_x[valid_pts]
valid_pix_y = pix_y[valid_pts]
valid_dist = dist[valid_pts]
# integration step
for i in range(len(valid_vox_x)):
# print(i)
voxel_coord = [valid_vox_x[i], valid_vox_y[i], valid_vox_z[i]]
target_entry = self.get_hash_entry(voxel_coord)
if target_entry is None:
voxel = v.Voxel()
voxel.integrate(valid_dist[i], color_im[valid_pix_y[i],
valid_pix_x[i]])
entry = he.HashEntry(voxel_coord, None, voxel)
self.add_hash_entry(entry)
else:
target_entry.integrate_voxel(
valid_dist[i], color_im[valid_pix_y[i], valid_pix_x[i]])
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 add_voxel(self, voxel, world_coord):
"""
add the (world_coord, voxel) pair to the hash map
"""
entry_to_add = he.HashEntry(world_coord, None, voxel)
self.add_hash_entry(entry_to_add)
# Get vertex colors
rgb_vals = color_volume[verts_ind[:, 0], verts_ind[:, 1], verts_ind[:,
2]]
colors_b = np.floor(rgb_vals / self._color_const)
colors_g = np.floor((rgb_vals - colors_b * self._color_const) / 256)
colors_r = rgb_vals - colors_b * self._color_const - colors_g * 256
colors = np.floor(np.asarray([colors_r, colors_g, colors_b])).T
colors = colors.astype(np.uint8)
pc = np.hstack([verts, colors])
return pc
if __name__ == '__main__':
world_coords = []
hash_map = HashTable([[-4.22106438, 3.86798203], [-2.6663104, 2.60146141],
[0., 5.76272371]], 0.02, 1000000, False)
for i in range(400000):
x = np.random.randint(50)
y = np.random.randint(50)
z = i
world_coords.append([x, y, z])
position = [x, y, z]
hash_entry = he.HashEntry(position, None, None)
hash_value = hash_map.hash_function(position)
bucket_index, entry_index = hash_map.add_hash_entry(hash_entry)
print("Point {} of hash value {} is added to ({},{})".format(
position, hash_value, bucket_index, entry_index))
print(hash_map.count_num_hash_entries())
print("Test add finished")
import unittest
import numpy as np
from data_structures import bucket as b
from data_structures import hash_entry as he
from data_structures import voxel as v
entry1 = he.HashEntry(np.array([1, 2, 3]), None, v.Voxel(None, None, None))
entry2 = he.HashEntry(np.array([2, 0, 5]), None, v.Voxel(None, None, None))
entry3 = he.HashEntry(np.array([3, 9, 4]), None, v.Voxel(None, None, None))
entry4 = he.HashEntry(np.array([4, 4, 3]), None, v.Voxel(None, None, None))
entry5 = he.HashEntry(np.array([5, 7, 3]), None, v.Voxel(None, None, None))
entry_list = [entry1, entry2, entry3, entry4, entry5]
entry6 = he.HashEntry(np.array([1, 2, 3]), None, v.Voxel(None, None, None))
class TestBucket(unittest.TestCase):
def test_is_full(self):
bucket = b.Bucket(5)
self.assertFalse(bucket.is_full())
for entry in entry_list:
bucket.add_hash_entry(entry)
self.assertTrue(bucket.is_full())
def test_is_overflow(self):
bucket = b.Bucket(5)
self.assertFalse(bucket.is_overflow())
for entry in entry_list:
bucket.add_hash_entry(entry)
self.assertEqual(bucket.get_num_entry_stored(), 5, "current number of entries is 5")
