def __setup_camera_tranforms(self,
name,
postprocessing,
field_of_view=90.0,
image_size=(800, 600),
position=(0.3, 0, 1.3),
rotation_pitch=0,
rotation_roll=0,
rotation_yaw=0):
camera = Camera(name,
PostProcessing=postprocessing,
FOV=field_of_view,
ImageSizeX=image_size[0],
ImageSizeY=image_size[1],
PositionX=position[0],
PositionY=position[1],
PositionZ=position[2],
RotationPitch=rotation_pitch,
RotationRoll=rotation_roll,
RotationYaw=rotation_yaw)
image_width = image_size[0]
image_height = image_size[1]
# (Intrinsic) K Matrix
intrinsic_mat = np.identity(3)
intrinsic_mat[0][2] = image_width / 2
intrinsic_mat[1][2] = image_height / 2
intrinsic_mat[0][0] = intrinsic_mat[1][1] = image_width / (
2.0 * math.tan(90.0 * math.pi / 360.0))
return (intrinsic_mat, camera.get_unreal_transform(), (image_width,
image_height))
def run_carla_client( args):
# Here we will run 3 episodes with 300 frames each.
number_of_episodes = 150
frames_per_episode = 500
# for start_i in range(150):
# if start_i%4==0:
# output_folder = 'Packages/CARLA_0.8.2/PythonClient/new_data-viz/test_' + str(start_i)
# if not os.path.exists(output_folder):
# os.mkdir(output_folder)
# print( "make "+str(output_folder))
# ./CarlaUE4.sh -carla-server -benchmark -fps=17 -windowed
# carla-server "/usr/local/carla/Unreal/CarlaUE4/CarlaUE4.uproject" /usr/local/carla/Maps/Town03 -benchmark -fps=10 -windowed
# We assume the CARLA server is already waiting for a client to connect at
# host:port. To create a connection we can use the `make_carla_client`
# context manager, it creates a CARLA client object and starts the
# connection. It will throw an exception if something goes wrong. The
# context manager makes sure the connection is always cleaned up on exit.
with make_carla_client(args.host, args.port) as client:
print('CarlaClient connected')
global episode_nbr
print (episode_nbr)
for episode in range(0,number_of_episodes):
if episode % 1 == 0:
output_folder = 'Datasets/carla-sync/train/test_' + str(episode)
if not os.path.exists(output_folder+"/cameras.p"):
# Start a new episode.
episode_nbr=episode
frame_step = 1 # Save one image every 100 frames
pointcloud_step=50
image_size = [800, 600]
camera_local_pos = [0.3, 0.0, 1.3] # [X, Y, Z]
camera_local_rotation = [0, 0, 0] # [pitch(Y), yaw(Z), roll(X)]
fov = 70
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
settings.set(
SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=50,
NumberOfPedestrians=200,
WeatherId=random.choice([1, 3, 7, 8, 14]),
QualityLevel=args.quality_level)
settings.randomize_seeds()
# Now we want to add a couple of cameras to the player vehicle.
# We will collect the images produced by these cameras every
# frame.
camera1 = Camera('CameraDepth', PostProcessing='Depth', FOV=fov)
camera1.set_image_size(*image_size)
camera1.set_position(*camera_local_pos)
camera1.set_rotation(*camera_local_rotation)
settings.add_sensor(camera1)
camera2 = Camera('CameraRGB', PostProcessing='SceneFinal', FOV=fov)
camera2.set_image_size(*image_size)
camera2.set_position(*camera_local_pos)
camera2.set_rotation(*camera_local_rotation)
settings.add_sensor(camera2)
camera3 = Camera('CameraSeg', PostProcessing='SemanticSegmentation', FOV=fov)
camera3.set_image_size(*image_size)
camera3.set_position(*camera_local_pos)
camera3.set_rotation(*camera_local_rotation)
settings.add_sensor(camera3)
# Now we load these settings into the server. The server replies
# with a scene description containing the available start spots for
# the player. Here we can provide a CarlaSettings object or a
# CarlaSettings.ini file as string.
scene = client.load_settings(settings)
# Choose one player start at random.
number_of_player_starts = len(scene.player_start_spots)
player_start = episode#random.randint(0, max(0, number_of_player_starts - 1))
output_folder = 'Datasets/carla-sync/train/test_' + str(episode)
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print('Starting new episode...')
client.start_episode(player_start)
cameras_dict = {}
pedestrians_dict = {}
cars_dict = {}
# Compute the camera transform matrix
camera_to_car_transform = camera2.get_unreal_transform()
# (Intrinsic) (3, 3) K Matrix
K = np.identity(3)
K[0, 2] = image_size[0] / 2.0
K[1, 2] = image_size[1] / 2.0
K[0, 0] = K[1, 1] = image_size[0] / (2.0 * np.tan(fov * np.pi / 360.0))
with open(output_folder + '/camera_intrinsics.p', 'w') as camfile:
pickle.dump(K, camfile)
# Iterate every frame in the episode.
for frame in range(0, frames_per_episode):
# Read the data produced by the server this frame.
measurements, sensor_data = client.read_data()
# Print some of the measurements.
#print_measurements(measurements)
if not frame % frame_step:
# Save the images to disk if requested.
for name, measurement in sensor_data.items():
filename = args.out_filename_format.format(episode, name, frame)
print (filename)
measurement.save_to_disk(filename)
# We can access the encoded data of a given image as numpy
# array using its "data" property. For instance, to get the
# depth value (normalized) at pixel X, Y
#
# depth_array = sensor_data['CameraDepth'].data
# value_at_pixel = depth_array[Y, X]
#
# Now we have to send the instructions to control the vehicle.
# If we are in synchronous mode the server will pause the
# simulation until we send this control.
# RGB image [[[r,g,b],..[r,g,b]],..[[r,g,b],..[r,g,b]]]
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
labels=labels_to_array(sensor_data['CameraSeg'])[:,:,np.newaxis]
image_seg = np.tile(labels, (1, 1, 3))
depth_array = sensor_data['CameraDepth'].data*1000
# 2d to (camera) local 3d
# We use the image_RGB to colorize each 3D point, this is optional.
# "max_depth" is used to keep only the points that are near to the
# camera, meaning 1.0 the farest points (sky)
if not frame % pointcloud_step:
point_cloud = depth_to_local_point_cloud(
sensor_data['CameraDepth'],
image_RGB,
max_depth=args.far
)
point_cloud_seg = depth_to_local_point_cloud(
sensor_data['CameraDepth'],
segmentation=image_seg,
max_depth=args.far
)
# (Camera) local 3d to world 3d.
# Get the transform from the player protobuf transformation.
world_transform = Transform(
measurements.player_measurements.transform
)
# Compute the final transformation matrix.
car_to_world_transform = world_transform * camera_to_car_transform
# Car to World transformation given the 3D points and the
# transformation matrix.
point_cloud.apply_transform(car_to_world_transform)
point_cloud_seg.apply_transform(car_to_world_transform)
Rt = car_to_world_transform.matrix
Rt_inv = car_to_world_transform.inverse().matrix
# R_inv=world_transform.inverse().matrix
cameras_dict[frame] = {}
cameras_dict[frame]['inverse_rotation'] = Rt_inv[:]
cameras_dict[frame]['rotation'] = Rt[:]
cameras_dict[frame]['translation'] = Rt_inv[0:3, 3]
cameras_dict[frame]['camera_to_car'] = camera_to_car_transform.matrix
# Get non-player info
vehicles = {}
pedestrians = {}
for agent in measurements.non_player_agents:
# check if the agent is a vehicle.
if agent.HasField('vehicle'):
pos = agent.vehicle.transform.location
pos_vector = np.array([[pos.x], [pos.y], [pos.z], [1.0]])
trnasformed_3d_pos = np.dot(Rt_inv, pos_vector)
pos2d = np.dot(K, trnasformed_3d_pos[:3])
# Normalize the point
norm_pos2d = np.array([
pos2d[0] / pos2d[2],
pos2d[1] / pos2d[2],
pos2d[2]])
# Now, pos2d contains the [x, y, d] values of the image in pixels (where d is the depth)
# You can use the depth to know the points that are in front of the camera (positive depth).
x_2d = image_size[0] - norm_pos2d[0]
y_2d = image_size[1] - norm_pos2d[1]
vehicles[agent.id] = {}
vehicles[agent.id]['transform'] = [agent.vehicle.transform.location.x,
agent.vehicle.transform.location.y,
agent.vehicle.transform.location.z]
vehicles[agent.id][
'bounding_box.transform'] = agent.vehicle.bounding_box.transform.location.z
vehicles[agent.id]['yaw'] = agent.vehicle.transform.rotation.yaw
vehicles[agent.id]['bounding_box'] = [agent.vehicle.bounding_box.extent.x,
agent.vehicle.bounding_box.extent.y,
agent.vehicle.bounding_box.extent.z]
vehicle_transform = Transform(agent.vehicle.bounding_box.transform)
pos = agent.vehicle.transform.location
bbox3d = agent.vehicle.bounding_box.extent
# Compute the 3D bounding boxes
# f contains the 4 points that corresponds to the bottom
f = np.array([[pos.x + bbox3d.x, pos.y - bbox3d.y,
pos.z - bbox3d.z + agent.vehicle.bounding_box.transform.location.z],
[pos.x + bbox3d.x, pos.y + bbox3d.y,
pos.z - bbox3d.z + agent.vehicle.bounding_box.transform.location.z],
[pos.x - bbox3d.x, pos.y + bbox3d.y,
pos.z - bbox3d.z + agent.vehicle.bounding_box.transform.location.z],
[pos.x - bbox3d.x, pos.y - bbox3d.y,
pos.z - bbox3d.z + agent.vehicle.bounding_box.transform.location.z],
[pos.x + bbox3d.x, pos.y - bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.bounding_box.transform.location.z],
[pos.x + bbox3d.x, pos.y + bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.bounding_box.transform.location.z],
[pos.x - bbox3d.x, pos.y + bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.bounding_box.transform.location.z],
[pos.x - bbox3d.x, pos.y - bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.bounding_box.transform.location.z]])
f_rotated = vehicle_transform.transform_points(f)
f_2D_rotated = []
vehicles[agent.id]['bounding_box_coord'] = f_rotated
for i in range(f.shape[0]):
point = np.array([[f_rotated[i, 0]], [f_rotated[i, 1]], [f_rotated[i, 2]], [1]])
transformed_2d_pos = np.dot(Rt_inv, point)
pos2d = np.dot(K, transformed_2d_pos[:3])
norm_pos2d = np.array([
pos2d[0] / pos2d[2],
pos2d[1] / pos2d[2],
pos2d[2]])
# print([image_size[0] - (pos2d[0] / pos2d[2]), image_size[1] - (pos2d[1] / pos2d[2])])
f_2D_rotated.append(
np.array([image_size[0] - norm_pos2d[0], image_size[1] - norm_pos2d[1]]))
vehicles[agent.id]['bounding_box_2D'] = f_2D_rotated
elif agent.HasField('pedestrian'):
pedestrians[agent.id] = {}
pedestrians[agent.id]['transform'] = [agent.pedestrian.transform.location.x,
agent.pedestrian.transform.location.y,
agent.pedestrian.transform.location.z]
pedestrians[agent.id]['yaw'] = agent.pedestrian.transform.rotation.yaw
pedestrians[agent.id]['bounding_box'] = [agent.pedestrian.bounding_box.extent.x,
agent.pedestrian.bounding_box.extent.y,
agent.pedestrian.bounding_box.extent.z]
# get the needed transformations
# remember to explicitly make it Transform() so you can use transform_points()
pedestrian_transform = Transform(agent.pedestrian.transform)
bbox_transform = Transform(agent.pedestrian.bounding_box.transform)
# get the box extent
ext = agent.pedestrian.bounding_box.extent
# 8 bounding box vertices relative to (0,0,0)
bbox = np.array([
[ ext.x, ext.y, ext.z],
[- ext.x, ext.y, ext.z],
[ ext.x, - ext.y, ext.z],
[- ext.x, - ext.y, ext.z],
[ ext.x, ext.y, - ext.z],
[- ext.x, ext.y, - ext.z],
[ ext.x, - ext.y, - ext.z],
[- ext.x, - ext.y, - ext.z]
])
# transform the vertices respect to the bounding box transform
bbox = bbox_transform.transform_points(bbox)
# the bounding box transform is respect to the pedestrian transform
# so let's transform the points relative to it's transform
bbox = pedestrian_transform.transform_points(bbox)
# pedestrian's transform is relative to the world, so now,
# bbox contains the 3D bounding box vertices relative to the world
pedestrians[agent.id]['bounding_box_coord'] =copy.deepcopy(bbox)
# Additionally, you can print these vertices to check that is working
f_2D_rotated=[]
ys=[]
xs=[]
zs=[]
for vertex in bbox:
pos_vector = np.array([
[vertex[0,0]], # [[X,
[vertex[0,1]], # Y,
[vertex[0,2]], # Z,
[1.0] # 1.0]]
])
# transform the points to camera
transformed_3d_pos =np.dot(Rt_inv, pos_vector)# np.dot(inv(self._extrinsic.matrix), pos_vector)
zs.append(transformed_3d_pos[2])
# transform the points to 2D
pos2d =np.dot(K, transformed_3d_pos[:3]) #np.dot(self._intrinsic, transformed_3d_pos[:3])
# normalize the 2D points
pos2d = np.array([
pos2d[0] / pos2d[2],
pos2d[1] / pos2d[2],
pos2d[2]
])
# print the points in the screen
if pos2d[2] > 0: # if the point is in front of the camera
x_2d = image_size[0]-pos2d[0]#WINDOW_WIDTH - pos2d[0]
y_2d = image_size[1]-pos2d[1]#WINDOW_HEIGHT - pos2d[1]
ys.append(y_2d)
xs.append(x_2d)
f_2D_rotated.append( (y_2d, x_2d))
if len(xs)>1:
bbox=[[int(min(xs)), int(max(xs))],[int(min(ys)), int(max(ys))]]
clipped_seg=labels[bbox[1][0]:bbox[1][1],bbox[0][0]:bbox[0][1]]
recounted = Counter(clipped_seg.flatten())
if 4 in recounted.keys() and recounted[4]>0.1*len(clipped_seg.flatten()):
clipped_depth=depth_array[bbox[1][0]:bbox[1][1],bbox[0][0]:bbox[0][1]]
#print (clipped_depth.shape)
people_indx=np.where(clipped_seg==4)
masked_depth=[]
for people in zip(people_indx[0],people_indx[1] ):
#print(people)
masked_depth.append(clipped_depth[people])
#masked_depth=clipped_depth[np.where(clipped_seg==4)]
#print (masked_depth)
#print ("Depth "+ str(min(zs))+" "+ str(max(zs)))
#recounted = Counter(masked_depth)
#print(recounted)
avg_depth=np.mean(masked_depth)
if avg_depth<700 and avg_depth>=min(zs)-10 and avg_depth<= max(zs)+10:
#print("Correct depth")
pedestrians[agent.id]['bounding_box_2D'] = f_2D_rotated
pedestrians[agent.id]['bounding_box_2D_size']=recounted[4]
pedestrians[agent.id]['bounding_box_2D_avg_depth']=avg_depth
pedestrians[agent.id]['bounding_box_2D_depths']=zs
#print ( pedestrians[agent.id].keys())
#else:
# print(recounted)
# print ("Depth "+ str(min(zs))+" "+ str(max(zs)))
#if sum(norm(depth_array-np.mean(zs))<1.0):
# pedestrians[agent.id] = {}
# pedestrians[agent.id]['transform'] = [agent.pedestrian.transform.location.x,
# agent.pedestrian.transform.location.y,
# agent.pedestrian.transform.location.z]
# pedestrians[agent.id]['yaw'] = agent.pedestrian.transform.rotation.yaw
# pedestrians[agent.id]['bounding_box'] = [agent.pedestrian.bounding_box.extent.x,
# agent.pedestrian.bounding_box.extent.y,
# agent.pedestrian.bounding_box.extent.z]
# vehicle_transform = Transform(agent.pedestrian.bounding_box.transform)
# pos = agent.pedestrian.transform.location
#
# bbox3d = agent.pedestrian.bounding_box.extent
#
# # Compute the 3D bounding boxes
# # f contains the 4 points that corresponds to the bottom
# f = np.array([[pos.x + bbox3d.x, pos.y - bbox3d.y, pos.z- bbox3d.z ],
# [pos.x + bbox3d.x, pos.y + bbox3d.y, pos.z- bbox3d.z ],
# [pos.x - bbox3d.x, pos.y + bbox3d.y, pos.z- bbox3d.z ],
# [pos.x - bbox3d.x, pos.y - bbox3d.y, pos.z- bbox3d.z ],
# [pos.x + bbox3d.x, pos.y - bbox3d.y, pos.z + bbox3d.z],
# [pos.x + bbox3d.x, pos.y + bbox3d.y, pos.z + bbox3d.z],
# [pos.x - bbox3d.x, pos.y + bbox3d.y, pos.z + bbox3d.z],
# [pos.x - bbox3d.x, pos.y - bbox3d.y, pos.z + bbox3d.z]])
#
# f_rotated = pedestrian_transform.transform_points(f)
# pedestrians[agent.id]['bounding_box_coord'] = f_rotated
# f_2D_rotated = []
#
# for i in range(f.shape[0]):
# point = np.array([[f_rotated[i, 0]], [f_rotated[i, 1]], [f_rotated[i, 2]], [1]])
# transformed_2d_pos = np.dot(Rt_inv, point)
# pos2d = np.dot(K, transformed_2d_pos[:3])
# norm_pos2d = np.array([
# pos2d[0] / pos2d[2],
# pos2d[1] / pos2d[2],
# pos2d[2]])
# f_2D_rotated.append([image_size[0] - norm_pos2d[0], image_size[1] - norm_pos2d[1]])
# pedestrians[agent.id]['bounding_box_2D'] = f_2D_rotated
cars_dict[frame] = vehicles
pedestrians_dict[frame] = pedestrians
#print("End of Episode")
#print(len(pedestrians_dict[frame]))
# Save PLY to disk
# This generates the PLY string with the 3D points and the RGB colors
# for each row of the file.
if not frame % pointcloud_step:
point_cloud.save_to_disk(os.path.join(
output_folder, '{:0>5}.ply'.format(frame))
)
point_cloud_seg.save_to_disk(os.path.join(
output_folder, '{:0>5}_seg.ply'.format(frame))
)
if not args.autopilot:
client.send_control(
hand_brake=True)
else:
# Together with the measurements, the server has sent the
# control that the in-game autopilot would do this frame. We
# can enable autopilot by sending back this control to the
# server. We can modify it if wanted, here for instance we
# will add some noise to the steer.
control = measurements.player_measurements.autopilot_control
control.steer += random.uniform(-0.1, 0.1)
client.send_control(control)
print ("Start pickle save")
with open(output_folder + '/cameras.p', 'w') as camerafile:
pickle.dump(cameras_dict, camerafile)
with open(output_folder + '/people.p', 'w') as peoplefile:
pickle.dump(pedestrians_dict, peoplefile)
with open(output_folder + '/cars.p', 'w') as carfile:
pickle.dump(cars_dict, carfile)
print ("Episode done")
def run_carla_client(host, port, far):
# Here we will run a single episode with 300 frames.
number_of_frames = 30
frame_step = 10 # Save one image every 100 frames
image_size = [800, 600]
camera_local_pos = [0.3, 0.0, 1.3] # [X, Y, Z]
camera_local_rotation = [0, 0, 0] # [pitch(Y), yaw(Z), roll(X)]
fov = 70
autopilot = False
control = VehicleControl()
for start_i in range(150):
output_folder = '/home2/mariap/Packages/CARLA_0.8.2/PythonClient/_out/pos_' + str(
start_i)
if not os.path.exists(output_folder):
os.mkdir(output_folder)
print("make " + str(output_folder))
# Connect with the server
with make_carla_client(host, port) as client:
print('CarlaClient connected')
for start_i in range(150):
output_folder = '/home2/mariap/Packages/CARLA_0.8.2/PythonClient/_out/pos_' + str(
start_i)
print(output_folder)
# Here we load the settings.
settings = CarlaSettings()
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=100,
NumberOfPedestrians=500,
WeatherId=random.choice([1, 3, 7, 8, 14]))
settings.randomize_seeds()
camera1 = Camera('CameraDepth', PostProcessing='Depth', FOV=fov)
camera1.set_image_size(*image_size)
camera1.set_position(*camera_local_pos)
camera1.set_rotation(*camera_local_rotation)
settings.add_sensor(camera1)
camera2 = Camera('CameraRGB', PostProcessing='SceneFinal', FOV=fov)
camera2.set_image_size(*image_size)
camera2.set_position(*camera_local_pos)
camera2.set_rotation(*camera_local_rotation)
settings.add_sensor(camera2)
camera3 = Camera('CameraSeg',
PostProcessing='SemanticSegmentation')
camera3.set_image_size(*image_size)
camera3.set_position(*camera_local_pos)
camera3.set_rotation(*camera_local_rotation)
settings.add_sensor(camera3)
client.load_settings(settings)
# Start at location index id '0'
client.start_episode(start_i)
cameras_dict = {}
pedestrians_dict = {}
cars_dict = {}
# Compute the camera transform matrix
camera_to_car_transform = camera2.get_unreal_transform()
# (Intrinsic) (3, 3) K Matrix
K = np.identity(3)
K[0, 2] = image_size[0] / 2.0
K[1, 2] = image_size[1] / 2.0
K[0,
0] = K[1,
1] = image_size[0] / (2.0 * np.tan(fov * np.pi / 360.0))
with open(output_folder + '/camera_intrinsics.p', 'w') as camfile:
pickle.dump(K, camfile)
# Iterate every frame in the episode except for the first one.
for frame in range(1, number_of_frames):
# Read the data produced by the server this frame.
measurements, sensor_data = client.read_data()
# Save one image every 'frame_step' frames
if not frame % frame_step:
for name, measurement in sensor_data.items():
filename = '{:s}/{:0>6d}'.format(name, frame)
measurement.save_to_disk(
os.path.join(output_folder, filename))
# Start transformations time mesure.
timer = StopWatch()
# RGB image [[[r,g,b],..[r,g,b]],..[[r,g,b],..[r,g,b]]]
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
image_seg = np.tile(
labels_to_array(sensor_data['CameraSeg']), (1, 1, 3))
# 2d to (camera) local 3d
# We use the image_RGB to colorize each 3D point, this is optional.
# "max_depth" is used to keep only the points that are near to the
# camera, meaning 1.0 the farest points (sky)
point_cloud = depth_to_local_point_cloud(
sensor_data['CameraDepth'], image_RGB, max_depth=far)
point_cloud_seg = depth_to_local_point_cloud(
sensor_data['CameraDepth'], image_seg, max_depth=far)
# (Camera) local 3d to world 3d.
# Get the transform from the player protobuf transformation.
world_transform = Transform(
measurements.player_measurements.transform)
# Compute the final transformation matrix.
car_to_world_transform = world_transform * camera_to_car_transform
# Car to World transformation given the 3D points and the
# transformation matrix.
point_cloud.apply_transform(car_to_world_transform)
point_cloud_seg.apply_transform(car_to_world_transform)
Rt = car_to_world_transform.matrix
Rt_inv = car_to_world_transform.inverse(
).matrix # Rt_inv is the world to camera matrix !!
#R_inv=world_transform.inverse().matrix
cameras_dict[frame] = {}
cameras_dict[frame]['inverse_rotation'] = Rt_inv[:]
cameras_dict[frame]['rotation'] = Rt[:]
cameras_dict[frame]['translation'] = Rt_inv[0:3, 3]
cameras_dict[frame][
'camera_to_car'] = camera_to_car_transform.matrix
# Get non-player info
vehicles = {}
pedestrians = {}
for agent in measurements.non_player_agents:
# check if the agent is a vehicle.
if agent.HasField('vehicle'):
pos = agent.vehicle.transform.location
pos_vector = np.array([[pos.x], [pos.y], [pos.z],
[1.0]])
trnasformed_3d_pos = np.dot(Rt_inv, pos_vector)
pos2d = np.dot(K, trnasformed_3d_pos[:3])
# Normalize the point
norm_pos2d = np.array([
pos2d[0] / pos2d[2], pos2d[1] / pos2d[2],
pos2d[2]
])
# Now, pos2d contains the [x, y, d] values of the image in pixels (where d is the depth)
# You can use the depth to know the points that are in front of the camera (positive depth).
x_2d = image_size[0] - norm_pos2d[0]
y_2d = image_size[1] - norm_pos2d[1]
vehicles[agent.id] = {}
vehicles[agent.id]['transform'] = [
agent.vehicle.transform.location.x,
agent.vehicle.transform.location.y,
agent.vehicle.transform.location.z
]
vehicles[agent.id][
'bounding_box.transform'] = agent.vehicle.bounding_box.transform.location.z
vehicles[agent.id][
'yaw'] = agent.vehicle.transform.rotation.yaw
vehicles[agent.id]['bounding_box'] = [
agent.vehicle.bounding_box.extent.x,
agent.vehicle.bounding_box.extent.y,
agent.vehicle.bounding_box.extent.z
]
vehicle_transform = Transform(
agent.vehicle.bounding_box.transform)
pos = agent.vehicle.transform.location
bbox3d = agent.vehicle.bounding_box.extent
# Compute the 3D bounding boxes
# f contains the 4 points that corresponds to the bottom
f = np.array([[
pos.x + bbox3d.x, pos.y - bbox3d.y,
pos.z - bbox3d.z +
agent.vehicle.bounding_box.transform.location.z
],
[
pos.x + bbox3d.x,
pos.y + bbox3d.y,
pos.z - bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x - bbox3d.x,
pos.y + bbox3d.y,
pos.z - bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x - bbox3d.x,
pos.y - bbox3d.y,
pos.z - bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x + bbox3d.x,
pos.y - bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x + bbox3d.x,
pos.y + bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x - bbox3d.x,
pos.y + bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x - bbox3d.x,
pos.y - bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
]])
f_rotated = vehicle_transform.transform_points(f)
f_2D_rotated = []
vehicles[
agent.id]['bounding_box_coord'] = f_rotated
for i in range(f.shape[0]):
point = np.array([[f_rotated[i, 0]],
[f_rotated[i, 1]],
[f_rotated[i, 2]], [1]])
transformed_2d_pos = np.dot(
Rt_inv,
point) # 3d Position in camera space
pos2d = np.dot(
K, transformed_2d_pos[:3]
) # Conversion to camera frustum space
norm_pos2d = np.array([
pos2d[0] / pos2d[2], pos2d[1] / pos2d[2],
pos2d[2]
])
#print([image_size[0] - (pos2d[0] / pos2d[2]), image_size[1] - (pos2d[1] / pos2d[2])])
f_2D_rotated.append(
np.array([
image_size[0] - norm_pos2d[0],
image_size[1] - norm_pos2d[1]
]))
vehicles[
agent.id]['bounding_box_2D'] = f_2D_rotated
elif agent.HasField('pedestrian'):
pedestrians[agent.id] = {}
pedestrians[agent.id]['transform'] = [
agent.pedestrian.transform.location.x,
agent.pedestrian.transform.location.y,
agent.pedestrian.transform.location.z
]
pedestrians[agent.id][
'yaw'] = agent.pedestrian.transform.rotation.yaw
pedestrians[agent.id]['bounding_box'] = [
agent.pedestrian.bounding_box.extent.x,
agent.pedestrian.bounding_box.extent.y,
agent.pedestrian.bounding_box.extent.z
]
vehicle_transform = Transform(
agent.pedestrian.bounding_box.transform)
pos = agent.pedestrian.transform.location
bbox3d = agent.pedestrian.bounding_box.extent
# Compute the 3D bounding boxes
# f contains the 4 points that corresponds to the bottom
f = np.array(
[[pos.x + bbox3d.x, pos.y - bbox3d.y, pos.z],
[pos.x + bbox3d.x, pos.y + bbox3d.y, pos.z],
[pos.x - bbox3d.x, pos.y + bbox3d.y, pos.z],
[pos.x - bbox3d.x, pos.y - bbox3d.y, pos.z],
[
pos.x + bbox3d.x, pos.y - bbox3d.y,
pos.z + bbox3d.z
],
[
pos.x + bbox3d.x, pos.y + bbox3d.y,
pos.z + bbox3d.z
],
[
pos.x - bbox3d.x, pos.y + bbox3d.y,
pos.z + bbox3d.z
],
[
pos.x - bbox3d.x, pos.y - bbox3d.y,
pos.z + bbox3d.z
]])
f_rotated = vehicle_transform.transform_points(f)
pedestrians[
agent.id]['bounding_box_coord'] = f_rotated
f_2D_rotated = []
for i in range(f.shape[0]):
point = np.array([[f_rotated[i, 0]],
[f_rotated[i, 1]],
[f_rotated[i, 2]], [1]])
transformed_2d_pos = np.dot(
Rt_inv, point) # See above for cars
pos2d = np.dot(K, transformed_2d_pos[:3])
norm_pos2d = np.array([
pos2d[0] / pos2d[2], pos2d[1] / pos2d[2],
pos2d[2]
])
f_2D_rotated.append([
image_size[0] - norm_pos2d[0],
image_size[1] - norm_pos2d[1]
])
pedestrians[
agent.id]['bounding_box_2D'] = f_2D_rotated
cars_dict[frame] = vehicles
pedestrians_dict[frame] = pedestrians
# End transformations time mesure.
timer.stop()
# Save PLY to disk
# This generates the PLY string with the 3D points and the RGB colors
# for each row of the file.
point_cloud.save_to_disk(
os.path.join(output_folder,
'{:0>5}.ply'.format(frame)))
point_cloud_seg.save_to_disk(
os.path.join(output_folder,
'{:0>5}_seg.ply'.format(frame)))
print_message(timer.milliseconds(), len(point_cloud),
frame)
if autopilot:
client.send_control(
measurements.player_measurements.autopilot_control)
else:
control.hand_brake = True
client.send_control(control)
with open(output_folder + '/cameras.p', 'w') as camerafile:
pickle.dump(cameras_dict, camerafile)
print(output_folder + "cameras.p")
with open(output_folder + '/people.p', 'w') as peoplefile:
pickle.dump(pedestrians_dict, peoplefile)
with open(output_folder + '/cars.p', 'w') as carfile:
pickle.dump(cars_dict, carfile)
def run_carla_client(host, port, far):
# Here we will run a single episode with 300 frames.
number_of_frames = 3000
frame_step = 10 # Save one image every 100 frames
output_folder = '_out'
image_size = [1280, 720]
camera_local_pos = [0.7, 0.0, 1.3] # [X, Y, Z]
camera_local_rotation = [0, 0, 0] # [pitch(Y), yaw(Z), roll(X)]
fov = 59
WINDOW_WIDTH = 1280
WINDOW_HEIGHT = 720
MINI_WINDOW_WIDTH = 320
MINI_WINDOW_HEIGHT = 180
WINDOW_WIDTH_HALF = WINDOW_WIDTH / 2
WINDOW_HEIGHT_HALF = WINDOW_HEIGHT / 2
k = np.identity(3)
k[0, 2] = WINDOW_WIDTH_HALF
k[1, 2] = WINDOW_HEIGHT_HALF
k[0, 0] = k[1, 1] = WINDOW_WIDTH / \
(2.0 * math.tan(59.0 * math.pi / 360.0))
intrinsic = k
# Connect with the server
with make_carla_client(host, port) as client:
print('CarlaClient connected')
# Here we load the settings.
settings = CarlaSettings()
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=20,
NumberOfPedestrians=40,
WeatherId=1)
settings.randomize_seeds()
camera1 = Camera('CameraDepth', PostProcessing='Depth', FOV=fov)
camera1.set_image_size(*image_size)
camera1.set_position(*camera_local_pos)
camera1.set_rotation(*camera_local_rotation)
settings.add_sensor(camera1)
camera2 = Camera('CameraRGB', PostProcessing='SceneFinal', FOV=fov)
camera2.set_image_size(*image_size)
camera2.set_position(*camera_local_pos)
camera2.set_rotation(*camera_local_rotation)
settings.add_sensor(camera2)
#scene = client.load_settings(settings)
client.load_settings(settings)
#print("sjdsjhdjshdjshdjshgds",scene.player_start_spots[0].location.x)
# Start at location index id '0'
client.start_episode(0)
# Compute the camera transform matrix
camera_to_car_transform = camera2.get_unreal_transform()
print("camera_to_car_transform", camera_to_car_transform)
carla_utils_obj = segmentation.carla_utils(intrinsic)
# Iterate every frame in the episode except for the first one.
for frame in range(1, number_of_frames):
# Read the data produced by the server this frame.
measurements, sensor_data = client.read_data()
# Save one image every 'frame_step' frames
if not frame % frame_step:
# Start transformations time mesure.
# RGB image [[[r,g,b],..[r,g,b]],..[[r,g,b],..[r,g,b]]]
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
image_depth = to_rgb_array(sensor_data['CameraDepth'])
#measurements.player_measurements.transform.location.x-=40
#measurements.player_measurements.transform.location.z-=2
world_transform = Transform(
measurements.player_measurements.transform)
im_bgr = cv2.cvtColor(image_RGB, cv2.COLOR_RGB2BGR)
pos_vector = ([
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
])
fdfd = str(world_transform)
sdsd = fdfd[1:-1].split('\n')
new = []
for i in sdsd:
dd = i[:-1].lstrip('[ ')
ff = re.sub("\s+", ",", dd.strip())
gg = ff.split(',')
new.append([float(i) for i in gg])
newworld = np.array(new)
fdfd = str(camera_to_car_transform)
sdsd = fdfd[1:-1].split('\n')
new = []
for i in sdsd:
dd = i[:-1].lstrip('[ ')
ff = re.sub("\s+", ",", dd.strip())
gg = ff.split(',')
new.append([float(i) for i in gg])
newcam = np.array(new)
extrinsic = np.matmul(newworld, newcam)
get_2d_point = carla_utils_obj.run_seg(im_bgr, extrinsic,
pos_vector)
print(get_2d_point)
depth = image_depth[int(get_2d_point[0]), int(get_2d_point[1])]
scale = depth[0] + depth[1] * 256 + depth[2] * 256 * 256
scale = scale / ((256 * 256 * 256) - 1)
depth = scale * 1000
pos2d = np.array([(WINDOW_WIDTH - get_2d_point[1]) * depth,
(WINDOW_HEIGHT - get_2d_point[0]) * depth,
depth])
first = np.dot(np.linalg.inv(intrinsic), pos2d)
first = np.append(first, 1)
sec = np.matmul((extrinsic), first)
print("present", pos_vector)
print('goal-', sec)
client.send_control(
measurements.player_measurements.autopilot_control)
def run_carla_client(args):
# Here we will run 3 episodes with 300 frames each.
number_of_episodes = 15
frames_per_episode = 1230
# [0 , 1 , 2 , 3 , 4 , 5 , 6 , 7 , 8 , 9 , 10, 11, 12, 13, 14]
vehicles_num = [
140, 100, 120, 80, 90, 100, 80, 90, 110, 100, 80, 70, 70, 80, 100
]
# We assume the CARLA server is already waiting for a client to connect at
# host:port. To create a connection we can use the `make_carla_client`
# context manager, it creates a CARLA client object and starts the
# connection. It will throw an exception if something goes wrong. The
# context manager makes sure the connection is always cleaned up on exit.
with make_carla_client(args.host, args.port) as client:
print('CarlaClient connected')
for episode in range(0, number_of_episodes):
# Start a new episode.
if args.settings_filepath is None:
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
settings.set(
SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=vehicles_num[
episode], #random.choice([0, 20, 15, 20, 25, 21, 24, 18, 40, 35, 25, 30]), #25,
NumberOfPedestrians=120,
DisableTwoWheeledVehicles=False,
WeatherId=episode, #1, #random.choice([1, 3, 7, 8, 14]),
QualityLevel=args.quality_level)
settings.randomize_seeds()
# Now we want to add a couple of cameras to the player vehicle.
# We will collect the images produced by these cameras every
# frame.
# LEFT RGB CAMERA
camera_l = Camera('LeftCameraRGB', PostProcessing='SceneFinal')
camera_l.set_image_size(800, 600)
camera_l.set_position(1.30, -0.27, 1.50)
settings.add_sensor(camera_l)
# LEFT DEPTH
camera_ld = Camera('LeftCameraDepth', PostProcessing='Depth')
camera_ld.set_image_size(800, 600)
camera_ld.set_position(1.30, -0.27, 1.50)
settings.add_sensor(camera_ld)
# LEFT SEGMENTATION
camera_ls = Camera('LeftCameraSeg',
PostProcessing='SemanticSegmentation')
camera_ls.set_image_size(800, 600)
camera_ls.set_position(1.30, -0.27, 1.50)
settings.add_sensor(camera_ls)
# RIGHT RGB CAMERA
camera_r = Camera('RightCameraRGB',
PostProcessing='SceneFinal')
camera_r.set_image_size(800, 600)
camera_r.set_position(1.30, 0.27, 1.50)
settings.add_sensor(camera_r)
# RIGHT DEPTH
camera_rd = Camera('RightCameraDepth', PostProcessing='Depth')
camera_rd.set_image_size(800, 600)
camera_rd.set_position(1.30, 0.27, 1.50)
settings.add_sensor(camera_rd)
# RIGHT SEGMENTATION
camera_rs = Camera('RightCameraSeg',
PostProcessing='SemanticSegmentation')
camera_rs.set_image_size(800, 600)
camera_rs.set_position(1.30, 0.27, 1.50)
settings.add_sensor(camera_rs)
# LEFT 15 DEGREE RGB CAMERA
camera_l_15 = Camera('15_LeftCameraRGB',
PostProcessing='SceneFinal')
camera_l_15.set_image_size(800, 600)
camera_l_15.set_position(1.30, -0.7, 1.50) # [X, Y, Z]
camera_l_15.set_rotation(0, -15.0,
0) # [pitch(Y), yaw(Z), roll(X)]
settings.add_sensor(camera_l_15)
# LEFT 15 DEGREE DEPTH
camera_ld_15 = Camera('15_LeftCameraDepth',
PostProcessing='Depth')
camera_ld_15.set_image_size(800, 600)
camera_ld_15.set_position(1.30, -0.7, 1.50)
camera_ld_15.set_rotation(0, -15.0, 0)
settings.add_sensor(camera_ld_15)
# LEFT 15 DEGREE SEGMENTATION
camera_ls_15 = Camera('15_LeftCameraSeg',
PostProcessing='SemanticSegmentation')
camera_ls_15.set_image_size(800, 600)
camera_ls_15.set_position(1.30, -0.7, 1.50)
camera_ls_15.set_rotation(0, -15.0, 0)
settings.add_sensor(camera_ls_15)
# Right 15 DEGREE RGB CAMERA
camera_r_15 = Camera('15_RightCameraRGB',
PostProcessing='SceneFinal')
camera_r_15.set_image_size(800, 600)
camera_r_15.set_position(1.30, 0.7, 1.50)
camera_r_15.set_rotation(0, 15.0, 0)
settings.add_sensor(camera_r_15)
# Right 15 DEGREE DEPTH
camera_rd_15 = Camera('15_RightCameraDepth',
PostProcessing='Depth')
camera_rd_15.set_image_size(800, 600)
camera_rd_15.set_position(1.30, 0.7, 1.50)
camera_rd_15.set_rotation(0, 15.0, 0)
settings.add_sensor(camera_rd_15)
# RIGHT 15 DEGREE SEGMENTATION
camera_ls_15 = Camera('15_RightCameraSeg',
PostProcessing='SemanticSegmentation')
camera_ls_15.set_image_size(800, 600)
camera_ls_15.set_position(1.30, 0.7, 1.50)
camera_ls_15.set_rotation(0, 15.0, 0)
settings.add_sensor(camera_ls_15)
# LEFT 30 DEGREE RGB CAMERA
camera_l_30 = Camera('30_LeftCameraRGB',
PostProcessing='SceneFinal')
camera_l_30.set_image_size(800, 600)
camera_l_30.set_position(1.30, -0.7, 1.50)
camera_l_30.set_rotation(0, -30.0, 0)
settings.add_sensor(camera_l_30)
# LEFT 30 DEGREE DEPTH
camera_ld_30 = Camera('30_LeftCameraDepth',
PostProcessing='Depth')
camera_ld_30.set_image_size(800, 600)
camera_ld_30.set_position(1.30, -0.7, 1.50)
camera_ld_30.set_rotation(0, -30.0, 0)
settings.add_sensor(camera_ld_30)
# LEFT 30 DEGREE SEGMENTATION
camera_ls_30 = Camera('30_LeftCameraSeg',
PostProcessing='SemanticSegmentation')
camera_ls_30.set_image_size(800, 600)
camera_ls_30.set_position(1.30, -0.7, 1.50)
camera_ls_30.set_rotation(0, -30.0, 0)
settings.add_sensor(camera_ls_30)
# RIGHT 30 DEGREE RGB CAMERA
camera_r_30 = Camera('30_RightCameraRGB',
PostProcessing='SceneFinal')
camera_r_30.set_image_size(800, 600)
camera_r_30.set_position(1.30, 0.7, 1.50)
camera_r_30.set_rotation(0, 30.0, 0)
settings.add_sensor(camera_r_30)
# RIGHT 30 DEGREE DEPTH
camera_rd_30 = Camera('30_RightCameraDepth',
PostProcessing='Depth')
camera_rd_30.set_image_size(800, 600)
camera_rd_30.set_position(1.30, 0.7, 1.50)
camera_rd_30.set_rotation(0, 30.0, 0)
settings.add_sensor(camera_rd_30)
# RIGHT 30 DEGREE SEGMENTATION
camera_rs_30 = Camera('30_RightCameraSeg',
PostProcessing='SemanticSegmentation')
camera_rs_30.set_image_size(800, 600)
camera_rs_30.set_position(1.30, 0.7, 1.50)
camera_rs_30.set_rotation(0, 30.0, 0)
settings.add_sensor(camera_rs_30)
else:
# Alternatively, we can load these settings from a file.
with open(args.settings_filepath, 'r') as fp:
settings = fp.read()
# Now we load these settings into the server. The server replies
# with a scene description containing the available start spots for
# the player. Here we can provide a CarlaSettings object or a
# CarlaSettings.ini file as string.
scene = client.load_settings(settings)
# Choose one player start at random.
number_of_player_starts = len(scene.player_start_spots)
player_start = random.randint(0, max(0,
number_of_player_starts - 1))
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print('Starting new episode...')
client.start_episode(player_start)
camera_l_to_car_transform = camera_l.get_unreal_transform()
camera_r_to_car_transform = camera_r.get_unreal_transform()
camera_l_15_to_car_transform = camera_l_15.get_unreal_transform()
camera_r_15_to_car_transform = camera_r_15.get_unreal_transform()
camera_l_30_to_car_transform = camera_l_30.get_unreal_transform()
camera_r_30_to_car_transform = camera_r_30.get_unreal_transform()
if not os.path.isdir(args.dataset_path +
"/episode_{:0>4d}".format(episode)):
os.makedirs(args.dataset_path +
"/episode_{:0>4d}".format(episode))
# Iterate every frame in the episode.
for frame in range(0, frames_per_episode):
# Read the data produced by the server this frame.
measurements, sensor_data = client.read_data()
#image = sensor_data.get('LeftCameraSeg', None)
# array = image_converter.depth_to_logarithmic_grayscale(image)
#array = image_converter.labels_to_cityscapes_palette(image)
#filename = '{:0>6d}'.format(frame)
#filename += '.png'
#cv2.imwrite(filename, array)
#image.save_to_disk("/data/khoshhal/Dataset/episode_{:0>4d}/{:s}/{:0>6d}".format(episode, "LeftCameraSeg", frame))
# Print some of the measurements.
# print_measurements(measurements)
#player_measurements = measurements.player_measurements
world_transform = Transform(
measurements.player_measurements.transform)
# Compute the final transformation matrix.
camera_l_to_world_transform = world_transform * camera_l_to_car_transform
camera_r_to_world_transform = world_transform * camera_r_to_car_transform
camera_l_15_to_world_transform = world_transform * camera_l_15_to_car_transform
camera_r_15_to_world_transform = world_transform * camera_r_15_to_car_transform
camera_l_30_to_world_transform = world_transform * camera_l_30_to_car_transform
camera_r_30_to_world_transform = world_transform * camera_r_30_to_car_transform
args.out_filename_format = dataset_path + '/episode_{:0>4d}/{:s}/{:0>6d}'
# Save the images to disk if requested.
if frame >= 30:
if args.save_images_to_disk:
for name, measurement in sensor_data.items():
filename = args.out_filename_format.format(
episode, name, frame - 30)
measurement.save_to_disk(filename)
# Save Transform matrix of each camera to separated files
line = ""
filename = "{}/episode_{:0>4d}/LeftCamera".format(
args.dataset_path, episode) + ".txt"
with open(filename, 'a') as LeftCamera:
for x in np.asarray(camera_l_to_world_transform.
matrix[:3, :]).reshape(-1):
line += "{:.8e} ".format(x)
line = line[:-1]
line += "\n"
LeftCamera.write(line)
line = ""
filename = "{}/episode_{:0>4d}/RightCamera".format(
args.dataset_path, episode) + ".txt"
with open(filename, 'a') as RightCamera:
for x in np.asarray(camera_r_to_world_transform.
matrix[:3, :]).reshape(-1):
line += "{:.8e} ".format(x)
line = line[:-1]
line += "\n"
RightCamera.write(line)
line = ""
filename = "{}/episode_{:0>4d}/15_LeftCamera".format(
args.dataset_path, episode) + ".txt"
with open(filename, 'a') as myfile:
for x in np.asarray(camera_l_15_to_world_transform.
matrix[:3, :]).reshape(-1):
line += "{:.8e} ".format(x)
line = line[:-1]
line += "\n"
myfile.write(line)
line = ""
filename = "{}/episode_{:0>4d}/15_RightCamera".format(
args.dataset_path, episode) + ".txt"
with open(filename, 'a') as myfile:
for x in np.asarray(camera_r_15_to_world_transform.
matrix[:3, :]).reshape(-1):
line += "{:.8e} ".format(x)
line = line[:-1]
line += "\n"
myfile.write(line)
line = ""
filename = "{}/episode_{:0>4d}/30_LeftCamera".format(
args.dataset_path, episode) + ".txt"
with open(filename, 'a') as myfile:
for x in np.asarray(camera_l_30_to_world_transform.
matrix[:3, :]).reshape(-1):
line += "{:.8e} ".format(x)
line = line[:-1]
line += "\n"
myfile.write(line)
line = ""
filename = "{}/episode_{:0>4d}/30_RightCamera".format(
args.dataset_path, episode) + ".txt"
with open(filename, 'a') as myfile:
for x in np.asarray(camera_r_30_to_world_transform.
matrix[:3, :]).reshape(-1):
line += "{:.8e} ".format(x)
line = line[:-1]
line += "\n"
myfile.write(line)
line = ""
# We can access the encoded data of a given image as numpy
# array using its "data" property. For instance, to get the
# depth value (normalized) at pixel X, Y
#
# depth_array = sensor_data['CameraDepth'].data
# value_at_pixel = depth_array[Y, X]
#
# Now we have to send the instructions to control the vehicle.
# If we are in synchronous mode the server will pause the
# simulation until we send this control.
if not args.autopilot:
client.send_control(steer=random.uniform(-1.0, 1.0),
throttle=0.5,
brake=0.0,
hand_brake=False,
reverse=False)
else:
# Together with the measurements, the server has sent the
# control that the in-game autopilot would do this frame. We
# can enable autopilot by sending back this control to the
# server. We can modify it if wanted, here for instance we
# will add some noise to the steer.
control = measurements.player_measurements.autopilot_control
#control.steer += random.uniform(-0.1, 0.1)
client.send_control(control)
#time.sleep(1)
myfile.close()
LeftCamera.close()
RightCamera.close()
def run_carla_client(host, port, far):
# Here we will run a single episode with 300 frames.
number_of_frames = 3000
frame_step = 10 # Save one image every 100 frames
output_folder = '_out'
image_size = [1280, 720]
camera_local_pos = [0.3, 0.0, 1.3] # [X, Y, Z]
camera_local_rotation = [0, 0, 0] # [pitch(Y), yaw(Z), roll(X)]
fov = 59
# Connect with the server
with make_carla_client(host, port) as client:
print('CarlaClient connected')
# Here we load the settings.
settings = CarlaSettings()
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=20,
NumberOfPedestrians=40,
WeatherId=7)
settings.randomize_seeds()
camera1 = Camera('CameraDepth', PostProcessing='Depth', FOV=fov)
camera1.set_image_size(*image_size)
camera1.set_position(*camera_local_pos)
camera1.set_rotation(*camera_local_rotation)
settings.add_sensor(camera1)
camera2 = Camera('CameraRGB', PostProcessing='SceneFinal', FOV=fov)
camera2.set_image_size(*image_size)
camera2.set_position(*camera_local_pos)
camera2.set_rotation(*camera_local_rotation)
settings.add_sensor(camera2)
client.load_settings(settings)
# Start at location index id '0'
client.start_episode(0)
# Compute the camera transform matrix
camera_to_car_transform = camera2.get_unreal_transform()
print("camera_to_car_transform", camera_to_car_transform)
# Iterate every frame in the episode except for the first one.
for frame in range(1, number_of_frames):
# Read the data produced by the server this frame.
measurements, sensor_data = client.read_data()
# Save one image every 'frame_step' frames
if not frame % frame_step:
# Start transformations time mesure.
# RGB image [[[r,g,b],..[r,g,b]],..[[r,g,b],..[r,g,b]]]
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
world_transform = Transform(
measurements.player_measurements.transform)
# Compute the final transformation matrix.
car_to_world_transform = world_transform * camera_to_car_transform
print("{}.png".format(str(frame)))
print([
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
])
print("world_transform\n", world_transform)
#print("car_to_world_transform\n",car_to_world_transform)
print('\n')
im_bgr = cv2.cvtColor(image_RGB, cv2.COLOR_RGB2BGR)
cv2.imwrite("./{}/{}.png".format(output_folder, str(frame)),
im_bgr)
# 2d to (camera) local 3d
# We use the image_RGB to colorize each 3D point, this is optional.
# "max_depth" is used to keep only the points that are near to the
# camera, meaning 1.0 the farest points (sky)
# Save PLY to disk
# This generates the PLY string with the 3D points and the RGB colors
# for each row of the file.
client.send_control(
measurements.player_measurements.autopilot_control)
def run_carla_client(host, port, far):
# Here we will run a single episode with 300 frames.
number_of_frames = 3000
frame_step = 100 # Save one image every 100 frames
output_folder = '_out'
image_size = [800, 600]
camera_local_pos = [0.3, 0.0, 1.3] # [X, Y, Z]
camera_local_rotation = [0, 0, 0] # [pitch(Y), yaw(Z), roll(X)]
fov = 70
# Connect with the server
with make_carla_client(host, port) as client:
print('CarlaClient connected')
# Here we load the settings.
settings = CarlaSettings()
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=20,
NumberOfPedestrians=40,
WeatherId=random.choice([1, 3, 7, 8, 14]))
settings.randomize_seeds()
camera1 = Camera('CameraDepth', PostProcessing='Depth', FOV=fov)
camera1.set_image_size(*image_size)
camera1.set_position(*camera_local_pos)
camera1.set_rotation(*camera_local_rotation)
settings.add_sensor(camera1)
camera2 = Camera('CameraRGB', PostProcessing='SceneFinal', FOV=fov)
camera2.set_image_size(*image_size)
camera2.set_position(*camera_local_pos)
camera2.set_rotation(*camera_local_rotation)
settings.add_sensor(camera2)
client.load_settings(settings)
# Start at location index id '0'
client.start_episode(0)
# Compute the camera transform matrix
camera_to_car_transform = camera2.get_unreal_transform()
# Iterate every frame in the episode except for the first one.
for frame in range(1, number_of_frames):
# Read the data produced by the server this frame.
measurements, sensor_data = client.read_data()
# Save one image every 'frame_step' frames
if not frame % frame_step:
# Start transformations time mesure.
timer = StopWatch()
# RGB image [[[r,g,b],..[r,g,b]],..[[r,g,b],..[r,g,b]]]
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
# 2d to (camera) local 3d
# We use the image_RGB to colorize each 3D point, this is optional.
# "max_depth" is used to keep only the points that are near to the
# camera, meaning 1.0 the farest points (sky)
point_cloud = depth_to_local_point_cloud(
sensor_data['CameraDepth'], image_RGB, max_depth=far)
# (Camera) local 3d to world 3d.
# Get the transform from the player protobuf transformation.
world_transform = Transform(
measurements.player_measurements.transform)
# Compute the final transformation matrix.
car_to_world_transform = world_transform * camera_to_car_transform
# Car to World transformation given the 3D points and the
# transformation matrix.
point_cloud.apply_transform(car_to_world_transform)
# End transformations time mesure.
timer.stop()
# Save PLY to disk
# This generates the PLY string with the 3D points and the RGB colors
# for each row of the file.
point_cloud.save_to_disk(
os.path.join(output_folder, '{:0>5}.ply'.format(frame)))
print_message(timer.milliseconds(), len(point_cloud), frame)
client.send_control(
measurements.player_measurements.autopilot_control)
def run_carla_client(host, port, far):
# Here we will run a single episode with 300 frames.
number_of_frames = 3000
frame_step = 100 # Save one image every 100 frames
output_folder = '_out'
image_size = [800, 600]
camera_local_pos = [0.3, 0.0, 1.3] # [X, Y, Z]
camera_local_rotation = [0, 0, 0] # [pitch(Y), yaw(Z), roll(X)]
fov = 70
# Connect with the server
with make_carla_client(host, port) as client:
print('CarlaClient connected')
# Here we load the settings.
settings = CarlaSettings()
settings.set(
SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=20,
NumberOfPedestrians=40,
WeatherId=random.choice([1, 3, 7, 8, 14]))
settings.randomize_seeds()
camera1 = Camera('CameraDepth', PostProcessing='Depth', FOV=fov)
camera1.set_image_size(*image_size)
camera1.set_position(*camera_local_pos)
camera1.set_rotation(*camera_local_rotation)
settings.add_sensor(camera1)
camera2 = Camera('CameraRGB', PostProcessing='SceneFinal', FOV=fov)
camera2.set_image_size(*image_size)
camera2.set_position(*camera_local_pos)
camera2.set_rotation(*camera_local_rotation)
settings.add_sensor(camera2)
client.load_settings(settings)
# Start at location index id '0'
client.start_episode(0)
# Compute the camera transform matrix
camera_to_car_transform = camera2.get_unreal_transform()
# Iterate every frame in the episode except for the first one.
for frame in range(1, number_of_frames):
# Read the data produced by the server this frame.
measurements, sensor_data = client.read_data()
# Save one image every 'frame_step' frames
if not frame % frame_step:
# Start transformations time mesure.
timer = StopWatch()
# RGB image [[[r,g,b],..[r,g,b]],..[[r,g,b],..[r,g,b]]]
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
# 2d to (camera) local 3d
# We use the image_RGB to colorize each 3D point, this is optional.
# "max_depth" is used to keep only the points that are near to the
# camera, meaning 1.0 the farest points (sky)
point_cloud = depth_to_local_point_cloud(
sensor_data['CameraDepth'],
image_RGB,
max_depth=far
)
# (Camera) local 3d to world 3d.
# Get the transform from the player protobuf transformation.
world_transform = Transform(
measurements.player_measurements.transform
)
# Compute the final transformation matrix.
car_to_world_transform = world_transform * camera_to_car_transform
# Car to World transformation given the 3D points and the
# transformation matrix.
point_cloud.apply_transform(car_to_world_transform)
# End transformations time mesure.
timer.stop()
# Save PLY to disk
# This generates the PLY string with the 3D points and the RGB colors
# for each row of the file.
point_cloud.save_to_disk(os.path.join(
output_folder, '{:0>5}.ply'.format(frame))
)
print_message(timer.milliseconds(), len(point_cloud), frame)
client.send_control(
measurements.player_measurements.autopilot_control
)
def run_carla_client(args):
number_of_episodes = 15
frames_per_episode = 5030
# [0 , 1 , 2 , 3 , 4 , 5 , 6 , 7 , 8 , 9 , 10, 11, 12, 13, 14]
vehicles_num = [25, 20, 30, 20, 15, 45, 45, 50, 25, 25, 25, 20, 25, 25, 25]
# We assume the CARLA server is already waiting for a client to connect at
# host:port. To create a connection we can use the `make_carla_client`
# context manager, it creates a CARLA client object and starts the
# connection. It will throw an exception if something goes wrong. The
# context manager makes sure the connection is always cleaned up on exit.
with make_carla_client(args.host, args.port) as client:
print('CarlaClient connected')
for episode in [1, 4, 11]:
# Start a new episode.
if args.settings_filepath is None:
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
settings.set(
SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles= vehicles_num[episode],#random.choice([0, 20, 15, 20, 25, 21, 24, 18, 40, 35, 25, 30]), #25,
NumberOfPedestrians=50,
DisableTwoWheeledVehicles=False,
WeatherId= episode, #1, #random.choice([1, 3, 7, 8, 14]),
QualityLevel=args.quality_level)
settings.randomize_seeds()
# Now we want to add a couple of cameras to the player vehicle.
# We will collect the images produced by these cameras every
# frame.
#### FRONT STEREO ####
# LEFT RGB CAMERA
camera_l = Camera('LeftCameraRGB', PostProcessing='SceneFinal')
camera_l.set_image_size(800, 600)
camera_l.set_position(1.50, -0.27, 1.50)
settings.add_sensor(camera_l)
# LEFT DEPTH
camera_ld = Camera('LeftCameraDepth', PostProcessing='Depth')
camera_ld.set_image_size(800, 600)
camera_ld.set_position(1.50, -0.27, 1.50)
settings.add_sensor(camera_ld)
# LEFT SEGMENTATION
camera_ls = Camera('LeftCameraSeg', PostProcessing='SemanticSegmentation')
camera_ls.set_image_size(800, 600)
camera_ls.set_position(1.50, -0.27, 1.50)
settings.add_sensor(camera_ls)
# RIGHT RGB CAMERA
camera_r = Camera('RightCameraRGB', PostProcessing='SceneFinal')
camera_r.set_image_size(800, 600)
camera_r.set_position(1.50, 0.27, 1.50)
settings.add_sensor(camera_r)
# RIGHT DEPTH
camera_rd = Camera('RightCameraDepth', PostProcessing='Depth')
camera_rd.set_image_size(800, 600)
camera_rd.set_position(1.50, 0.27, 1.50)
settings.add_sensor(camera_rd)
# RIGHT SEGMENTATION
camera_rs = Camera('RightCameraSeg', PostProcessing='SemanticSegmentation')
camera_rs.set_image_size(800, 600)
camera_rs.set_position(1.50, 0.27, 1.50)
settings.add_sensor(camera_rs)
#### -45 DEGREE STEREO CAMERA ####
# LEFT STEREO -45 DEGREE RGB
camera_45_n_l = Camera('45_N_LeftCameraRGB', PostProcessing='SceneFinal')
camera_45_n_l.set_image_size(800, 600)
camera_45_n_l.set_position(0.8, -1.0, 1.50) # [X, Y, Z]
camera_45_n_l.set_rotation(0, -45.0, 0) # [pitch(Y), yaw(Z), roll(X)]
settings.add_sensor(camera_45_n_l)
# RIGHT STEREO -45 DEGREE RGB
camera_45_n_r = Camera('45_N_RightCameraRGB', PostProcessing='SceneFinal')
camera_45_n_r.set_image_size(800, 600)
camera_45_n_r.set_position(1.2, -0.6, 1.50)
camera_45_n_r.set_rotation(0, -45.0, 0)
settings.add_sensor(camera_45_n_r)
# LEFT STEREO -45 DEGREE DEPTH
camera_45_n_ld = Camera('45_N_LeftCameraDepth', PostProcessing='Depth')
camera_45_n_ld.set_image_size(800, 600)
camera_45_n_ld.set_position(0.8, -1.0, 1.50)
camera_45_n_ld.set_rotation(0, -45.0, 0)
settings.add_sensor(camera_45_n_ld)
# RIGHT STEREO -45 DEGREE DEPTH
camera_45_n_rd = Camera('45_N_RightCameraDepth', PostProcessing='Depth')
camera_45_n_rd.set_image_size(800, 600)
camera_45_n_rd.set_position(1.2, -0.6, 1.50)
camera_45_n_rd.set_rotation(0, -45.0, 0)
settings.add_sensor(camera_45_n_rd)
# LEFT STEREO -45 DEGREE SEGMENTATION
camera_45_n_ls = Camera('45_N_LeftCameraSeg', PostProcessing='SemanticSegmentation')
camera_45_n_ls.set_image_size(800, 600)
camera_45_n_ls.set_position(0.8, -1.0, 1.50)
camera_45_n_ls.set_rotation(0, -45.0, 0)
settings.add_sensor(camera_45_n_ls)
# RIGHT STEREO -45 DEGREE SEGMENTATION
camera_45_n_rs = Camera('45_N_RightCameraSeg', PostProcessing='SemanticSegmentation')
camera_45_n_rs.set_image_size(800, 600)
camera_45_n_rs.set_position(1.2, -0.6, 1.50)
camera_45_n_rs.set_rotation(0, -45.0, 0)
settings.add_sensor(camera_45_n_rs)
#### +45 DEGREE STEREO CAMERA ####
# LEFT STEREO +45 DEGREE RGB
camera_45_p_l = Camera('45_P_LeftCameraRGB', PostProcessing='SceneFinal')
camera_45_p_l.set_image_size(800, 600)
camera_45_p_l.set_position(1.2, 0.6, 1.50) # [X, Y, Z]
camera_45_p_l.set_rotation(0, 45.0, 0) # [pitch(Y), yaw(Z), roll(X)]
settings.add_sensor(camera_45_p_l)
# RIGHT STEREO +45 DEGREE RGB
camera_45_p_r = Camera('45_P_RightCameraRGB', PostProcessing='SceneFinal')
camera_45_p_r.set_image_size(800, 600)
camera_45_p_r.set_position(0.8, 1.0, 1.50)
camera_45_p_r.set_rotation(0, 45.0, 0)
settings.add_sensor(camera_45_p_r)
# LEFT STEREO +45 DEGREE DEPTH
camera_45_p_ld = Camera('45_P_LeftCameraDepth', PostProcessing='Depth')
camera_45_p_ld.set_image_size(800, 600)
camera_45_p_ld.set_position(1.2, 0.6, 1.50)
camera_45_p_ld.set_rotation(0, 45.0, 0)
settings.add_sensor(camera_45_p_ld)
# RIGHT STEREO +45 DEGREE DEPTH
camera_45_p_rd = Camera('45_P_RightCameraDepth', PostProcessing='Depth')
camera_45_p_rd.set_image_size(800, 600)
camera_45_p_rd.set_position(0.8, 1.0, 1.50)
camera_45_p_rd.set_rotation(0, 45.0, 0)
settings.add_sensor(camera_45_p_rd)
# LEFT STEREO +45 DEGREE SEGMENTATION
camera_45_p_ls = Camera('45_P_LeftCameraSeg', PostProcessing='SemanticSegmentation')
camera_45_p_ls.set_image_size(800, 600)
camera_45_p_ls.set_position(1.2, 0.6, 1.50)
camera_45_p_ls.set_rotation(0, 45.0, 0)
settings.add_sensor(camera_45_p_ls)
# RIGHT STEREO +45 DEGREE SEGMENTATION
camera_45_p_rs = Camera('45_P_RightCameraSeg', PostProcessing='SemanticSegmentation')
camera_45_p_rs.set_image_size(800, 600)
camera_45_p_rs.set_position(0.8, 1.0, 1.50)
camera_45_p_rs.set_rotation(0, 45.0, 0)
settings.add_sensor(camera_45_p_rs)
'''
#### -90 DEGREE STEREO CAMERA ####
# LEFT STEREO -90 DEGREE RGB
camera_90_n_l = Camera('90_N_LeftCameraRGB', PostProcessing='SceneFinal')
camera_90_n_l.set_image_size(800, 600)
camera_90_n_l.set_position(-0.27, -1.0, 1.50) # [X, Y, Z]
camera_90_n_l.set_rotation(0, -90.0, 0) # [pitch(Y), yaw(Z), roll(X)]
settings.add_sensor(camera_90_n_l)
# RIGHT STEREO -90 DEGREE RGB
camera_90_n_r = Camera('90_N_RightCameraRGB', PostProcessing='SceneFinal')
camera_90_n_r.set_image_size(800, 600)
camera_90_n_r.set_position(0.27, -1.0, 1.50)
camera_90_n_r.set_rotation(0, -90.0, 0)
settings.add_sensor(camera_90_n_r)
# LEFT STEREO -90 DEGREE DEPTH
camera_90_n_ld = Camera('90_N_LeftCameraDepth', PostProcessing='Depth')
camera_90_n_ld.set_image_size(800, 600)
camera_90_n_ld.set_position(-0.27, -1.0, 1.50)
camera_90_n_ld.set_rotation(0, -90.0, 0)
settings.add_sensor(camera_90_n_ld)
# RIGHT STEREO -90 DEGREE DEPTH
camera_90_n_rd = Camera('90_N_RightCameraDepth', PostProcessing='Depth')
camera_90_n_rd.set_image_size(800, 600)
camera_90_n_rd.set_position(0.27, -1.0, 1.50)
camera_90_n_rd.set_rotation(0, -90.0, 0)
settings.add_sensor(camera_90_n_rd)
# LEFT STEREO -90 DEGREE SEGMENTATION
camera_90_n_ls = Camera('90_N_LeftCameraSeg', PostProcessing='SemanticSegmentation')
camera_90_n_ls.set_image_size(800, 600)
camera_90_n_ls.set_position(-0.27, -1.0, 1.50)
camera_90_n_ls.set_rotation(0, -90.0, 0)
settings.add_sensor(camera_90_n_ls)
# RIGHT STEREO -90 DEGREE SEGMENTATION
camera_90_n_rs = Camera('90_N_RightCameraSeg', PostProcessing='SemanticSegmentation')
camera_90_n_rs.set_image_size(800, 600)
camera_90_n_rs.set_position(0.27, -1.0, 1.50)
camera_90_n_rs.set_rotation(0, -90.0, 0)
settings.add_sensor(camera_90_n_rs)
#### +90 DEGREE STEREO CAMERA ####
# LEFT STEREO +90 DEGREE RGB
camera_90_p_l = Camera('90_P_LeftCameraRGB', PostProcessing='SceneFinal')
camera_90_p_l.set_image_size(800, 600)
camera_90_p_l.set_position(0.27, 1.0, 1.50) # [X, Y, Z]
camera_90_p_l.set_rotation(0, 90.0, 0) # [pitch(Y), yaw(Z), roll(X)]
settings.add_sensor(camera_90_p_l)
# RIGHT STEREO +90 DEGREE RGB
camera_90_p_r = Camera('90_P_RightCameraRGB', PostProcessing='SceneFinal')
camera_90_p_r.set_image_size(800, 600)
camera_90_p_r.set_position(-0.27, 1.0, 1.50)
camera_90_p_r.set_rotation(0, 90.0, 0)
settings.add_sensor(camera_90_p_r)
# LEFT STEREO +90 DEGREE DEPTH
camera_90_p_ld = Camera('90_P_LeftCameraDepth', PostProcessing='Depth')
camera_90_p_ld.set_image_size(800, 600)
camera_90_p_ld.set_position(0.27, 1.0, 1.50)
camera_90_p_ld.set_rotation(0, 90.0, 0)
settings.add_sensor(camera_90_p_ld)
# RIGHT STEREO +90 DEGREE DEPTH
camera_90_p_rd = Camera('90_P_RightCameraDepth', PostProcessing='Depth')
camera_90_p_rd.set_image_size(800, 600)
camera_90_p_rd.set_position(-0.27, 1.0, 1.50)
camera_90_p_rd.set_rotation(0, 90.0, 0)
settings.add_sensor(camera_90_p_rd)
# LEFT STEREO +90 DEGREE SEGMENTATION
camera_90_p_ls = Camera('90_P_LeftCameraSeg', PostProcessing='SemanticSegmentation')
camera_90_p_ls.set_image_size(800, 600)
camera_90_p_ls.set_position(0.27, 1.0, 1.50)
camera_90_p_ls.set_rotation(0, 90.0, 0)
settings.add_sensor(camera_90_p_ls)
# RIGHT STEREO +90 DEGREE SEGMENTATION
camera_90_p_rs = Camera('90_P_RightCameraSeg', PostProcessing='SemanticSegmentation')
camera_90_p_rs.set_image_size(800, 600)
camera_90_p_rs.set_position(-0.27, 1.0, 1.50)
camera_90_p_rs.set_rotation(0, 90.0, 0)
settings.add_sensor(camera_90_p_rs)
'''
'''
if args.lidar:
lidar = Lidar('Lidar32')
lidar.set_position(0, 0, 2.50)
lidar.set_rotation(0, 0, 0)
lidar.set(
Channels=32,
Range=50,
PointsPerSecond=100000,
RotationFrequency=10,
UpperFovLimit=10,
LowerFovLimit=-30)
settings.add_sensor(lidar)
'''
else:
# Alternatively, we can load these settings from a file.
with open(args.settings_filepath, 'r') as fp:
settings = fp.read()
# Now we load these settings into the server. The server replies
# with a scene description containing the available start spots for
# the player. Here we can provide a CarlaSettings object or a
# CarlaSettings.ini file as string.
scene = client.load_settings(settings)
# Choose one player start at random.
number_of_player_starts = len(scene.player_start_spots)
player_start = random.randint(0, max(0, number_of_player_starts - 1))
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print('Starting new episode...')
client.start_episode(player_start)
camera_l_to_car_transform = camera_l.get_unreal_transform()
camera_r_to_car_transform = camera_r.get_unreal_transform()
camera_45_n_l_to_car_transform = camera_45_n_l.get_unreal_transform()
camera_45_n_r_to_car_transform = camera_45_n_r.get_unreal_transform()
camera_45_p_l_to_car_transform = camera_45_p_l.get_unreal_transform()
camera_45_p_r_to_car_transform = camera_45_p_r.get_unreal_transform()
'''
camera_90_n_l_to_car_transform = camera_90_n_l.get_unreal_transform()
camera_90_n_r_to_car_transform = camera_90_n_r.get_unreal_transform()
camera_90_p_l_to_car_transform = camera_90_p_l.get_unreal_transform()
camera_90_p_r_to_car_transform = camera_90_p_r.get_unreal_transform()
'''
# Create a folder for saving episode data
if not os.path.isdir("/data/khoshhal/Dataset/episode_{:0>4d}".format(episode)):
os.makedirs("/data/khoshhal/Dataset/episode_{:0>4d}".format(episode))
# Iterate every frame in the episode.
for frame in range(0, frames_per_episode):
# Read the data produced by the server this frame.
measurements, sensor_data = client.read_data()
# player_measurements = measurements.player_measurements
world_transform = Transform(measurements.player_measurements.transform)
# Compute the final transformation matrix.
camera_l_to_world_transform = world_transform * camera_l_to_car_transform
camera_r_to_world_transform = world_transform * camera_r_to_car_transform
camera_45_n_l_to_world_transform = world_transform * camera_45_n_l_to_car_transform
camera_45_n_r_to_world_transform = world_transform * camera_45_n_r_to_car_transform
camera_45_p_l_to_world_transform = world_transform * camera_45_p_l_to_car_transform
camera_45_p_r_to_world_transform = world_transform * camera_45_p_r_to_car_transform
'''
camera_90_n_l_to_world_transform = world_transform * camera_90_n_l_to_car_transform
camera_90_n_r_to_world_transform = world_transform * camera_90_n_r_to_car_transform
camera_90_p_l_to_world_transform = world_transform * camera_90_p_l_to_car_transform
camera_90_p_r_to_world_transform = world_transform * camera_90_p_r_to_car_transform
'''
# Save the images to disk if requested.
if frame >= 30:# and (frame % 2 == 0):
if args.save_images_to_disk:
for name, measurement in sensor_data.items():
filename = args.out_filename_format.format(episode, name, (frame-30))
measurement.save_to_disk(filename)
# Save Transform matrix of each camera to separated files
line = ""
filename = "{}episode_{:0>4d}/LeftCamera".format(args.root_path, episode) + ".txt"
with open(filename, 'a') as myfile:
for x in np.asarray(camera_l_to_world_transform.matrix[:3, :]).reshape(-1):
line += "{:.8e} ".format(x)
line = line[:-1]
line += "\n"
myfile.write(line)
line = ""
filename = "{}episode_{:0>4d}/RightCamera".format(args.root_path, episode) + ".txt"
with open(filename, 'a') as myfile:
for x in np.asarray(camera_r_to_world_transform.matrix[:3, :]).reshape(-1):
line += "{:.8e} ".format(x)
line = line[:-1]
line += "\n"
myfile.write(line)
line = ""
filename = "{}episode_{:0>4d}/45_N_LeftCamera".format(args.root_path, episode) + ".txt"
with open(filename, 'a') as myfile:
for x in np.asarray(camera_45_n_l_to_world_transform.matrix[:3, :]).reshape(-1):
line += "{:.8e} ".format(x)
line = line[:-1]
line += "\n"
myfile.write(line)
line = ""
filename = "{}episode_{:0>4d}/45_N_RightCamera".format(args.root_path, episode) + ".txt"
with open(filename, 'a') as myfile:
for x in np.asarray(camera_45_n_r_to_world_transform.matrix[:3, :]).reshape(-1):
line += "{:.8e} ".format(x)
line = line[:-1]
line += "\n"
myfile.write(line)
line = ""
filename = "{}episode_{:0>4d}/45_P_LeftCamera".format(args.root_path, episode) + ".txt"
with open(filename, 'a') as myfile:
for x in np.asarray(camera_45_p_l_to_world_transform.matrix[:3, :]).reshape(-1):
line += "{:.8e} ".format(x)
line = line[:-1]
line += "\n"
myfile.write(line)
line = ""
filename = "{}episode_{:0>4d}/45_P_RightCamera".format(args.root_path, episode) + ".txt"
with open(filename, 'a') as myfile:
for x in np.asarray(camera_45_p_r_to_world_transform.matrix[:3, :]).reshape(-1):
line += "{:.8e} ".format(x)
line = line[:-1]
line += "\n"
myfile.write(line)
line = ""
'''
filename = "{}episode_{:0>4d}/90_N_LeftCamera".format(args.root_path, episode) + ".txt"
with open(filename, 'a') as myfile:
for x in np.asarray(camera_90_n_l_to_world_transform.matrix[:3, :]).reshape(-1):
line += "{:.8e} ".format(x)
line = line[:-1]
line += "\n"
myfile.write(line)
line = ""
filename = "{}episode_{:0>4d}/90_N_RightCamera".format(args.root_path, episode) + ".txt"
with open(filename, 'a') as myfile:
for x in np.asarray(camera_90_n_r_to_world_transform.matrix[:3, :]).reshape(-1):
line += "{:.8e} ".format(x)
line = line[:-1]
line += "\n"
myfile.write(line)
line = ""
filename = "{}episode_{:0>4d}/90_P_LeftCamera".format(args.root_path, episode) + ".txt"
with open(filename, 'a') as myfile:
for x in np.asarray(camera_90_p_l_to_world_transform.matrix[:3, :]).reshape(-1):
line += "{:.8e} ".format(x)
line = line[:-1]
line += "\n"
myfile.write(line)
line = ""
filename = "{}episode_{:0>4d}/90_P_RightCamera".format(args.root_path, episode) + ".txt"
with open(filename, 'a') as myfile:
for x in np.asarray(camera_90_p_r_to_world_transform.matrix[:3, :]).reshape(-1):
line += "{:.8e} ".format(x)
line = line[:-1]
line += "\n"
myfile.write(line)
line = ""
'''
# We can access the encoded data of a given image as numpy
# array using its "data" property. For instance, to get the
# depth value (normalized) at pixel X, Y
#
# depth_array = sensor_data['CameraDepth'].data
# value_at_pixel = depth_array[Y, X]
#
# Now we have to send the instructions to control the vehicle.
# If we are in synchronous mode the server will pause the
# simulation until we send this control.
if not args.autopilot:
client.send_control(
steer=random.uniform(-1.0, 1.0),
throttle=0.5,
brake=0.0,
hand_brake=False,
reverse=False)
else:
# Together with the measurements, the server has sent the
# control that the in-game autopilot would do this frame. We
# can enable autopilot by sending back this control to the
# server. We can modify it if wanted, here for instance we
# will add some noise to the steer.
control = measurements.player_measurements.autopilot_control
#control.steer += random.uniform(-0.1, 0.1)
client.send_control(control)
