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
bernat
parent
838e79ffca
commit
e6e651952a
|
|
@ -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.')
|
||||||
|
|
||||||
Loading…
Reference in New Issue