p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

check_lidar_bb: Added script used to check bounding boxes and lidar 

This script compares that all the points that lidar measure are inside
the bounding boxes of all the cars.
This is useful to check that both lidar colliders and BB are ok.
This commit is contained in:
Daniel Santos-Olivan 2021-02-17 14:53:30 +01:00 committed by bernat
parent 838e79ffca
commit e6e651952a
1 changed files with 371 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

371
PythonAPI/util/check_lidar_bb.py Normal file
View File

@ -0,0 +1,371 @@
#!/usr/bin/env python
# Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma de
# Barcelona (UAB).
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.
"""
Lidar/BB check for CARLA
This script obtains the LiDAR's point cloud corresponding to all the vehicles
of the scene and make sure that they are inside the bounding box of the
corresponding actor.
This is done in a predefined route in Town03 with a high speed and several agressive
turns.
In a nutshell, the script have a queue that is filled in each frame with a lidar point
cloud and an structure for storing the Bounding Boxes. This last one is emulated as a
sensor filling the queue in the on_tick callback of the carla.world. In this way, we make
sure that we are correctly syncronizing the lidar point cloud and BB/actor transformations.
Then, we select the points corresponding to each actor (car) in the scene and check they
are inside the bounding boxes of that actor, all in each vehicle frame of reference.
Important Data structure description:
+ Lidar data structure: four element tuple with:
- [0] Frame
- [1] Sensor name: 'semlidar'
- [2] Point cloud in the form of a numpy dictionary with all semantic lidar information
- [3] Global transformation of the sensor
+ Bounding box data structure: four element tuple with:
- [0] Frame
- [1] Sensor name: 'bb'
- [2] List of actor information: each a tuple with:
- [0] Actor id
- [1] Actor type (blueprint's name)
- [0] Actor's global transformation
- [0] Actor's bounding box
+ ActorTrace class: Takes the Lidar data structure and one actor information and
check if all the data points related with this actor are inside its BB.
This is done in the local coordinate frame of the actor and should be done like:
trace = ActorTrace(actor_info, lidar_data)
trace.process()
trace.check_lidar_data()
"""
import glob
import os
import sys
import numpy as np
from queue import Queue
from queue import Empty
try:
sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
sys.version_info.major,
sys.version_info.minor,
'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
pass
import carla
class ActorTrace(object):
"""Class that store and process information about an actor at certain moment."""
def __init__(self, actor, lidar):
self.set_lidar(lidar)
self.set_actor(actor)
self._lidar_pc_local = np.array([])
self._bb_vertices = np.array([])
self._bb_minlimits = [0, 0, 0]
self._bb_maxlimits = [0, 0, 0]
def set_lidar(self, lidar):
self._frame = lidar[0]
self._lidar_data = lidar[2]
self._lidar_transf = lidar[3]
def set_actor(self, actor):
self._actor_id = actor[0]
self._actor_type = actor[1]
self._actor_transf = actor[2]
self._actor_bb = actor[3]
def process(self):
# Filter lidar points that correspond to my actor id
data_actor = self._lidar_data[self._lidar_data['ObjIdx'] == self._actor_id]
# Take the xyz point cloud data and transform it to actor's frame
points = np.array([data_actor['x'], data_actor['y'], data_actor['z']]).T
points = np.append(points, np.ones((points.shape[0], 1)), axis=1)
points = np.dot(self._lidar_transf.get_matrix(), points.T).T # sensor -> world
points = np.dot(self._actor_transf.get_inverse_matrix(), points.T).T # world -> actor
points = points[:, :-1]
# Saving the points in 'local' coordinates
self._lidar_pc_local = points
# We compute the limits in the local frame of reference using the
# vertices of the bounding box
vertices = self._actor_bb.get_local_vertices()
ver_py = []
for v in vertices:
ver_py.append([v.x, v.y, v.z])
ver_np = np.array(ver_py)
self._bb_vertices = ver_np
self._bb_minlimits = ver_np.min(axis=0) - 0.001
self._bb_maxlimits = ver_np.max(axis=0) + 0.001
def print(self, print_if_empty = False):
if self._lidar_pc_local.shape[0] > 0 or print_if_empty:
np.savetxt("veh_data_%d_%s_%d.out" % (self._frame, self._actor_type, self._actor_id), self._lidar_pc_local)
np.savetxt("bb_data_%d_%s_%d.out" % (self._frame, self._actor_type, self._actor_id), self._bb_vertices)
def lidar_is_outside_bb(self, check_axis = [True, True, True]):
lidar_pc = self._lidar_pc_local
if check_axis[0]:
xmin = self._bb_minlimits[0]
xmax = self._bb_maxlimits[0]
out = np.any((lidar_pc[:,0] > xmax) | (lidar_pc[:,0] < xmin))
if out:
print("Problem with x axis")
return True
if check_axis[1]:
ymin = self._bb_minlimits[1]
ymax = self._bb_maxlimits[1]
out = np.any((lidar_pc[:, 1] > ymax) | (lidar_pc[:, 1] < ymin))
if out:
print("Problem with y axis")
return True
if check_axis[2]:
zmin = self._bb_minlimits[2]
zmax = self._bb_maxlimits[2]
out = np.any((lidar_pc[:, 2] > zmax) | (lidar_pc[:, 2] < zmin))
if out:
print("Problem with z axis")
return True
return False
def check_lidar_data(self):
if self.lidar_is_outside_bb():
print("Error!!! Points of lidar point cloud are outside its BB for car %d: %s " % (self._actor_id, self._actor_type))
self.print()
return False
else:
return True
def wait(world, frames=100, queue = None, slist = None):
for i in range(0, frames):
world.tick()
if queue != None and slist != None:
try:
for _i in range (0, len(slist)):
s_frame = queue.get(True, 1.0)
except Empty:
print(" Some of the sensor information is missed")
# Sensor callback.
# This is where you receive the sensor data and
# process it as you liked and the important part is that,
# at the end, it should include an element into the sensor queue.
def lidar_callback(sensor_data, sensor_queue, sensor_name):
sensor_pc_local = np.frombuffer(sensor_data.raw_data, dtype=np.dtype([
('x', np.float32), ('y', np.float32), ('z', np.float32),
('CosAngle', np.float32), ('ObjIdx', np.uint32), ('ObjTag', np.uint32)]))
sensor_transf = sensor_data.transform
sensor_queue.put((sensor_data.frame, sensor_name, sensor_pc_local, sensor_transf))
def bb_callback(snapshot, world, sensor_queue, sensor_name):
data_array = []
vehicles = world.get_actors().filter('vehicle.*')
for actor in vehicles:
data_array.append((actor.id, actor.type_id, actor.get_transform(), actor.bounding_box))
sensor_queue.put((snapshot.frame, sensor_name, data_array))
def move_spectator(world, actor):
actor_tr = actor.get_transform()
spectator_transform = carla.Transform(actor_tr.location, actor_tr.rotation)
spectator_transform.location -= actor_tr.get_forward_vector() * 5
spectator_transform.location -= actor_tr.get_up_vector() * 3
spectator = world.get_spectator()
spectator.set_transform(spectator_transform)
def world_callback(snapshot, world, sensor_queue, sensor_name, actor):
move_spectator(world, actor)
bb_callback(snapshot, world, sensor_queue, sensor_name)
def process_sensors(w_frame, sensor_queue, sensor_number):
if sensor_number != 2:
print("Error!!! Sensor number should be two")
sl_data = None
bb_data = None
try:
for i in range (0, sensor_number):
s_frame = sensor_queue.get(True, 1.0)
while s_frame[0] != w_frame:
print("Warning! Missmatch for sensor %s in the frame timestamp (w: %d, s: %d)" % (s_frame[1], w_frame, s_frame[0]))
print("This could be due to accumulated data for previous steps")
s_frame = sensor_queue.get(True, 1.0)
if s_frame[1] == "semlidar":
sl_data = s_frame
elif s_frame[1] == "bb":
bb_data = s_frame
#print(" Frame: %d Sensor: %s Len: %d " % (s_frame[0], s_frame[1], len(s_frame[2])))
except Empty:
print("Error!!! The needeinformation is not here!!!")
return
if sl_data == None or bb_data == None:
print("Error!!! Missmatch for sensor %s in the frame timestamp (w: %d, s: %d)" % (s_frame[1], w_frame, s_frame[0]))
for actor_data in bb_data[2]:
trace_vehicle = ActorTrace(actor_data, sl_data)
trace_vehicle.process()
trace_vehicle.check_lidar_data()
class SpawnCar(object):
def __init__(self, location, rotation, filter="vehicle.*", autopilot = False, velocity = None):
self._filter = filter
self._transform = carla.Transform(location, rotation)
self._autopilot = autopilot
self._velocity = velocity
self._actor = None
self._world = None
def spawn(self, world):
self._world = world
actor_BP = world.get_blueprint_library().filter(self._filter)[0]
self._actor = world.spawn_actor(actor_BP, self._transform)
self._actor.set_autopilot(True)
return self._actor
def destroy(self):
if self._actor != None:
self._actor.destroy()
CarPropList = [
SpawnCar(carla.Location(x=83, y= -40, z=5), carla.Rotation(yaw=-90), filter= "*lincoln*", autopilot=True),
SpawnCar(carla.Location(x=83, y= -30, z=3), carla.Rotation(yaw=-90), filter= "*ambulance*", autopilot=True),
SpawnCar(carla.Location(x=83, y= -20, z=3), carla.Rotation(yaw=-90), filter= "*etron*", autopilot=True),
SpawnCar(carla.Location(x=120, y= -3.5, z=2), carla.Rotation(yaw=+180), filter= "*isetta*", autopilot=True),
SpawnCar(carla.Location(x=100, y= -3.5, z=2), carla.Rotation(yaw=+180), filter= "*etron*", autopilot=True),
SpawnCar(carla.Location(x=140, y= -3.5, z=2), carla.Rotation(yaw=+180), filter= "*model3*", autopilot=True),
SpawnCar(carla.Location(x=160, y= -3.5, z=2), carla.Rotation(yaw=+180), filter= "*impala*", autopilot=False),
SpawnCar(carla.Location(x=180, y= -3.5, z=2), carla.Rotation(yaw=+180), filter= "*a2*", autopilot=True),
SpawnCar(carla.Location(x=60, y= +6, z=2), carla.Rotation(yaw=+00), filter= "*model3*", autopilot=True),
SpawnCar(carla.Location(x=80, y= +6, z=2), carla.Rotation(yaw=+00), filter= "*etron*", autopilot=True),
SpawnCar(carla.Location(x=100, y= +6, z=2), carla.Rotation(yaw=+00), filter= "*mustan*", autopilot=True),
SpawnCar(carla.Location(x=120, y= +6, z=2), carla.Rotation(yaw=+00), filter= "*isetta*", autopilot=True),
SpawnCar(carla.Location(x=140, y= +6, z=2), carla.Rotation(yaw=+00), filter= "*impala*", autopilot=True),
SpawnCar(carla.Location(x=160, y= +6, z=2), carla.Rotation(yaw=+00), filter= "*prius*", autopilot=True),
SpawnCar(carla.Location(x=234, y= +20,z=2), carla.Rotation(yaw=+90), filter= "*dodge*", autopilot=True),
SpawnCar(carla.Location(x=234, y= +40,z=2), carla.Rotation(yaw=+90), filter= "*isetta*", autopilot=True),
SpawnCar(carla.Location(x=234, y= +80,z=2), carla.Rotation(yaw=+90), filter= "*tt*", autopilot=True),
SpawnCar(carla.Location(x=243, y= -40,z=2), carla.Rotation(yaw=-90), filter= "*etron*", autopilot=True),
SpawnCar(carla.Location(x=243, y= -20,z=2), carla.Rotation(yaw=-90), filter= "*mkz2017*", autopilot=True),
SpawnCar(carla.Location(x=243, y= +00,z=2), carla.Rotation(yaw=-90), filter= "*mustan*", autopilot=True),
SpawnCar(carla.Location(x=243, y= +20,z=2), carla.Rotation(yaw=-90), filter= "*dodge*", autopilot=True),
SpawnCar(carla.Location(x=243, y= +40,z=2), carla.Rotation(yaw=-90), filter= "*charger2020*", autopilot=True),
SpawnCar(carla.Location(x=243, y= +60,z=2), carla.Rotation(yaw=-90), filter= "*lincoln2020*", autopilot=True),
SpawnCar(carla.Location(x=243, y= +80,z=2), carla.Rotation(yaw=-90), filter= "*tt*", autopilot=True),
SpawnCar(carla.Location(x=243, y=+100,z=2), carla.Rotation(yaw=-90), filter= "*a2*", autopilot=True),
SpawnCar(carla.Location(x=243, y=+120,z=2), carla.Rotation(yaw=-90), filter= "*wrangler_rubicon*", autopilot=True),
SpawnCar(carla.Location(x=243, y=+140,z=2), carla.Rotation(yaw=-90), filter= "*c3*", autopilot=True)
]
def spawn_prop_vehicles(world):
for car in CarPropList:
car.spawn(world)
def destroy_prop_vehicles():
for car in CarPropList:
car.destroy()
def main():
# We start creating the client
client = carla.Client('localhost', 2000)
client.set_timeout(2.0)
world = client.get_world()
try:
# We need to save the settings to be able to recover them at the end
# of the script to leave the server in the same state that we found it.
original_settings = world.get_settings()
settings = world.get_settings()
# We set CARLA syncronous mode
settings.fixed_delta_seconds = 0.05
settings.synchronous_mode = True
world.apply_settings(settings)
traffic_manager = client.get_trafficmanager(8000)
traffic_manager.set_synchronous_mode(True)
# We create the sensor queue in which we keep track of the information
# already received. This structure is thread safe and can be
# accessed by all the sensors callback concurrently without problem.
sensor_queue = Queue()
# Spawning ego vehicle
actor_BP = world.get_blueprint_library().filter("vehicle.lincoln.mkz2017")[0]
car_tr = carla.Transform(carla.Location(x=239, y=125, z=0.9), carla.Rotation(yaw=-88.5))
actor = world.spawn_actor(actor_BP, car_tr)
world.tick()
move_spectator(world, actor)
spawn_prop_vehicles(world)
wait(world, 10)
# We create all the sensors and keep them in a list for convenience.
sensor_list = []
lidar_bp = world.get_blueprint_library().find('sensor.lidar.ray_cast_semantic')
lidar_bp.set_attribute('channels', '64')
lidar_bp.set_attribute('points_per_second', '500000')
lidar_bp.set_attribute('range', '300')
lidar_bp.set_attribute('upper_fov', '10.0')
lidar_bp.set_attribute('lower_fov', '-90.0')
lidar_tr = carla.Transform(carla.Location(z=3), carla.Rotation(yaw=0))
lidar = world.spawn_actor(lidar_bp, lidar_tr, attach_to=actor)
lidar.listen(lambda data: lidar_callback(data, sensor_queue, "semlidar"))
world.on_tick(lambda snapshot: world_callback(snapshot, world, sensor_queue, "bb", actor))
sensor_list.append(lidar)
sensor_list.append(actor) # actor acts as a 'sensor' to simplify bb-lidar data comparison
# Set autopilot for main vehicle
actor.enable_constant_velocity(carla.Vector3D(20, 0, 0))
for _i in range(0, 100):
# Tick the server
world.tick()
w_frame = world.get_snapshot().frame
process_sensors(w_frame, sensor_queue, len(sensor_list))
actor.disable_constant_velocity()
finally:
world.apply_settings(original_settings)
# Destroy all the actors
destroy_prop_vehicles()
for sensor in sensor_list:
sensor.destroy()
if __name__ == "__main__":
try:
main()
except KeyboardInterrupt:
print(' - Exited by user.')