def test_vector_to_camera_view(location, expected):
from pylot.drivers.sensor_setup import CameraSetup
camera_setup = CameraSetup('test_camera',
'sensor.camera.rgb',
width=1920,
height=1080,
transform=Transform(Location(), Rotation()))
location = Location(*location)
assert all(
np.isclose(
location.to_camera_view(
camera_setup.get_extrinsic_matrix(),
camera_setup.get_intrinsic_matrix()).as_numpy_array(),
expected)), "The camera transformation was not as expected."
def get_pixel_location(self, pixel, camera_setup: CameraSetup):
""" Gets the 3D world location from pixel coordinates.
Args:
pixel (:py:class:`~pylot.utils.Vector2D`): Pixel coordinates.
camera_setup (:py:class:`~pylot.drivers.sensor_setup.CameraSetup`):
The setup of the camera with its transform in the world frame
of reference.
Returns:
:py:class:`~pylot.utils.Location`: The 3D world location, or None
if all the point cloud points are behind.
"""
# Select only points that are in front.
# Setting the threshold to 0.1 because super close points cause
# floating point errors.
fwd_points = self.points[np.where(self.points[:, 2] > 0.1)]
if len(fwd_points) == 0:
return None
intrinsic_mat = camera_setup.get_intrinsic_matrix()
# Project our 2D pixel location into 3D space, onto the z=1 plane.
p3d = np.dot(inv(intrinsic_mat), np.array([[pixel.x], [pixel.y],
[1.0]]))
if self._lidar_setup.lidar_type == 'sensor.lidar.ray_cast':
location = PointCloud.get_closest_point_in_point_cloud(
fwd_points, Vector2D(p3d[0], p3d[1]), normalized=True)
# Use the depth from the retrieved location.
p3d *= np.array([location.z])
p3d = p3d.transpose()
# Convert from camera to unreal coordinates if the lidar type is
# sensor.lidar.ray_cast
to_world_transform = camera_setup.get_unreal_transform()
camera_point_cloud = to_world_transform.transform_points(p3d)[0]
pixel_location = Location(camera_point_cloud[0],
camera_point_cloud[1],
camera_point_cloud[2])
elif self._lidar_setup.lidar_type == 'velodyne':
location = PointCloud.get_closest_point_in_point_cloud(
fwd_points, Vector2D(p3d[0], p3d[1]), normalized=False)
# Use the depth from the retrieved location.
p3d[2] = location.z
p3d = p3d.transpose()
pixel_location = Location(p3d[0, 0], p3d[0, 1], p3d[0, 2])
return pixel_location
def on_camera_msg(carla_image):
game_time = int(carla_image.timestamp * 1000)
print("Received camera msg {}".format(game_time))
camera_transform = pylot.utils.Transform.from_carla_transform(
carla_image.transform)
camera_setup = CameraSetup("rgb_camera",
"sensor.camera.rgb",
800,
600,
camera_transform,
fov=90.0)
global last_frame
last_frame = CameraFrame.from_carla_frame(carla_image, camera_setup)
def test_get_pixel_locations(depth_frame, pixels, expected):
height, width = depth_frame.shape
camera_setup = CameraSetup('test_setup',
'sensor.camera.depth',
width,
height,
Transform(location=Location(0, 0, 0),
rotation=Rotation(0, 0, 0)),
fov=90)
depth_frame = DepthFrame(depth_frame, camera_setup)
locations = depth_frame.get_pixel_locations(pixels)
for i in range(len(pixels)):
assert np.isclose(locations[i].x, expected[i].x), 'Returned x '
'value is not the same as expected'
assert np.isclose(locations[i].y, expected[i].y), 'Returned y '
'value is not the same as expected'
assert np.isclose(locations[i].z, expected[i].z), 'Returned z '
'value is not the same as expected'
def test_depth_to_point_cloud(depth_frame, expected):
height, width = depth_frame.shape
camera_setup = CameraSetup('test_setup',
'sensor.camera.depth',
width,
height,
Transform(location=Location(0, 0, 0),
rotation=Rotation(0, 0, 0)),
fov=90)
depth_frame = DepthFrame(depth_frame, camera_setup)
# Resulting unreal coordinates.
point_cloud = depth_frame.as_point_cloud()
for i in range(width * height):
assert np.isclose(point_cloud[i][0], expected[i][0]), \
'Returned x value is not the same as expected'
assert np.isclose(point_cloud[i][1], expected[i][1]), \
'Returned y value is not the same as expected'
assert np.isclose(point_cloud[i][2], expected[i][2]), \
'Returned z value is not the same as expected'
def test_point_cloud_get_pixel_location(lidar_points, pixel, expected):
camera_setup = CameraSetup(
'test_setup',
'sensor.camera.depth',
801,
601, # width, height
Transform(location=Location(0, 0, 0), rotation=Rotation(0, 0, 0)),
fov=90)
lidar_setup = LidarSetup('lidar', 'sensor.lidar.ray_cast',
Transform(Location(), Rotation()))
point_cloud = PointCloud(lidar_points, lidar_setup)
location = point_cloud.get_pixel_location(pixel, camera_setup)
assert np.isclose(location.x,
expected.x), 'Returned x value is not the same '
'as expected'
assert np.isclose(location.y,
expected.y), 'Returned y value is not the same '
'as expected'
assert np.isclose(location.z,
expected.z), 'Returned z value is not the same '
'as expected'
def on_lidar_msg(carla_pc):
game_time = int(carla_pc.timestamp * 1000)
print("Received lidar msg {}".format(game_time))
lidar_transform = pylot.utils.Transform.from_carla_transform(
carla_pc.transform)
lidar_setup = LidarSetup('lidar',
lidar_type='sensor.lidar.ray_cast',
transform=lidar_transform)
point_cloud = PointCloud.from_carla_point_cloud(carla_pc, lidar_setup)
camera_setup = CameraSetup("lidar_camera",
"sensor.camera.depth",
800,
600,
lidar_transform,
fov=90.0)
for (x, y) in pixels_to_check:
pixel = pylot.utils.Vector2D(x, y)
location = point_cloud.get_pixel_location(pixel, camera_setup)
print("{} Computed using lidar {}".format((x, y), location))
global lidar_pc
lidar_pc = point_cloud
def on_depth_msg(simulator_image):
game_time = int(simulator_image.timestamp * 1000)
print("Received depth camera msg {}".format(game_time))
depth_camera_transform = pylot.utils.Transform.from_simulator_transform(
simulator_image.transform)
camera_setup = CameraSetup("depth_camera",
"sensor.camera.depth",
800,
600,
depth_camera_transform,
fov=90.0)
depth_frame = DepthFrame.from_simulator_frame(simulator_image, camera_setup)
for (x, y) in pixels_to_check:
print("{} Depth at pixel {}".format((x, y), depth_frame.frame[y][x]))
pos3d_depth = depth_frame.get_pixel_locations(
[pylot.utils.Vector2D(x, y)])[0]
print("{} Computed using depth map {}".format((x, y), pos3d_depth))
global depth_pc
depth_pc = depth_frame.as_point_cloud()