def get_relative_rotation_y(agent, player):
""" Returns the relative rotation of the agent to the camera in yaw
The relative rotation is the difference between the camera rotation (on car) and the agent rotation"""
# We only car about the rotation for the classes we do detection on
if agent.get_transform():
rot_agent = agent.get_transform().rotation.yaw
rot_car = player.get_transform().rotation.yaw
return degrees_to_radians(rot_agent - rot_car)
def get_relative_rotation_y(agent, player_measurements):
""" Returns the relative rotation of the agent to the camera in yaw
The relative rotation is the difference between the camera rotation (on car) and the agent rotation"""
# We only car about the rotation for the classes we do detection on
if agent.vehicle.transform:
rot_agent = agent.vehicle.transform.rotation.yaw
rot_car = player_measurements.transform.rotation.yaw
# print("=========================================", rot_agent, rot_car)
return degrees_to_radians(rot_agent - rot_car)
def save_groundplanes(planes_fname, player_measurements, lidar_height):
from math import cos, sin
""" Saves the groundplane vector of the current frame.
The format of the ground plane file is first three lines describing the file (number of parameters).
The next line is the three parameters of the normal vector, and the last is the height of the normal vector,
which is the same as the distance to the camera in meters.
"""
rotation = player_measurements.transform.rotation
pitch, roll = rotation.pitch, rotation.roll
# Since measurements are in degrees, convert to radians
pitch = degrees_to_radians(pitch)
roll = degrees_to_radians(roll)
# Rotate normal vector (y) wrt. pitch and yaw
normal_vector = [
cos(pitch) * sin(roll), -cos(pitch) * cos(roll),
sin(pitch)
]
normal_vector = map(str, normal_vector)
with open(planes_fname, 'w') as f:
f.write("# Plane\n")
f.write("Width 4\n")
f.write("Height 1\n")
f.write("{} {}\n".format(" ".join(normal_vector), lidar_height))
logging.info("Wrote plane data to %s", planes_fname)
def processing(world, image_rgb, image_depth, image_lidar, intrinsic, player,
agents, camera_to_car_transform, lidar_to_car_transform,
gen_time):
if image_rgb is not None and image_depth is not None:
# Convert main image
image = to_rgb_array(image_rgb)
extrinsic = get_matrix(
player.get_transform()) * camera_to_car_transform
# Retrieve and draw datapoints
#IMAGE IS AN ARRAY!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
image, datapoints = generate_datapoints(world, image, intrinsic,
extrinsic, image_depth, player,
agents, gen_time)
#Lidar signal processing
# Calculation to shift bboxes relative to pitch and roll of player
rotation = player.get_transform().rotation
pitch, roll, yaw = rotation.pitch, rotation.roll, rotation.yaw
# Since measurements are in degrees, convert to radians
pitch = degrees_to_radians(pitch)
roll = degrees_to_radians(roll)
yaw = degrees_to_radians(yaw)
#print('pitch: ', pitch)
#print('roll: ', roll)
#print('yaw: ', yaw)
# Rotation matrix for pitch
rotP = np.array([[cos(pitch), 0, sin(pitch)], [0, 1, 0],
[-sin(pitch), 0, cos(pitch)]])
# Rotation matrix for roll
rotR = np.array([[1, 0, 0], [0, cos(roll), -sin(roll)],
[0, sin(roll), cos(roll)]])
# combined rotation matrix, must be in order roll, pitch, yaw
rotRP = np.matmul(rotR, rotP)
# Take the points from the point cloud and transform to car space
pc_arr_cpy = np.frombuffer(image_lidar.raw_data, dtype=np.dtype('f4'))
#print(pc_arr.shape)
pc_arr = np.reshape(pc_arr_cpy,
(int(pc_arr_cpy.shape[0] / 4), 4))[:, :3].copy()
#print(pc_arr.shape)
pc_arr[:, [0, 2]] = -pc_arr[:, [0, 2]]
pc_arr[:, [0, 1]] = pc_arr[:, [1, 0]]
#print(pc_arr.shape)
point_cloud = np.array(transform_points(pc_arr,
lidar_to_car_transform))
#print(lidar_to_car_transform)
point_cloud[:, 2] -= LIDAR_HEIGHT_POS
#print(point_cloud.shape)
point_cloud = np.matmul(rotRP, point_cloud.T).T
# print(self._lidar_to_car_transform.matrix)
# print(self._camera_to_car_transform.matrix)
# Draw lidar
# Camera coordinate system is left, up, forwards
if VISUALIZE_LIDAR:
# Transform to camera space by the inverse of camera_to_car transform
point_cloud_cam = transform_points(
point_cloud, np.linalg.inv(camera_to_car_transform))
point_cloud_cam[:, 1] += LIDAR_HEIGHT_POS
image = lidar_utils.project_point_cloud(image, point_cloud_cam,
intrinsic, 1)
#determine whether to save data
#TO_DO!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
if image is not None:
return image, datapoints, point_cloud, extrinsic
else:
logging.debug("Image is None")
def __init__(self,
tcp_host_ip,
tcp_port,
rtc_host_ip,
rtc_port,
camera_calibration_file=''):
# Connect to robot client
self.tcp_host_ip = tcp_host_ip
self.tcp_port = tcp_port
# Connect as real-time client to parse state data
self.rtc_host_ip = rtc_host_ip
self.rtc_port = rtc_port
# Default home joint configuration
self.home_joint_config = [
utils.degrees_to_radians(90), #BASE
utils.degrees_to_radians(-135), #HOMBRO
utils.degrees_to_radians(145), #CODO
utils.degrees_to_radians(-100), #MUÑ1
utils.degrees_to_radians(270), #MUÑ2
utils.degrees_to_radians(0), #MUÑ3
]
# Default photo joint configuration
self.photo_joint_config = [
utils.degrees_to_radians(115.60), #BASE
utils.degrees_to_radians(-57.30), #HOMBRO
utils.degrees_to_radians(8.65), #CODO
utils.degrees_to_radians(-40.91), #MUÑ1
utils.degrees_to_radians(273.94), #MUÑ2
utils.degrees_to_radians(-65.93), #MUÑ3
]
# Default joint speed configuration
self.JOINT_ACC_DEFAULT = 8
self.JOINT_VEL_DEFAULT = 3
self.JOINT_ACC_SAFE = 1.4
self.JOINT_VEL_SAFE = 1.05
self.joint_acc = self.JOINT_ACC_DEFAULT # default: 8, safe: 1.4 m/s^2
self.joint_vel = self.JOINT_VEL_DEFAULT # default: 3, safe: 1.05 m/s
# Joint tolerance for blocking calls
self.joint_tolerance = 0.1
# Default tool speed configuration
self.TOOL_ACC_DEFAULT = 1.2
self.TOOL_VEL_DEFAULT = 0.25
self.TOOL_ACC_SAFE = 0.5
self.TOOL_VEL_SAFE = 0.2
self.tool_acc = self.TOOL_ACC_DEFAULT # default: 1.2, safe: 0.5
self.tool_vel = self.TOOL_VEL_DEFAULT # default: 0.25, safe: 0.2
# Tool pose tolerance for blocking calls
self.tool_pose_tolerance = [0.002, 0.002, 0.002, 0.01, 0.01, 0.01]
# Fetch RGB-D data from RealSense camera
self.camera = Camera()
# Load camera pose (from running calibrate.py), intrinsics and depth scale
self.cam_pose = np.loadtxt('camera_pose.txt', delimiter=' ')
self.cam_depth_scale = np.loadtxt('camera_depth_scale.txt',
delimiter=' ')
def _generate_datapoints(self, image):
""" Returns a list of datapoints (labels and such) that are generated this frame together with the main image image """
datapoints = {}
# print('{0:06}.jpg'.format(self.captured_frame_no))
imgNameFmt = os.path.join("{0}", "{1:06}.jpg")
datapoints['name'] = imgNameFmt.format(TIME_ON_NEW_EPISODE,
self.captured_frame_no)
datapoints['videoName'] = TIME_ON_NEW_EPISODE
# datapoints.append(imgNameFmt.format(TIME_ON_NEW_EPISODE, self.captured_frame_no))
datapoints['resolution'] = {'width': 1920, 'height': 1080}
datapoints['attributes'] = {
"weather": "clouds",
"scene": "undefined",
"timeofday": "noon"
}
datapoints['intrinsics'] = {
"focal": [960.0000000000001, 960.0000000000001],
"center": [960, 540],
"fov":
90.0,
"nearClip":
0.15,
"cali": [[960.0000000000001, 0, 960, 0],
[0, 960.0000000000001, 540, 0], [0, 0, 1, 0]]
}
rot = self._measurements.player_measurements.transform.rotation
loc = self._measurements.player_measurements.transform.location
datapoints['extrinsics'] = {
"location": [-loc.x, -loc.y, loc.z],
"rotation": [
degrees_to_radians(rot.pitch),
degrees_to_radians(rot.roll),
degrees_to_radians(rot.yaw)
]
}
datapoints['timestamp'] = int(time.time())
datapoints['frameIndex'] = self.captured_frame_no
image = image.copy()
# Remove this
rotRP = np.identity(3)
datapoints['labels'] = []
# Stores all datapoints for the current frames
for agent in self._measurements.non_player_agents:
if should_detect_class(agent) and GEN_DATA:
image, kitti_datapoint = create_kitti_datapoint(
agent, self._intrinsic, self._extrinsic.matrix, image,
self._depth_image, self._measurements.player_measurements,
rotRP)
if kitti_datapoint:
orientation = degrees_to_radians(
agent.vehicle.transform.rotation.yaw)
ob_center_x = (kitti_datapoint.bbox[2] +
kitti_datapoint.bbox[0]) / 2
rotation_y2alpha = orientation - np.arctan2(
960.0000000000001, (ob_center_x - 960))
rotation_y2alpha = rotation_y2alpha % (2 * np.pi) - np.pi
# print(kitti_datapoint)
datapoints['labels'].append({
'id': int(agent.id),
'category': kitti_datapoint.type,
'manualShape': False,
'manualAttributes': False,
'attributes': {
"occluded": kitti_datapoint.occluded,
"truncated": kitti_datapoint.truncated,
"ignore": False
},
'box2d': {
'x1': kitti_datapoint.bbox[0],
'y1': kitti_datapoint.bbox[1],
'x2': kitti_datapoint.bbox[2],
'y2': kitti_datapoint.bbox[3],
'confidence': 1.0
},
'box3d': {
"alpha": rotation_y2alpha,
"orientation": orientation,
"location": kitti_datapoint.location,
"dimension": kitti_datapoint.dimensions
}
})
return image, datapoints
def _on_render(self):
datapoints = []
if self._main_image is not None and self._depth_image is not None:
# Convert main image
image = image_converter.to_rgb_array(self._main_image)
# Retrieve and draw datapoints
image, datapoints = self._generate_datapoints(image)
# Draw lidar
# Camera coordinate system is left, up, forwards
if VISUALIZE_LIDAR:
# Calculation to shift bboxes relative to pitch and roll of player
rotation = self._measurements.player_measurements.transform.rotation
pitch, roll, yaw = rotation.pitch, rotation.roll, rotation.yaw
# Since measurements are in degrees, convert to radians
pitch = degrees_to_radians(pitch)
roll = degrees_to_radians(roll)
yaw = degrees_to_radians(yaw)
print('pitch: ', pitch)
print('roll: ', roll)
print('yaw: ', yaw)
# Rotation matrix for pitch
rotP = np.array([[cos(pitch), 0, sin(pitch)], [0, 1, 0],
[-sin(pitch), 0, cos(pitch)]])
# Rotation matrix for roll
rotR = np.array([[1, 0, 0], [0, cos(roll), -sin(roll)],
[0, sin(roll), cos(roll)]])
# combined rotation matrix, must be in order roll, pitch, yaw
rotRP = np.matmul(rotR, rotP)
# Take the points from the point cloud and transform to car space
point_cloud = np.array(
self._lidar_to_car_transform.transform_points(
self._lidar_measurement.data))
point_cloud[:, 2] -= LIDAR_HEIGHT_POS
point_cloud = np.matmul(rotRP, point_cloud.T).T
# print(self._lidar_to_car_transform.matrix)
# print(self._camera_to_car_transform.matrix)
# Transform to camera space by the inverse of camera_to_car transform
point_cloud_cam = self._camera_to_car_transform.inverse(
).transform_points(point_cloud)
point_cloud_cam[:, 1] += LIDAR_HEIGHT_POS
image = lidar_utils.project_point_cloud(
image, point_cloud_cam, self._intrinsic, 1)
# Display image
surface = pygame.surfarray.make_surface(image.swapaxes(0, 1))
self._display.blit(surface, (0, 0))
if self._map_view is not None:
self._display_agents(self._map_view)
pygame.display.flip()
# Determine whether to save files
distance_driven = self._distance_since_last_recording()
#print("Distance driven since last recording: {}".format(distance_driven))
has_driven_long_enough = distance_driven is None or distance_driven > DISTANCE_SINCE_LAST_RECORDING
if (self._timer.step + 1) % STEPS_BETWEEN_RECORDINGS == 0:
if has_driven_long_enough and datapoints:
# Avoid doing this twice or unnecessarily often
if not VISUALIZE_LIDAR:
# Calculation to shift bboxes relative to pitch and roll of player
rotation = self._measurements.player_measurements.transform.rotation
pitch, roll, yaw = rotation.pitch, rotation.roll, rotation.yaw
# Since measurements are in degrees, convert to radians
pitch = degrees_to_radians(pitch)
roll = degrees_to_radians(roll)
yaw = degrees_to_radians(yaw)
print('pitch: ', pitch)
print('roll: ', roll)
print('yaw: ', yaw)
# Rotation matrix for pitch
rotP = np.array([[cos(pitch), 0,
sin(pitch)], [0, 1, 0],
[-sin(pitch), 0,
cos(pitch)]])
# Rotation matrix for roll
rotR = np.array([[1, 0, 0], [0,
cos(roll), -sin(roll)],
[0, sin(roll),
cos(roll)]])
# combined rotation matrix, must be in order roll, pitch, yaw
rotRP = np.matmul(rotR, rotP)
# Take the points from the point cloud and transform to car space
point_cloud = np.array(
self._lidar_to_car_transform.transform_points(
self._lidar_measurement.data))
point_cloud[:, 2] -= LIDAR_HEIGHT_POS
point_cloud = np.matmul(rotRP, point_cloud.T).T
self._update_agent_location()
# Save screen, lidar and kitti training labels together with calibration and groundplane files
self._save_training_files(datapoints, point_cloud)
self.captured_frame_no += 1
self._captured_frames_since_restart += 1
self._frames_since_last_capture = 0
else:
logging.debug(
"Could save datapoint, but agent has not driven {} meters since last recording (Currently {} meters)"
.format(DISTANCE_SINCE_LAST_RECORDING,
distance_driven))
else:
self._frames_since_last_capture += 1
logging.debug(
"Could not save training data - no visible agents of selected classes in scene"
)
def _on_render(self):
datapoints = []
if self._main_image is not None and self._depth_image is not None:
# Convert main image
image = image_converter.to_rgb_array(self._main_image)
# Retrieve and draw datapoints
image, datapoints = self._generate_datapoints(image)
# Draw lidar
# Camera coordinate system is left, up, forwards
if VISUALIZE_LIDAR:
# Calculation to shift bboxes relative to pitch and roll of player
rotation = self._measurements.player_measurements.transform.rotation
pitch, roll, yaw = rotation.pitch, rotation.roll, rotation.yaw
# Since measurements are in degrees, convert to radians
pitch = degrees_to_radians(pitch)
roll = degrees_to_radians(roll)
yaw = degrees_to_radians(yaw)
print('pitch: ', pitch)
print('roll: ', roll)
print('yaw: ', yaw)
# Rotation matrix for pitch
rotP = np.array([[cos(pitch), 0, sin(pitch)], [0, 1, 0],
[-sin(pitch), 0, cos(pitch)]])
# Rotation matrix for roll
rotR = np.array([[1, 0, 0], [0, cos(roll), -sin(roll)],
[0, sin(roll), cos(roll)]])
# combined rotation matrix, must be in order roll, pitch, yaw
rotRP = np.matmul(rotR, rotP)
# Take the points from the point cloud and transform to car space
point_cloud = np.array(
self._lidar_to_car_transform.transform_points(
self._lidar_measurement.data))
point_cloud[:, 2] -= LIDAR_HEIGHT_POS
point_cloud = np.matmul(rotRP, point_cloud.T).T
# Transform to camera space by the inverse of camera_to_car transform
point_cloud_cam = self._camera_to_car_transform.inverse(
).transform_points(point_cloud)
point_cloud_cam[:, 1] += LIDAR_HEIGHT_POS
image = lidar_utils.project_point_cloud(
image, point_cloud_cam, self._intrinsic, 1)
# Display image
surface = pygame.surfarray.make_surface(image.swapaxes(0, 1))
self._display.blit(surface, (0, 0))
pygame.display.flip()
if (self._timer.step + 1) % STEPS_BETWEEN_RECORDINGS == 0:
if datapoints:
# Avoid doing this twice or unnecessarily often
if not VISUALIZE_LIDAR:
# Calculation to shift bboxes relative to pitch and roll of player
rotation = self._measurements.player_measurements.transform.rotation
pitch, roll, yaw = rotation.pitch, rotation.roll, rotation.yaw
# Since measurements are in degrees, convert to radians
pitch = degrees_to_radians(pitch)
roll = degrees_to_radians(roll)
yaw = degrees_to_radians(yaw)
print('pitch: ', pitch)
print('roll: ', roll)
print('yaw: ', yaw)
# Rotation matrix for pitch
rotP = np.array([[cos(pitch), 0,
sin(pitch)], [0, 1, 0],
[-sin(pitch), 0,
cos(pitch)]])
# Rotation matrix for roll
rotR = np.array([[1, 0, 0], [0,
cos(roll), -sin(roll)],
[0, sin(roll),
cos(roll)]])
# combined rotation matrix, must be in order roll, pitch, yaw
rotRP = np.matmul(rotR, rotP)
# Take the points from the point cloud and transform to car space
point_cloud = np.array(
self._lidar_to_car_transform.transform_points(
self._lidar_measurement.data))
point_cloud[:, 2] -= LIDAR_HEIGHT_POS
point_cloud = np.matmul(rotRP, point_cloud.T).T
self._update_agent_location()
# Save screen, lidar and kitti training labels together with calibration and groundplane files
# self._save_training_files(datapoints, point_cloud)
img = get_image_data(
image_converter.to_rgb_array(self._main_image))
lid = get_lidar_data(point_cloud)
calib = get_calibration_matrices(self._intrinsic,
self._extrinsic)
mess = {
"idx": self.captured_frame_no,
"image_2": img,
"velodyne": lid,
"calib": calib
}
annos = self.evaluator.predict(mess)
# annos = self.evaluator.evaluate(self.captured_frame_no)
print(annos)
scores = annos["score"]
vertices = annos["vertices"]
bbox = annos["bbox"]
for i, score in enumerate(scores):
if score * 100 > 35:
# draw 3D
image = draw_projected_box3d(image,
vertices[i],
color=(0, 0, 255),
thickness=1)
image = cv2.putText(
image, str(round(score * 100, 2)),
(int(bbox[i][0]), int(bbox[i][1])),
cv2.FONT_HERSHEY_SIMPLEX, 0.3, (255, 0, 255),
1, cv2.LINE_AA)
# draw 2D
# bbox = annos["bbox"]
# x1, y1, x2, y2 = bbox[i]
# cv2.rectangle(image, pt1=(x1, y1), pt2=(x2, y2), color=(0, 0, 255), thickness=2)
# Display image
surface = pygame.surfarray.make_surface(
image.swapaxes(0, 1))
self._display.blit(surface, (0, 0))
pygame.display.flip()
self.captured_frame_no += 1
self._captured_frames_since_restart += 1
self._frames_since_last_capture = 0
time.sleep(0.5)
else:
logging.debug("Could save datapoint".format(
DISTANCE_SINCE_LAST_RECORDING))
else:
self._frames_since_last_capture += 1
logging.debug(
"Could not save training data - no visible agents of selected classes in scene"
)
