def setUp(self):
self.map = VoxelMap()
self.map.voxel_size = 0.5
self.map.free_update = -1.0
self.map.hit_update = 2.0
self.map.occupancy_threshold = 0.0
self.assertIsNotNone(self.map)
def __init__(self):
ns = 'voxel_map/'
self._visual_threshold = rospy.get_param(ns + 'visual_threshold')
voxels_per_side = rospy.get_param(ns + 'voxels_per_side')
width_on_a_side = rospy.get_param(ns + 'width_on_a_side')
initial_prob = rospy.get_param(ns + 'initial_prob')
pD = rospy.get_param(ns + 'pD')
pFA = rospy.get_param(ns + 'pFA')
ps = rospy.get_param(ns + 'ps')
pt = rospy.get_param(ns + 'pt')
self.map = VoxelMap(voxels_per_side, width_on_a_side, initial_prob, pD,
pFA, ps, pt)
rospy.Subscriber("truth/NED",
Odometry,
callback=self.state_cb,
queue_size=1)
rospy.Subscriber("triang_points",
PointCloud,
callback=self.measurement_cb)
self.map_pub = rospy.Publisher("voxel_map", PointCloud, queue_size=1)
self.vis_pub = rospy.Publisher("voxel_map_visual",
PointCloud,
queue_size=1)
self.map_center_pub = rospy.Publisher("voxel_map_center", Odometry)
self.state = State()
self.blank_cloud = PointCloud()
zero_point = Point32(0, 0, 0)
self.blank_cloud.points = [zero_point] * self.map.N**3
def __init__(self, frame_id, resolution=0.1):
self.voxel_map = VoxelMap(resolution, -1.0, 2.0, 0.0)
self.min = -10.0
self.max = 10.0
self.msg = OccupancyGrid()
self.msg.header.frame_id = frame_id
self.msg.info.resolution = resolution
# Initially, set the grid origin to identity.
self.msg.info.origin.orientation.w = 1.0
def pointclouds_to_voxelmap_with_map(pointclouds,
camera_posisions,
voxel_size=0.25,
free_update=-1.0,
hit_update=2.0):
assert len(pointclouds) == len(camera_posisions)
# start = time.time()
map_obj = VoxelMap()
map_obj.voxel_size = voxel_size
map_obj.free_update = free_update
map_obj.hit_update = hit_update # zkusit 2násobný hit oproti free, zkusit include directories env var
map_obj.occupancy_threshold = 0.0
# end = time.time()
# print('setup of voxelmap', end - start)
# start = time.time()
for pointcloud, cam_pos in zip(pointclouds, camera_posisions):
line_starts = np.repeat(cam_pos[:, np.newaxis],
pointcloud.shape[1],
axis=1)
map_obj.update_lines(line_starts, pointcloud)
# end = time.time()
# print('updating voxelmap by all cameras', end - start)
# start = time.time()
[voxels, levels, values] = map_obj.get_voxels()
# end = time.time()
# print('obtaining voxels', end - start)
return voxels, values, map_obj.voxel_size, map_obj
def __init__(self):
ns = 'voxel_map/'
self._visual_threshold = rospy.get_param(ns + 'visual_threshold')
self.map = VoxelMap()
rospy.Subscriber("truth/NED",
Odometry,
callback=self.state_cb,
queue_size=1)
rospy.Subscriber("triang_points",
PointCloud,
callback=self.measurement_cb)
self.map_pub = rospy.Publisher("voxel_map", PointCloud, queue_size=1)
self.vis_pub = rospy.Publisher("voxel_map_visual",
PointCloud,
queue_size=1)
self.state = State()
self.blank_cloud = PointCloud()
zero_point = Point32(0, 0, 0)
self.blank_cloud.points = [zero_point] * self.map.N**3
def pointcloud_to_voxelmap_with_map(pointcloud,
camera_pos=np.array([0, 0, 0]),
voxel_size=0.25,
free_update=-1.0,
hit_update=2.0):
map_obj = VoxelMap()
map_obj.voxel_size = voxel_size
map_obj.free_update = free_update
map_obj.hit_update = hit_update # zkusit 2násobný hit oproti free, zkusit include directories env var
map_obj.occupancy_threshold = 0.0
line_starts = np.repeat(camera_pos[:, np.newaxis],
pointcloud.shape[1],
axis=1)
map_obj.update_lines(line_starts, pointcloud)
[voxels, levels, values] = map_obj.get_voxels()
# size je počet známých voxelů, počet prvků v hashmapě
# můžu získávat i hodnoty konkrétních voxelů přes map.get_voxels(voxels, levels)
# voxely zobrazovat jako pointcloud
# do get_voxels mohu poslat body v deformovaných NDC součadicích a voxelmapa mi sama přes nearest neighbour najde odpovídající voxely, takže nemusím řešit deformaci při transformaci dat do pohledu hlavní kamery
return voxels, values, map_obj.voxel_size, map_obj
def playing_with_voxelmap():
map = VoxelMap()
map.voxel_size = 1
map.free_update = -1.0
map.hit_update = 1.0
map.occupancy_threshold = 0.5
x0 = np.array([
[1, 1],
[1, 1],
[1, 1],
], dtype=np.float32)
x1 = np.array([
[4, 5],
[4, 3],
[2, 1],
], dtype=np.float32)
map.update_lines(x0, x1)
[voxels, levels, values] = map.get_voxels()
# ret = map.trace_lines()
with open('voxels.rick', 'wb+') as f:
pickle.dump([voxels, values], f)
class VoxelMapNode:
def __init__(self):
ns = 'voxel_map/'
self._visual_threshold = rospy.get_param(ns + 'visual_threshold')
voxels_per_side = rospy.get_param(ns + 'voxels_per_side')
width_on_a_side = rospy.get_param(ns + 'width_on_a_side')
initial_prob = rospy.get_param(ns + 'initial_prob')
pD = rospy.get_param(ns + 'pD')
pFA = rospy.get_param(ns + 'pFA')
ps = rospy.get_param(ns + 'ps')
pt = rospy.get_param(ns + 'pt')
self.map = VoxelMap(voxels_per_side, width_on_a_side, initial_prob, pD,
pFA, ps, pt)
rospy.Subscriber("truth/NED",
Odometry,
callback=self.state_cb,
queue_size=1)
rospy.Subscriber("triang_points",
PointCloud,
callback=self.measurement_cb)
self.map_pub = rospy.Publisher("voxel_map", PointCloud, queue_size=1)
self.vis_pub = rospy.Publisher("voxel_map_visual",
PointCloud,
queue_size=1)
self.map_center_pub = rospy.Publisher("voxel_map_center", Odometry)
self.state = State()
self.blank_cloud = PointCloud()
zero_point = Point32(0, 0, 0)
self.blank_cloud.points = [zero_point] * self.map.N**3
def run(self):
rospy.spin()
def state_cb(self, msg):
position = msg.pose.pose.position
shift = np.array([
position.x - self.state.pos[0], position.y - self.state.pos[1],
position.z - self.state.pos[2]
])
self.map.shift_map(shift)
self.state.pos = np.array([position.x, position.y, position.z])
self.publish_map_center()
def measurement_cb(self, msg):
num_points = len(msg.points)
if num_points > 0:
measurements_in = np.zeros((num_points, 3))
# There may be a slicker way to fill up this array.
for i in range(num_points):
measurements_in[i, 0] = msg.points[i].x - self.state.pos[0]
measurements_in[i, 1] = msg.points[i].y - self.state.pos[1]
measurements_in[i, 2] = msg.points[i].z - self.state.pos[2]
self.map.update_map(measurements_in)
else:
self.map.update_map()
# We may want to change this to where we only publsh at the voxel map at a set frequency
# instead of as soon as we get any updates to our measurements
self.publish_map()
def publish_map(self):
out_cloud = copy.deepcopy(self.blank_cloud)
out_cloud.header.stamp = rospy.Time.now()
out_cloud.header.frame_id = "ned"
N = self.map.N
offset = self.map.offset
voxel_width = self.map.voxel_width
point_it = 0
# TODO Get these map probabilities into a pointcloud without a loop
for k in range(N):
for j in range(N):
for i in range(N):
out_cloud.points[point_it] = (Point32(
voxel_width * (i - offset) + self.state.pos[0],
voxel_width * (j - offset) + self.state.pos[1],
voxel_width * (k - offset) + self.state.pos[2]))
out_cloud.channels.append(ChannelFloat32())
out_cloud.channels[point_it].name = "occupancy probability"
out_cloud.channels[point_it].values.append(
self.map.prob_map[i, j, k])
point_it += 1
self.publish_visual(out_cloud)
self.map_pub.publish(out_cloud)
def publish_visual(self, probability_cloud):
vis_cloud = PointCloud()
vis_cloud.header.stamp = rospy.Time.now()
vis_cloud.header.frame_id = "ned"
num_points = len(probability_cloud.points)
for i in range(num_points):
probability = probability_cloud.channels[i].values[0]
#print("probability: " , probability)
if (probability > self._visual_threshold):
vis_cloud.points.append(probability_cloud.points[i])
self.vis_pub.publish(vis_cloud)
def publish_map_center(self):
out_odom = Odometry()
out_odom.header.stamp = rospy.Time.now()
out_odom.header.frame_id = "ned"
out_odom.pose.pose.position.x = self.state.pos[0] - self.map.residual[0]
out_odom.pose.pose.position.y = self.state.pos[1] - self.map.residual[1]
out_odom.pose.pose.position.z = self.state.pos[2] - self.map.residual[2]
self.map_center_pub.publish(out_odom)
class TestVoxelMap(unittest.TestCase):
def setUp(self):
self.map = VoxelMap()
self.map.voxel_size = 0.5
self.map.free_update = -1.0
self.map.hit_update = 2.0
self.map.occupancy_threshold = 0.0
self.assertIsNotNone(self.map)
def test_setget_voxels(self):
n = 5
x0 = 10 * np.random.rand(3, n)
l0 = np.zeros((n, ), dtype=np.float64)
v0 = 2 * np.random.rand(n) - 1
print(x0, l0, v0)
self.map.set_voxels(x0, l0, v0)
print(x0, l0, v0)
v1 = self.map.get_voxels(x0, l0)
for el0, el1 in zip(v0.tolist(), v1.tolist()):
self.assertAlmostEqual(el0, el1)
[x2, l2, v2] = self.map.get_voxels()
v3 = self.map.get_voxels(x2, l2)
for el0, el1 in zip(v2.tolist(), v3.tolist()):
self.assertEqual(el0, el1)
def test_update_lines(self):
n = 2
x0 = np.zeros((3, n), dtype=np.float64)
x1 = 10 * np.random.rand(3, n)
self.map.update_lines(x0, x1)
[x2, l2, v2] = self.map.get_voxels()
def test_pass(self):
pass
def test_pickle(self):
x0 = np.zeros((3, 10))
x1 = np.random.uniform(-5, 5, (3, 10))
self.map.update_lines(x0, x1)
fd, path = tempfile.mkstemp()
try:
with os.fdopen(fd, 'wb') as f:
pickle.dump(self.map, f)
with open(path, 'rb') as f:
loaded = pickle.load(f)
self.assertEqual(self.map.voxel_size, loaded.voxel_size)
self.assertEqual(self.map.free_update, loaded.free_update)
self.assertEqual(self.map.hit_update, loaded.hit_update)
self.assertEqual(self.map.occupied_threshold,
loaded.occupied_threshold)
x, _, val = self.map.get_voxels()
x_loaded, _, val_loaded = loaded.get_voxels()
self.assertEqual(x.ndim, x_loaded.ndim)
self.assertEqual(x.min(), x_loaded.min())
self.assertEqual(x.max(), x_loaded.max())
self.assertEqual(val.ndim, val_loaded.ndim)
self.assertEqual(val.min(), val_loaded.min())
self.assertEqual(val.max(), val_loaded.max())
finally:
os.remove(path)
class OccupancyMap(object):
def __init__(self, frame_id, resolution=0.1):
self.voxel_map = VoxelMap(resolution, -1.0, 2.0, 0.0)
self.min = -10.0
self.max = 10.0
self.msg = OccupancyGrid()
self.msg.header.frame_id = frame_id
self.msg.info.resolution = resolution
# Initially, set the grid origin to identity.
self.msg.info.origin.orientation.w = 1.0
def map_to_grid(self, x):
"""Transform points from map coordinates to grid."""
# TODO: Handle orientation too.
x = x - col(array(self.msg.info.origin.position))
return x
def grid_to_map(self, x):
"""Transform points from grid coordinates to map."""
# TODO: Handle orientation too.
x = x + col(array(self.msg.info.origin.position))
return x
def fit_grid(self):
"""Accommodate the grid to contain all points."""
# Update grid origin so that all coordinates are non-negative.
x, _, v = self.voxel_map.get_voxels()
x = x[:2] # Only x,y used in 2D grid.
x_min = x.min(axis=1) - self.voxel_map.voxel_size / 2.
x_max = x.max(axis=1) + self.voxel_map.voxel_size / 2.
nx = np.round(
(x_max - x_min) / self.msg.info.resolution).astype(np.int)
self.msg.info.origin.position = Point(x_min[0], x_min[1], 0.0)
self.msg.info.width, self.msg.info.height = nx
def grid_voxels(self):
"""Return voxel coordinates corresponding to the current grid."""
i, j = np.meshgrid(np.arange(self.msg.info.width),
np.arange(self.msg.info.height),
indexing='xy')
x = np.stack((i.ravel(), j.ravel(), np.zeros_like(i).ravel()))
x = (x + 0.5) * self.msg.info.resolution
return x
def to_msg(self):
"""Return as grid message. (Update grid parameters as needed.)"""
self.fit_grid()
x = self.grid_voxels()
x = self.grid_to_map(x)
x[2, :] = self.voxel_map.voxel_size / 2.0
l = np.zeros((x.shape[1], ))
v = self.voxel_map.get_voxels(x, l)
v = 100. * logistic(v)
v[np.isnan(v)] = -1.
self.msg.data = v.astype(int).tolist()
return self.msg
def voxel_map_points(self, x):
x = points_3d(x.copy())
x[2, :] = self.voxel_map.voxel_size / 2.0
return x
def update(self, x, y, stamp):
"""Update internal occupancy map."""
x = self.voxel_map_points(x)
y = self.voxel_map_points(y)
if x.shape[1] == 1:
x = np.broadcast_to(x, y.shape)
elif y.shape[1] == 1:
y = np.broadcast_to(y, x.shape)
self.voxel_map.update_lines(x, y)
self.clip_values()
self.msg.header.stamp = stamp
self.msg.info.map_load_time = stamp
def occupied(self, x):
x = self.voxel_map_points(x)
l = np.zeros((x.shape[1], ))
v = self.voxel_map.get_voxels(x, l)
occupied = v > self.voxel_map.occupied_threshold
return occupied
def clip_values(self):
x, l, v = self.voxel_map.get_voxels()
v = np.clip(v, self.min, self.max)
self.voxel_map.set_voxels(x, l, v)