class TestVisionSensors(TestCore):
def setUp(self):
super().setUp()
[self.pyrep.step() for _ in range(10)]
self.cam = VisionSensor('cam0')
def test_handle_explicitly(self):
cam = VisionSensor.create((640, 480))
# blank image
rgb = cam.capture_rgb()
self.assertEqual(rgb.sum(), 0)
# non-blank image
cam.set_explicit_handling(value=1)
cam.handle_explicitly()
rgb = cam.capture_rgb()
self.assertNotEqual(rgb.sum(), 0)
cam.remove()
def test_capture_rgb(self):
img = self.cam.capture_rgb()
self.assertEqual(img.shape, (16, 16, 3))
# Check that it's not a blank image
self.assertFalse(img.min() == img.max() == 0.0)
def test_capture_depth(self):
img = self.cam.capture_depth()
self.assertEqual(img.shape, (16, 16))
# Check that it's not a blank depth map
self.assertFalse(img.min() == img.max() == 1.0)
def test_create(self):
cam = VisionSensor.create([640, 480],
perspective_mode=True,
view_angle=35.0,
near_clipping_plane=0.25,
far_clipping_plane=5.0,
render_mode=RenderMode.OPENGL3)
self.assertEqual(cam.capture_rgb().shape, (480, 640, 3))
self.assertEqual(cam.get_perspective_mode(),
PerspectiveMode.PERSPECTIVE)
self.assertAlmostEqual(cam.get_perspective_angle(), 35.0, 3)
self.assertEqual(cam.get_near_clipping_plane(), 0.25)
self.assertEqual(cam.get_far_clipping_plane(), 5.0)
self.assertEqual(cam.get_render_mode(), RenderMode.OPENGL3)
def test_get_set_resolution(self):
self.cam.set_resolution([320, 240])
self.assertEqual(self.cam.get_resolution(), [320, 240])
self.assertEqual(self.cam.capture_rgb().shape, (240, 320, 3))
def test_get_set_perspective_mode(self):
for perspective_mode in PerspectiveMode:
self.cam.set_perspective_mode(perspective_mode)
self.assertEqual(self.cam.get_perspective_mode(), perspective_mode)
def test_get_set_render_mode(self):
for render_mode in [RenderMode.OPENGL, RenderMode.OPENGL3]:
self.cam.set_render_mode(render_mode)
self.assertEqual(self.cam.get_render_mode(), render_mode)
def test_get_set_perspective_angle(self):
self.cam.set_perspective_angle(45.0)
self.assertAlmostEqual(self.cam.get_perspective_angle(), 45.0, 3)
def test_get_set_orthographic_size(self):
self.cam.set_orthographic_size(1.0)
self.assertEqual(self.cam.get_orthographic_size(), 1.0)
def test_get_set_near_clipping_plane(self):
self.cam.set_near_clipping_plane(0.5)
self.assertEqual(self.cam.get_near_clipping_plane(), 0.5)
def test_get_set_far_clipping_plane(self):
self.cam.set_far_clipping_plane(0.5)
self.assertEqual(self.cam.get_far_clipping_plane(), 0.5)
def test_get_set_windowed_size(self):
self.cam.set_windowed_size((640, 480))
self.assertEqual(self.cam.get_windowed_size(), (640, 480))
def test_get_set_entity_to_render(self):
self.cam.set_entity_to_render(-1)
self.assertEqual(self.cam.get_entity_to_render(), -1)
class SphericalVisionSensor(Object):
"""An object able capture 360 degree omni-directional images.
"""
def __init__(self, name):
super().__init__(name)
# final omni-directional sensors
self._sensor_depth = VisionSensor('%s_sensorDepth' % (self.get_name()))
self._sensor_rgb = VisionSensor('%s_sensorRGB' % (self.get_name()))
# directed sub-sensors
names = ['front', 'top', 'back', 'bottom', 'left', 'right']
self._six_sensors = [VisionSensor('%s_%s' % (self.get_name(), n)) for n in names]
self._front = self._six_sensors[0]
# directed sub-sensor handles list
self._six_sensor_handles = [vs.get_handle() for vs in self._six_sensors]
# set all to explicit handling
self._set_all_to_explicit_handling()
# assert sensor properties are matching
self._assert_matching_resolutions()
self._assert_matching_render_modes()
self._assert_matching_entities_to_render()
self._assert_matching_window_sizes()
self._assert_matching_near_clipping_planes()
self._assert_matching_far_clipping_planes()
# omni resolution
self._omni_resolution = self._sensor_rgb.get_resolution()
# near and far clipping plane
self._near_clipping_plane = self.get_near_clipping_plane()
self._far_clipping_plane = self.get_far_clipping_plane()
self._clipping_plane_diff = self._far_clipping_plane - self._near_clipping_plane
# projective cam region image
self._planar_to_radial_depth_scalars = self._get_planar_to_radial_depth_scalars()
# flip config
self._flip_x = True
# Private #
# --------#
def _set_all_to_explicit_handling(self):
self._sensor_depth.set_explicit_handling(1)
self._sensor_rgb.set_explicit_handling(1)
for sensor in self._six_sensors:
sensor.set_explicit_handling(1)
def _assert_matching_resolutions(self):
assert self._sensor_depth.get_resolution() == self._sensor_rgb.get_resolution()
front_sensor_res = self._front.get_resolution()
for sensor in self._six_sensors[1:]:
assert sensor.get_resolution() == front_sensor_res
def _assert_matching_render_modes(self):
assert self._sensor_depth.get_render_mode() == self._sensor_rgb.get_render_mode()
front_sensor_render_mode = self._front.get_render_mode()
for sensor in self._six_sensors[1:]:
assert sensor.get_render_mode() == front_sensor_render_mode
def _assert_matching_entities_to_render(self):
assert self._sensor_depth.get_entity_to_render() == self._sensor_rgb.get_entity_to_render()
front_sensor_entity_to_render = self._front.get_entity_to_render()
for sensor in self._six_sensors[1:]:
assert sensor.get_entity_to_render() == front_sensor_entity_to_render
def _assert_matching_window_sizes(self):
assert self._sensor_depth.get_windowed_size() == self._sensor_rgb.get_windowed_size()
def _assert_matching_near_clipping_planes(self):
front_sensor_near = self._front.get_near_clipping_plane()
for sensor in self._six_sensors[1:]:
assert sensor.get_near_clipping_plane() == front_sensor_near
def _assert_matching_far_clipping_planes(self):
front_sensor_far = self._front.get_far_clipping_plane()
for sensor in self._six_sensors[1:]:
assert sensor.get_far_clipping_plane() == front_sensor_far
def _get_requested_type(self) -> ObjectType:
return ObjectType(sim.simGetObjectType(self.get_handle()))
@staticmethod
def _create_uniform_pixel_coords_image(image_dims):
pixel_x_coords = np.reshape(np.tile(np.arange(image_dims[1]), [image_dims[0]]),
(image_dims[0], image_dims[1], 1)).astype(np.float32)
pixel_y_coords_ = np.reshape(np.tile(np.arange(image_dims[0]), [image_dims[1]]),
(image_dims[1], image_dims[0], 1)).astype(np.float32)
pixel_y_coords = np.transpose(pixel_y_coords_, (1, 0, 2))
return np.concatenate((pixel_x_coords, pixel_y_coords), -1)
def _get_planar_to_radial_depth_scalars(self):
# reference: https://en.wikipedia.org/wiki/Spherical_coordinate_system#Cartesian_coordinates
# polar coord image
coord_img = self._create_uniform_pixel_coords_image([self._omni_resolution[1], self._omni_resolution[0]])
pixels_per_rad = self._omni_resolution[1] / np.pi
polar_angles = coord_img / pixels_per_rad
phi = polar_angles[..., 0:1]
theta = polar_angles[..., 1:2]
# image borders wrt the 6 projective cameras
phis_rel = ((phi + np.pi/4) % (np.pi/2)) / (np.pi/2)
lower_borders = np.arccos(1/((phis_rel*2-1)**2 + 2)**0.5)
upper_borders = np.pi - lower_borders
# omni image regions from the 6 projective cameras
xy_region = np.logical_and(theta <= upper_borders, theta >= lower_borders)
pos_x = np.logical_and(xy_region, np.logical_or(phi <= 45 * np.pi/180, phi >= (45 + 270) * np.pi/180))
pos_y = np.logical_and(xy_region,
np.logical_and(phi >= 45 * np.pi/180, phi <= (45 + 90) * np.pi/180))
neg_x = np.logical_and(xy_region,
np.logical_and(phi >= (45 + 90) * np.pi/180, phi <= (45 + 180) * np.pi/180))
neg_y = np.logical_and(xy_region,
np.logical_and(phi >= (45 + 180) * np.pi/180, phi <= (45 + 270) * np.pi/180))
pos_z = theta <= lower_borders
neg_z = theta >= upper_borders
# trig terms for conversion
sin_theta = np.sin(theta)
cos_theta = np.cos(theta)
sin_phi = np.sin(phi)
cos_phi = np.cos(phi)
# reciprocal terms
recip_sin_theta_cos_phi = 1/(sin_theta * cos_phi + MIN_DIVISOR)
recip_sin_theta_sin_phi = 1/(sin_theta * sin_phi + MIN_DIVISOR)
recip_cos_theta = 1/(cos_theta + MIN_DIVISOR)
# planar to radial depth scalars
return np.where(pos_x, recip_sin_theta_cos_phi,
np.where(neg_x, -recip_sin_theta_cos_phi,
np.where(pos_y, recip_sin_theta_sin_phi,
np.where(neg_y, -recip_sin_theta_sin_phi,
np.where(pos_z, recip_cos_theta,
np.where(neg_z, -recip_cos_theta,
np.zeros_like(recip_cos_theta)))))))[..., 0]
# Public #
# -------#
def handle_explicitly(self) -> None:
"""Handle spherical vision sensor explicitly.
This enables capturing image (e.g., capture_rgb())
without PyRep.step().
"""
utils.script_call('handleSpherical@PyRep', PYREP_SCRIPT_TYPE,
[self._sensor_depth._handle, self._sensor_rgb._handle] + self._six_sensor_handles, [], [], [])
def capture_rgb(self) -> np.ndarray:
"""Retrieves the rgb-image of a spherical vision sensor.
:return: A numpy array of size (width, height, 3)
"""
rgb = self._sensor_rgb.capture_rgb()
if self._flip_x:
return np.flip(rgb, 1)
return rgb
def capture_depth(self, in_meters=False) -> np.ndarray:
"""Retrieves the depth-image of a spherical vision sensor.
:param in_meters: Whether the depth should be returned in meters.
:return: A numpy array of size (width, height)
"""
planar_depth = self._sensor_depth.capture_rgb()[:, :, 0]*self._clipping_plane_diff + self._near_clipping_plane
radial_depth = planar_depth * self._planar_to_radial_depth_scalars
if in_meters:
if self._flip_x:
return np.flip(radial_depth, 1)
return radial_depth
scaled_depth = (radial_depth - self._near_clipping_plane)/self._clipping_plane_diff
if self._flip_x:
return np.flip(scaled_depth, 1)
return scaled_depth
def get_resolution(self) -> List[int]:
""" Return the spherical vision sensor's resolution.
:return: Resolution [x, y]
"""
return self._sensor_depth.get_resolution()
def set_resolution(self, resolution: List[int]) -> None:
""" Set the spherical vision sensor's resolution.
:param resolution: New resolution [x, y]
"""
if resolution[0] != 2*resolution[1]:
raise Exception('Spherical vision sensors must have an X resolution 2x the Y resolution')
if resolution[1] % 2 != 0:
raise Exception('Spherical vision sensors must have an even Y resolution')
box_sensor_resolution = [int(resolution[1]/2), int(resolution[1]/2)]
for sensor in self._six_sensors:
sensor.set_resolution(box_sensor_resolution)
self._sensor_depth.set_resolution(resolution)
self._sensor_rgb.set_resolution(resolution)
self._omni_resolution = resolution
self._planar_to_radial_depth_scalars = self._get_planar_to_radial_depth_scalars()
def get_render_mode(self) -> RenderMode:
""" Retrieves the spherical vision sensor's rendering mode
:return: RenderMode for the current rendering mode.
"""
return self._sensor_depth.get_render_mode()
def set_render_mode(self, render_mode: RenderMode) -> None:
""" Set the spherical vision sensor's rendering mode
:param render_mode: The new sensor rendering mode, one of:
RenderMode.OPENGL
RenderMode.OPENGL_AUXILIARY
RenderMode.OPENGL_COLOR_CODED
RenderMode.POV_RAY
RenderMode.EXTERNAL
RenderMode.EXTERNAL_WINDOWED
RenderMode.OPENGL3
RenderMode.OPENGL3_WINDOWED
"""
self._sensor_depth.set_render_mode(render_mode)
self._sensor_rgb.set_render_mode(render_mode)
for sensor in self._six_sensors:
sensor.set_render_mode(render_mode)
def get_windowed_size(self) -> Sequence[int]:
"""Get the size of windowed rendering for the omni-directional image.
:return: The (x, y) resolution of the window. 0 for full-screen.
"""
return self._sensor_depth.get_windowed_size()
def set_windowed_size(self, resolution: Sequence[int] = (0, 0)) -> None:
"""Set the size of windowed rendering for the omni-directional image.
:param resolution: The (x, y) resolution of the window.
0 for full-screen.
"""
self._sensor_depth.set_windowed_size(resolution)
self._sensor_rgb.set_windowed_size(resolution)
def get_near_clipping_plane(self) -> float:
""" Get the spherical depth sensor's near clipping plane.
:return: Near clipping plane (metres)
"""
return self._front.get_near_clipping_plane()
def set_near_clipping_plane(self, near_clipping: float) -> None:
""" Set the spherical depth sensor's near clipping plane.
:param near_clipping: New near clipping plane (in metres)
"""
self._near_clipping_plane = near_clipping
self._clipping_plane_diff = self._far_clipping_plane - near_clipping
for sensor in self._six_sensors:
sensor.set_near_clipping_plane(near_clipping)
def get_far_clipping_plane(self) -> float:
""" Get the Sensor's far clipping plane.
:return: Near clipping plane (metres)
"""
return self._front.get_far_clipping_plane()
def set_far_clipping_plane(self, far_clipping: float) -> None:
""" Set the spherical depth sensor's far clipping plane.
:param far_clipping: New far clipping plane (in metres)
"""
self._far_clipping_plane = far_clipping
self._clipping_plane_diff = far_clipping - self._near_clipping_plane
for sensor in self._six_sensors:
sensor.set_far_clipping_plane(far_clipping)
def set_entity_to_render(self, entity_to_render: int) -> None:
""" Set the entity to render to the spherical vision sensor,
this can be an object or more usefully a collection. -1 to render all objects in scene.
:param entity_to_render: Handle of the entity to render
"""
self._sensor_depth.set_entity_to_render(entity_to_render)
self._sensor_rgb.set_entity_to_render(entity_to_render)
def get_entity_to_render(self) -> None:
""" Get the entity to render to the spherical vision sensor,
this can be an object or more usefully a collection. -1 if all objects in scene are rendered.
:return: Handle of the entity to render
"""
return self._sensor_depth.get_entity_to_render()
class SphericalVisionSensor(Object):
"""An object able capture 360 degree omni-directional images.
"""
def __init__(self, name):
super().__init__(name)
# final omni-directional sensors
self._sensor_depth = VisionSensor('%s_sensorDepth' % (self.get_name()))
self._sensor_rgb = VisionSensor('%s_sensorRGB' % (self.get_name()))
# directed sub-sensors
names = ['front', 'top', 'back', 'bottom', 'left', 'right']
self._six_sensors = [
VisionSensor('%s_%s' % (self.get_name(), n)) for n in names
]
self._front = self._six_sensors[0]
# directed sub-sensor handles list
self._six_sensor_handles = [
vs.get_handle() for vs in self._six_sensors
]
# set all to explicit handling
self._set_all_to_explicit_handling()
# assert sensor properties are matching
self._assert_matching_resolutions()
self._assert_matching_render_modes()
self._assert_matching_entities_to_render()
self._assert_matching_window_sizes()
self._assert_matching_near_clipping_planes()
self._assert_matching_far_clipping_planes()
# near and far clipping plane
self._near_clipping_plane = self.get_near_clipping_plane()
self._far_clipping_plane = self.get_far_clipping_plane()
self._clipping_plane_diff = self._far_clipping_plane - self._near_clipping_plane
# Private #
# --------#
def _set_all_to_explicit_handling(self):
self._sensor_depth.set_explicit_handling(1)
self._sensor_rgb.set_explicit_handling(1)
for sensor in self._six_sensors:
sensor.set_explicit_handling(1)
def _assert_matching_resolutions(self):
assert self._sensor_depth.get_resolution(
) == self._sensor_rgb.get_resolution()
front_sensor_res = self._front.get_resolution()
for sensor in self._six_sensors[1:]:
assert sensor.get_resolution() == front_sensor_res
def _assert_matching_render_modes(self):
assert self._sensor_depth.get_render_mode(
) == self._sensor_rgb.get_render_mode()
front_sensor_render_mode = self._front.get_render_mode()
for sensor in self._six_sensors[1:]:
assert sensor.get_render_mode() == front_sensor_render_mode
def _assert_matching_entities_to_render(self):
assert self._sensor_depth.get_entity_to_render(
) == self._sensor_rgb.get_entity_to_render()
front_sensor_entity_to_render = self._front.get_entity_to_render()
for sensor in self._six_sensors[1:]:
assert sensor.get_entity_to_render(
) == front_sensor_entity_to_render
def _assert_matching_window_sizes(self):
assert self._sensor_depth.get_windowed_size(
) == self._sensor_rgb.get_windowed_size()
def _assert_matching_near_clipping_planes(self):
front_sensor_near = self._front.get_near_clipping_plane()
for sensor in self._six_sensors[1:]:
assert sensor.get_near_clipping_plane() == front_sensor_near
def _assert_matching_far_clipping_planes(self):
front_sensor_far = self._front.get_far_clipping_plane()
for sensor in self._six_sensors[1:]:
assert sensor.get_far_clipping_plane() == front_sensor_far
def _get_requested_type(self) -> ObjectType:
return ObjectType(sim.simGetObjectType(self.get_handle()))
# Public #
# -------#
def handle_explicitly(self) -> None:
"""Handle spherical vision sensor explicitly.
This enables capturing image (e.g., capture_rgb())
without PyRep.step().
"""
utils.script_call(
'handleSpherical@PyRep', PYREP_SCRIPT_TYPE,
[self._sensor_depth._handle, self._sensor_rgb._handle] +
self._six_sensor_handles, [], [], [])
def capture_rgb(self) -> np.ndarray:
"""Retrieves the rgb-image of a spherical vision sensor.
:return: A numpy array of size (width, height, 3)
"""
return self._sensor_rgb.capture_rgb()
def capture_depth(self, in_meters=False) -> np.ndarray:
"""Retrieves the depth-image of a spherical vision sensor.
:param in_meters: Whether the depth should be returned in meters.
:return: A numpy array of size (width, height)
"""
if in_meters:
return self._sensor_depth.capture_rgb(
)[:, :, 0] * self._clipping_plane_diff + self._near_clipping_plane
return self._sensor_depth.capture_rgb()[:, :, 0]
def get_resolution(self) -> List[int]:
""" Return the spherical vision sensor's resolution.
:return: Resolution [x, y]
"""
return self._sensor_depth.get_resolution()
def set_resolution(self, resolution: List[int]) -> None:
""" Set the spherical vision sensor's resolution.
:param resolution: New resolution [x, y]
"""
if resolution[0] != 2 * resolution[1]:
raise Exception(
'Spherical vision sensors must have an X resolution 2x the Y resolution'
)
if resolution[1] % 2 != 0:
raise Exception(
'Spherical vision sensors must have an even Y resolution')
box_sensor_resolution = [
int(resolution[1] / 2),
int(resolution[1] / 2)
]
for sensor in self._six_sensors:
sensor.set_resolution(box_sensor_resolution)
self._sensor_depth.set_resolution(resolution)
self._sensor_rgb.set_resolution(resolution)
def get_render_mode(self) -> RenderMode:
""" Retrieves the spherical vision sensor's rendering mode
:return: RenderMode for the current rendering mode.
"""
return self._sensor_depth.get_render_mode()
def set_render_mode(self, render_mode: RenderMode) -> None:
""" Set the spherical vision sensor's rendering mode
:param render_mode: The new sensor rendering mode, one of:
RenderMode.OPENGL
RenderMode.OPENGL_AUXILIARY
RenderMode.OPENGL_COLOR_CODED
RenderMode.POV_RAY
RenderMode.EXTERNAL
RenderMode.EXTERNAL_WINDOWED
RenderMode.OPENGL3
RenderMode.OPENGL3_WINDOWED
"""
self._sensor_depth.set_render_mode(render_mode)
self._sensor_rgb.set_render_mode(render_mode)
for sensor in self._six_sensors:
sensor.set_render_mode(render_mode)
def get_windowed_size(self) -> Sequence[int]:
"""Get the size of windowed rendering for the omni-directional image.
:return: The (x, y) resolution of the window. 0 for full-screen.
"""
return self._sensor_depth.get_windowed_size()
def set_windowed_size(self, resolution: Sequence[int] = (0, 0)) -> None:
"""Set the size of windowed rendering for the omni-directional image.
:param resolution: The (x, y) resolution of the window.
0 for full-screen.
"""
self._sensor_depth.set_windowed_size(resolution)
self._sensor_rgb.set_windowed_size(resolution)
def get_near_clipping_plane(self) -> float:
""" Get the spherical depth sensor's near clipping plane.
:return: Near clipping plane (metres)
"""
return self._front.get_near_clipping_plane()
def set_near_clipping_plane(self, near_clipping: float) -> None:
""" Set the spherical depth sensor's near clipping plane.
:param near_clipping: New near clipping plane (in metres)
"""
self._near_clipping_plane = near_clipping
self._clipping_plane_diff = self._far_clipping_plane - near_clipping
for sensor in self._six_sensors:
sensor.set_near_clipping_plane(near_clipping)
def get_far_clipping_plane(self) -> float:
""" Get the Sensor's far clipping plane.
:return: Near clipping plane (metres)
"""
return self._front.get_far_clipping_plane()
def set_far_clipping_plane(self, far_clipping: float) -> None:
""" Set the spherical depth sensor's far clipping plane.
:param far_clipping: New far clipping plane (in metres)
"""
self._far_clipping_plane = far_clipping
self._clipping_plane_diff = far_clipping - self._near_clipping_plane
for sensor in self._six_sensors:
sensor.set_far_clipping_plane(far_clipping)
def set_entity_to_render(self, entity_to_render: int) -> None:
""" Set the entity to render to the spherical vision sensor,
this can be an object or more usefully a collection. -1 to render all objects in scene.
:param entity_to_render: Handle of the entity to render
"""
self._sensor_depth.set_entity_to_render(entity_to_render)
self._sensor_rgb.set_entity_to_render(entity_to_render)
def get_entity_to_render(self) -> None:
""" Get the entity to render to the spherical vision sensor,
this can be an object or more usefully a collection. -1 if all objects in scene are rendered.
:return: Handle of the entity to render
"""
return self._sensor_depth.get_entity_to_render()
