def main():
# Parse passed-in config yaml file
argparser = argparse.ArgumentParser(
description='Offline Detection and Pole Map Generator')
argparser.add_argument('recording_dir',
type=dir_path,
help='directory of recording')
argparser.add_argument('vision_config',
type=argparse.FileType('r'),
help='yaml file for vision algorithm parameters')
argparser.add_argument(
'sim_detection_config',
type=argparse.FileType('r'),
help='yaml file for simulated detection configuration')
argparser.add_argument(
'pole_map_config',
type=argparse.FileType('r'),
help='yaml file for pole map generation configuration')
args = argparser.parse_args()
# Read carla simulation configs of the recording
path_to_config = os.path.join(args.recording_dir, 'settings/config.yaml')
with open(path_to_config, 'r') as f:
carla_config = yaml.safe_load(f)
# Read configurations for detection simulation
with args.vision_config as f:
vision_config = yaml.safe_load(f)
with args.sim_detection_config as f:
sim_detection_config = yaml.safe_load(f)
with args.pole_map_config as f:
pole_map_config = yaml.safe_load(f)
# Retrieve configs of detection simulation
pole_detection_sim_config = sim_detection_config['pole']
lane_detection_sim_config = sim_detection_config['lane']
rs_stop_detection_sim_config = sim_detection_config['rs_stop']
# Load camera parameters
with open('calib_data.pkl', 'rb') as f:
calib_data = pickle.load(f)
K = calib_data['K']
R = calib_data['R']
x0 = calib_data['x0']
# Load parameters for bird's eye view projection
with open('ipm_data.pkl', 'rb') as f:
ipm_data = pickle.load(f)
M = ipm_data['M']
warped_size = ipm_data['bev_size']
valid_mask = ipm_data['valid_mask']
px_per_meter_x = ipm_data['px_per_meter_x']
px_per_meter_y = ipm_data['px_per_meter_y']
dist_cam_to_intersect = ipm_data['dist_to_intersect']
# Distance from rear axle to front bumper
dist_raxle_to_fbumper = carla_config['ego_veh']['raxle_to_fbumper']
# Distance from camera to front bumper
dist_cam_to_fbumper = (dist_raxle_to_fbumper -
carla_config['sensor']['front_camera']['pos_x'] -
carla_config['ego_veh']['raxle_to_cg'])
# For lane detector
# Distance from front bumper to intersection point between camera's vertical FOV and ground surface
dist_fbumper_to_intersect = dist_cam_to_intersect - dist_cam_to_fbumper
# Load recorded data that are stored in dictionaries
with open(os.path.join(args.recording_dir, 'sensor_data.pkl'), 'rb') as f:
sensor_data = pickle.load(f)
with open(os.path.join(args.recording_dir, 'gt_data.pkl'), 'rb') as f:
gt_data = pickle.load(f)
# Retrieve required sensor data
ss_images = sensor_data['semantic_camera']['ss_image']
depth_buffers = sensor_data['depth_camera']['depth_buffer']
yaw_rates = sensor_data['imu']['gyro_z']
# Retrieve required ground truth data
traffic_signs = gt_data['static']['traffic_sign']
raxle_locations = gt_data['seq']['pose']['raxle_location']
raxle_orientations = gt_data['seq']['pose']['raxle_orientation']
lane_id_seq = gt_data['seq']['lane']['lane_id']
next_left_marking_coeffs_seq = gt_data['seq']['lane'][
'next_left_marking_coeffs']
next_left_marking_seq = gt_data['seq']['lane']['next_left_marking']
left_marking_coeffs_seq = gt_data['seq']['lane']['left_marking_coeffs']
left_marking_seq = gt_data['seq']['lane']['left_marking']
right_marking_coeffs_seq = gt_data['seq']['lane']['right_marking_coeffs']
right_marking_seq = gt_data['seq']['lane']['right_marking']
next_right_marking_coeffs_seq = gt_data['seq']['lane'][
'next_right_marking_coeffs']
next_right_marking_seq = gt_data['seq']['lane']['next_right_marking']
# Create detector objects
pole_detector = PoleDetector(K, R, x0, vision_config['pole'])
lane_detector = LaneMarkingDetector(M, px_per_meter_x, px_per_meter_y,
warped_size, valid_mask,
dist_fbumper_to_intersect,
vision_config['lane'])
rs_stop_detector = RSStopDetectionSimulator(traffic_signs,
rs_stop_detection_sim_config)
# Create a pole detector that has a stronger ability to extract poles, which is for building the pole map
# It is stronger in that it allows smaller region of poles to be extracted so the pole map can contain more poles than those
# detected by pole_detector
pole_detector_for_pole_map = PoleDetector(
K, R, x0, pole_map_config['pole_extraction'])
# Container to accumulate accurate world coordinates of poles at each step
all_accurate_poles = []
# Containers for detection sequences
# Sequence of pole detecions
pole_detection_seq = []
# The accurate version of detected poles in world frame
accurate_pole_detections_in_world_seq = []
lane_marking_detection_seq = []
rs_stop_detecion_seq = []
# Loop over recorded data
for image_idx, (ss_image,
depth_buffer) in enumerate(zip(ss_images, depth_buffers)):
# Retrieve data at current step
pole_image = (ss_image == 5).astype(np.uint8)
lane_image = (ss_image == 6) | (ss_image == 8).astype(np.uint8)
depth_image = decode_depth(depth_buffer)
# The pose of rear axle at current step
raxle_location = raxle_locations[image_idx]
raxle_orientation = raxle_orientations[image_idx]
# Front bumper's pose at current step
fbumper_location = get_fbumper_location(raxle_location,
raxle_orientation,
dist_raxle_to_fbumper)
fbumper_orientation = raxle_orientation
# Lane markings
lane_id = lane_id_seq[image_idx]
next_left_coeffs_gt = next_left_marking_coeffs_seq[image_idx]
next_left_marking_gt = next_left_marking_seq[image_idx]
left_coeffs_gt = left_marking_coeffs_seq[image_idx]
left_marking_gt = left_marking_seq[image_idx]
right_coeffs_gt = right_marking_coeffs_seq[image_idx]
right_marking_gt = right_marking_seq[image_idx]
next_right_coeffs_gt = next_right_marking_coeffs_seq[image_idx]
next_right_marking_gt = next_right_marking_seq[image_idx]
# Instantiate a Transform object to make transformation between front bumper and world
fbumper2world_tfrom = Transform.from_conventional(
fbumper_location, fbumper_orientation)
############ Pole detection (wrt front bumper) ############
# x-y coordinates assuming z = 0
# Coordinates are based on the bottom center of bboxes
poles_xy_z0 = pole_detector.update_poles(pole_image,
z=0,
use_bbox_center=True,
upper_lim=305)
# Find the pole bases again using the lowest pixel
# This is to get really accurate measurements for proximity-based labelling later
pole_detector.find_pole_bases(pole_image,
use_bbox_center=False,
upper_lim=305)
# Accurate x-y-z coordinates using ground truth depth image
accurate_detected_pole_xyz = pole_detector.get_pole_xyz_from_depth(
depth_image, dist_cam_to_fbumper)
if poles_xy_z0 is not None:
pole_detection_seq.append(
[Pole(coord[0], coord[1]) for coord in poles_xy_z0.T])
# Add accurate detected poles to the container. This is for proximity-based labelling later.
accurate_detected_in_world = fbumper2world_tfrom.tform_e2w_numpy_array(
accurate_detected_pole_xyz)
accurate_pole_detections_in_world_seq.append(
accurate_detected_in_world)
else:
pole_detection_seq.append(None)
accurate_pole_detections_in_world_seq.append(None)
############ Lane detection (wrt front bumper) ############
left_coeffs, right_coeffs = lane_detector.update_lane_coeffs(
lane_image, yaw_rates[image_idx])
# Add color and type properties as part of lane detections
# The simulated detections are first compared to the ground truth. If the coefficients are close to the ground truth,
# the recorded marking properties are used as the classification result.
# It should be noted that due to the lane detection algorithm implementation details, when the ego vehicle makes a lane
# change, the detected lane markings will switch to the next lane before the front bumper's center actually enters the next lane.
# So a detection is also compared to ground truth types on the other side.
# When there is a false positive detection, whose coefficients are not consistent with the ground truth,
# the Unknown type will be used as default.
# Perturbations are applied afterwards to mess the type a bit to simulate noise.
# Coeffs from detector are in descending order, while those from ground truth are in ascending order
if left_coeffs is None:
# No detection
left_detection = None
elif left_marking_gt and \
abs(left_coeffs[-1] - left_coeffs_gt[0]) < lane_detection_sim_config['c0_thres'] and \
abs(left_coeffs[-2] - left_coeffs_gt[1]) < lane_detection_sim_config['c1_thres']:
# True positive (close to left lane marking gt)
# Thresholding doesn't guarantee definite true positive nevertheless
left_detection = MELaneMarking.from_lane_marking(
left_coeffs, left_marking_gt, lane_id)
# Perturb lane marking type
left_detection.perturb_type(lane_detection_sim_config['fc_prob'])
elif next_left_marking_gt and \
abs(left_coeffs[-1] - next_left_coeffs_gt[0]) < lane_detection_sim_config['c0_thres'] and \
abs(left_coeffs[-2] - next_left_coeffs_gt[1]) < lane_detection_sim_config['c1_thres']:
# True positive (close to next left lane marking gt)
# Thresholding doesn't guarantee definite true positive nevertheless
left_detection = MELaneMarking.from_lane_marking(
left_coeffs, next_left_marking_gt, lane_id)
# Perturb lane marking type
left_detection.perturb_type(lane_detection_sim_config['fc_prob'])
elif right_marking_gt and \
abs(left_coeffs[-1] - right_coeffs_gt[0]) < lane_detection_sim_config['c0_thres'] and \
abs(left_coeffs[-2] - right_coeffs_gt[1]) < lane_detection_sim_config['c1_thres']:
# True positive (close to right lane marking gt)
# Thresholding doesn't guarantee definite true positive nevertheless
left_detection = MELaneMarking.from_lane_marking(
left_coeffs, right_marking_gt, lane_id)
# Perturb lane marking type
left_detection.perturb_type(lane_detection_sim_config['fc_prob'])
else:
# False positive
# Use unknown type as default for false detection
left_detection = MELaneMarking(left_coeffs, LaneMarkingColor.Other,
MELaneMarkingType.Unknown)
# Perturb lane marking type so it could be a type other than unknown
left_detection.perturb_type(
lane_detection_sim_config['fc_prob_false_positive'])
if right_coeffs is None:
# No detection
right_detection = None
elif right_marking_gt and \
abs(right_coeffs[-1] - right_coeffs_gt[0]) < lane_detection_sim_config['c0_thres'] and \
abs(right_coeffs[-2] - right_coeffs_gt[1]) < lane_detection_sim_config['c1_thres']:
# True positive (close to left lane marking gt)
# Thresholding doesn't guarantee definite true positive nevertheless
right_detection = MELaneMarking.from_lane_marking(
right_coeffs, right_marking_gt, lane_id)
# Perturb lane marking type
right_detection.perturb_type(lane_detection_sim_config['fc_prob'])
elif next_right_marking_gt and \
abs(right_coeffs[-1] - next_right_coeffs_gt[0]) < lane_detection_sim_config['c0_thres'] and \
abs(right_coeffs[-2] - next_right_coeffs_gt[1]) < lane_detection_sim_config['c1_thres']:
# True positive (close to next right lane marking gt)
# Thresholding doesn't guarantee definite true positive nevertheless
right_detection = MELaneMarking.from_lane_marking(
right_coeffs, next_right_marking_gt, lane_id)
# Perturb lane marking type
right_detection.perturb_type(lane_detection_sim_config['fc_prob'])
elif left_marking_gt and \
abs(right_coeffs[-1] - left_coeffs_gt[0]) < lane_detection_sim_config['c0_thres'] and \
abs(right_coeffs[-2] - left_coeffs_gt[1]) < lane_detection_sim_config['c1_thres']:
# True positive (close to left lane marking gt)
# Thresholding doesn't guarantee definite true positive nevertheless
right_detection = MELaneMarking.from_lane_marking(
right_coeffs, left_marking_gt, lane_id)
# Perturb lane marking type
right_detection.perturb_type(lane_detection_sim_config['fc_prob'])
else:
# False positive
# Use unknown type as default for false detection
right_detection = MELaneMarking(right_coeffs,
LaneMarkingColor.Other,
MELaneMarkingType.Unknown)
# Perturb lane marking type so it could be a type other than unknown
right_detection.perturb_type(
lane_detection_sim_config['fc_prob_false_positive'])
lane_marking_detection_seq.append(
MELaneDetection(left_detection, right_detection))
############ RS stop sign detection (wrt front bumper) ############
longi_dist_to_rs_stop = rs_stop_detector.update_rs_stop(
fbumper_location, fbumper_orientation)
rs_stop_detecion_seq.append(longi_dist_to_rs_stop)
############ Accurate poles for building pole map ############
# The bases in image are stored internally in the detector
pole_detector_for_pole_map.find_pole_bases(pole_image,
use_bbox_center=False)
accurate_pole_xyz = pole_detector_for_pole_map.get_pole_xyz_from_depth(
depth_image, dist_cam_to_fbumper)
if accurate_pole_xyz is not None:
# Filter out points that are too high to focus on the lower part of poles
accurate_pole_xyz = accurate_pole_xyz[:, accurate_pole_xyz[
2, :] < 0.5]
# Transform accurate poles to world frame
accurate_pole_xyz_world = fbumper2world_tfrom.tform_e2w_numpy_array(
accurate_pole_xyz)
all_accurate_poles.append(accurate_pole_xyz_world)
print(image_idx)
############ Pole map generation ############
# Concatenate all poles as an np.array
all_accurate_poles_xy = np.concatenate(all_accurate_poles,
axis=1)[0:2, :] # 2-by-N
pole_map = gen_pole_map(all_accurate_poles_xy, traffic_signs,
pole_map_config)
############ Proximity-based pole detecion labelling ############
if __debug__:
for poles in accurate_pole_detections_in_world_seq:
if poles is not None:
plt.plot(poles[0, :], poles[1, :], 'ro', ms=1)
plt.title('Accurate Detected Poles')
# Make a kd-tree out of pole map for later queries
pole_map_coords = np.asarray([[pole.x, pole.y] for pole in pole_map])
kd_poles = KDTree(pole_map_coords)
# Loop over accurate detection sequence and try to associate it to the nearest pole landmark in the pole map
for detections, accurate_detections in zip(
pole_detection_seq, accurate_pole_detections_in_world_seq):
if accurate_detections is not None:
for pole_idx, accurate_detection in enumerate(
accurate_detections.T):
nearest_dist, nearest_idx = kd_poles.query(
accurate_detection[0:2])
if nearest_dist < pole_detection_sim_config['max_dist']:
detections[pole_idx].type = pole_map[nearest_idx].type
# Perturb pole detections at this time step
for detection in detections:
detection.perturb_type(pole_detection_sim_config['fc_prob'])
############ Save data ############
# Copy configuration files for future reference
dst = os.path.join(args.recording_dir, 'settings/vision.yaml')
copyfile(args.vision_config.name, dst)
dst = os.path.join(args.recording_dir, 'settings/sim_detection.yaml')
copyfile(args.sim_detection_config.name, dst)
dst = os.path.join(args.recording_dir, 'settings/pole_map.yaml')
copyfile(args.pole_map_config.name, dst)
# Save detections
detections = {}
detections['pole'] = pole_detection_seq
detections['lane'] = lane_marking_detection_seq
detections['rs_stop'] = rs_stop_detecion_seq
with open(os.path.join(args.recording_dir, 'detections.pkl'), 'wb') as f:
pickle.dump(detections, f)
# Save pole map
with open(os.path.join(args.recording_dir, 'pole_map.pkl'), 'wb') as f:
pickle.dump(pole_map, f)
def error(self, variables):
########## Expectation ##########
pose = variables.at(self.keys()[0])
# Append 0 as z since this factor is in 2D space
location = np.append(pose.translation(), self.z)
orientation = np.array([0, 0, pose.so2().theta()])
fbumper_location = get_fbumper_location(location,
orientation,
self.px)
self.expected_rs_stop_dists = self.expected_rs_stop_extractor.extract(fbumper_location,
orientation)
########## Measurement ##########
# No need for parsing measurement since it is so simple
# Null hypothesis
# Use the measurement itself at every optimization iteration as the null hypothesis.
# This is, of course, just a trick.
# This means the error for null hypothesis is always zeros.
null_expected_dist = self.detected_rs_stop_dist
null_error = np.zeros((1, 1))
# Compute innovation matrix for the null hypo
null_noise_var = self.noise_var * self.null_std_scale**2
# Compute measurement likelihood weighted by null probability
null_weighted_meas_likelihood = self.prob_null \
* univariate_normal_pdf(null_error.squeeze(), var=null_noise_var)
########## Data Association ##########
if not self.expected_rs_stop_dists:
self._null_hypo = True
else:
# Note: No gating is performed
errors = [null_error]
exp_dist_list = [null_expected_dist]
meas_likelihoods = []
asso_probs = []
for exp_dist in self.expected_rs_stop_dists:
H = np.array([[-1.0, 0, 0]])
innov = (H @ self.pose_uncert @ H.T + self.noise_var).squeeze()
# Compute squared mahalanobis distance
error = np.asarray(exp_dist).reshape(
1, -1) - self.detected_rs_stop_dist
errors.append(error)
exp_dist_list.append(exp_dist)
# Measurement likelihood (based on noise cov)
meas_likelihood = univariate_normal_pdf(error.squeeze(),
var=self.noise_var)
# Geometric likelihood (based on innov)
geo_likelihood = univariate_normal_pdf(error.squeeze(),
var=innov)
# Due to numerical errors, likelihood can become exactly 0.0
# in some very rare cases.
# When it happens, simply ignore it.
if meas_likelihood > 0.0 and geo_likelihood > 0.0:
meas_likelihoods.append(meas_likelihood)
asso_probs.append(geo_likelihood)
asso_probs = np.asarray(asso_probs)
meas_likelihoods = np.asarray(meas_likelihoods)
# Compute weights based on total probability theorem
weights = (1-self.prob_null) * \
(asso_probs/np.sum(asso_probs))
# Weight measurement likelihoods
weighted_meas_likelihood = weights*meas_likelihoods
# Add weight and weighted likelihood of null hypothesis
weights = np.insert(weights, 0, self.prob_null)
weighted_meas_likelihood = np.insert(
weighted_meas_likelihood, 0, null_weighted_meas_likelihood)
asso_idx = np.argmax(weighted_meas_likelihood)
if asso_idx == 0:
self._null_hypo = True
else:
self._null_hypo = False
self.chosen_expected_dist = exp_dist_list[asso_idx]
# Scale down the hypothesis to account for target uncertainty
# This form is empirically chosen
self._scale = weights[asso_idx]**1
# Scale down the error based on weight
# This is to achieve the same effect of scaling infomation matrix during optimzation
chosen_error = errors[asso_idx] * self._scale
if self._null_hypo:
# Null hypothesis
self.chosen_expected_dist = null_expected_dist
chosen_error = null_error
return chosen_error
def error(self, variables):
########## Expectation ##########
pose = variables.at(self.keys()[0])
location = np.append(pose.translation(), self.z) # append z
orientation = np.array([0, 0, pose.so2().theta()])
if self._first_time:
# Store the initially guessed pose when computing error the first time
self._init_tform = Transform.from_conventional(
location, orientation)
self._init_orientation = orientation
if self.static:
# Static mode
if self._first_time:
# First time extracting expected land boundaries
fbumper_location = get_fbumper_location(
location, orientation, self.px)
self.in_junction, self.into_junction, self.me_format_expected_markings = self.expected_lane_extractor.extract(
fbumper_location, orientation)
expected_coeffs_list = [expected.get_c0c1_list()
for expected in self.me_format_expected_markings]
expected_type_list = [expected.type
for expected in self.me_format_expected_markings]
# The snapshot is stored in their normal forms; i.e. a, b, c, and alpha describing the lines
self._init_normal_forms = [compute_normal_form_line_coeffs(self.px, c[0], c[1])
for c in expected_coeffs_list]
# Snapshot of lane boundary types
self._init_types = expected_type_list
self._first_time = False
else:
# Not first time, use snapshot of lane boundaries extracted the first time to compute error
# Pose difference is wrt local frame
pose_diff = self._get_pose_diff(location, orientation)
# Compute expected lane boundary coefficients using the snapshot
expected_coeffs_list = []
for normal_form in self._init_normal_forms:
c0c1 = self._compute_expected_c0c1(normal_form, pose_diff)
expected_coeffs_list.append(c0c1)
# Retrieve lane boundary types from snapshot
expected_type_list = self._init_types
else:
# Not static mode
# Extract ground truth from the Carla server
fbumper_location = get_fbumper_location(
location, orientation, self.px)
self.in_junction, self.into_junction, self.me_format_expected_markings = self.expected_lane_extractor.extract(
fbumper_location, orientation)
# List of expected markings' coefficients
expected_coeffs_list = [expected.get_c0c1_list()
for expected in self.me_format_expected_markings]
# List of expected markings' type
expected_type_list = [expected.type
for expected in self.me_format_expected_markings]
########## Measurement ##########
measured_coeffs = np.asarray(
self.detected_marking.get_c0c1_list()).reshape(2, -1)
measured_type = self.detected_marking.type
# Null hypothesis
# Use the measurement itself at every optimization iteration as the null hypothesis.
# This is, of course, just a trick.
# This means the error for null hypothesis is always zeros.
null_expected_c0c1 = measured_coeffs.squeeze().tolist()
null_error = np.zeros((2, 1))
# Compute innovation matrix for the null hypo
null_noise_cov = self.noise_cov * self.null_std_scale**2
# Compute measurement likelihood weighted by null probability
null_weighted_meas_likelihood = self.prob_null * \
multivariate_normal.pdf(null_error.squeeze(), cov=null_noise_cov)
# In this implementation, scaling down error and jacobian is done to achieve
# the same effect of tuning the information matrix online.
# Here, however, scale down error for null hypo; i.e.
# null_error /= self.null_std_scale
# is not necessary, since its always zero.
# Zero error and jacobian effectively result in zero information matrix as well.
if self.ignore_junction and (self.in_junction or self.into_junction):
self._null_hypo = True
elif not expected_coeffs_list:
self._null_hypo = True
else:
# Data association
errors = [null_error]
gated_coeffs_list = [null_expected_c0c1]
asso_probs = []
meas_likelihoods = []
sem_likelihoods = []
for exp_coeffs, exp_type in zip(expected_coeffs_list, expected_type_list):
# Compute innovation matrix
expected_c0, expected_c1 = exp_coeffs
H = compute_H(self.px, expected_c0, expected_c1)
innov = H @ self.pose_uncert @ H.T + self.noise_cov
# Compute squared mahalanobis distance
error = np.asarray(exp_coeffs).reshape(
2, -1) - measured_coeffs
# Mahalanobis distance is only needed for geometric gating
# squared_mahala_dist = error.T @ np.linalg.inv(innov) @ error
# Semantic likelihood
if self.semantic:
# Conditional probability on type
sem_likelihood = self._conditional_prob_type(
exp_type, measured_type)
else:
# Truning off semantic association is equivalent to always
# set semantic likelihood to 1.0
sem_likelihood = 1.0
# Gating (geometric and semantic)
# Reject both geometrically and semantically unlikely associations
# Note:
# Since location distribution across lanes is essentially multimodal,
# geometric gating often used when assuming location is unimodal is not
# very reasonable and will inevitablly reject possible associations.
# Here the geometric gate is set very large so we can preserve associations that
# are a bit far from the current mode (the only mode that exists in the optimization)
# but still possible when the multimodal nature is concerned.
# Or, we can simply give up geometric gating and use semantic gating only.
# The large geometric gate is an inelegant remedy after all.
# if squared_mahala_dist <= self.geo_gate and sem_likelihood > self.sem_gate:
if sem_likelihood > self.sem_gate:
gated_coeffs_list.append(exp_coeffs)
errors.append(error)
# Measurement likelihood (based on noise cov)
meas_likelihood = multivariate_normal.pdf(
error.reshape(-1), cov=self.noise_cov)
# Geometric likelihood (based on innov)
geo_likelihood = multivariate_normal.pdf(
error.reshape(-1), cov=innov)
# Due to numerical errors, likelihood can become exactly 0.0
# in some very rare cases.
# When it happens, simply ignore it.
if meas_likelihood > 0.0 and geo_likelihood > 0.0:
meas_likelihoods.append(sem_likelihood*meas_likelihood)
sem_likelihoods.append(sem_likelihood)
asso_prob = geo_likelihood * sem_likelihood
asso_probs.append(asso_prob)
# Check if any possible association exists after gating
if asso_probs:
asso_probs = np.asarray(asso_probs)
meas_likelihoods = np.asarray(meas_likelihoods)
# Compute weights based on total probability theorem
weights = (1-self.prob_null) * \
(asso_probs/np.sum(asso_probs))
# Weight measurement likelihoods
weighted_meas_likelihood = weights*meas_likelihoods
# Add weight and weighted likelihood of null hypothesis
weights = np.insert(weights, 0, self.prob_null)
weighted_meas_likelihood = np.insert(
weighted_meas_likelihood, 0, null_weighted_meas_likelihood)
asso_idx = np.argmax(weighted_meas_likelihood)
if asso_idx == 0:
self._null_hypo = True
else:
self._null_hypo = False
self.chosen_expected_coeffs = gated_coeffs_list[asso_idx]
# Scale down the hypothesis to account for target uncertainty
# This form is empirically chosen
self._scale = weights[asso_idx] * sem_likelihoods[asso_idx-1]
# Scale down the error based on weight
# This is to achieve the same effect of scaling infomation matrix during optimzation
chosen_error = errors[asso_idx] * self._scale
else:
self._null_hypo = True
if self._null_hypo:
# Null hypothesis
self.chosen_expected_coeffs = null_expected_c0c1
chosen_error = null_error
return chosen_error
def error(self, variables):
########## Expectation ##########
pose = variables.at(self.keys()[0])
location = np.append(pose.translation(), self.z) # append z
orientation = np.array([0, 0, pose.so2().theta()])
if self._first_time:
# Store the initially guessed pose when computing error the first time
self._init_tform = Transform.from_conventional(
location, orientation)
self._init_orientation = orientation
if self.static:
# Static mode
if self._first_time:
# First time extracting expected land boundaries
fbumper_location = get_fbumper_location(
location, orientation, self.px)
self.in_junction, self.into_junction, self.me_format_expected_markings = self.expected_lane_extractor.extract(
fbumper_location, orientation)
expected_coeffs_list = [expected.get_c0c1_list()
for expected in self.me_format_expected_markings]
expected_type_list = [expected.type
for expected in self.me_format_expected_markings]
# The snapshot is stored in their normal forms; i.e. a, b, c, and alpha describing the lines
self._init_normal_forms = [compute_normal_form_line_coeffs(self.px, c[0], c[1])
for c in expected_coeffs_list]
# Snapshot of lane boundary types
self._init_types = expected_type_list
self._first_time = False
else:
# Not first time, use snapshot of lane boundaries extracted the first time to compute error
# Pose difference is wrt local frame
pose_diff = self._get_pose_diff(location, orientation)
# Compute expected lane boundary coefficients using the snapshot
expected_coeffs_list = []
for normal_form in self._init_normal_forms:
c0c1 = self._compute_expected_c0c1(normal_form, pose_diff)
expected_coeffs_list.append(c0c1)
# Retrieve lane boundary types from snapshot
expected_type_list = self._init_types
else:
# Not static mode
# Extract ground truth from the Carla server
fbumper_location = get_fbumper_location(
location, orientation, self.px)
self.in_junction, self.into_junction, self.me_format_expected_markings = self.expected_lane_extractor.extract(
fbumper_location, orientation)
# List of expected markings' coefficients
expected_coeffs_list = [expected.get_c0c1_list()
for expected in self.me_format_expected_markings]
# List of expected markings' type
expected_type_list = [expected.type
for expected in self.me_format_expected_markings]
########## Measurement ##########
if self.left_marking:
measured_coeffs_left = np.asarray(
self.left_marking.get_c0c1_list()).reshape(2, -1)
measured_type_left = self.left_marking.type
if self.right_marking:
measured_coeffs_right = np.asarray(
self.right_marking.get_c0c1_list()).reshape(2, -1)
measured_type_right = self.right_marking.type
# Null hypothesis
# Use the measurement itself at every optimization iteration as the null hypothesis.
# This is, of course, just a trick.
# This means the error for null hypothesis is always zeros.
null_expected_c0c1_left = self.left_marking.get_c0c1_list()
null_expected_c0c1_right = self.right_marking.get_c0c1_list()
null_error = np.zeros((2, 1)) # same for both left and right
# Compute innovation matrix for the null hypo
null_noise_cov = self.noise_cov * self.null_std_scale**2
# Compute measurement likelihood weighted by null probability
null_weighted_meas_likelihood = self.prob_null * \
multivariate_normal.pdf(null_error.squeeze(), cov=null_noise_cov)
# In this implementation, scaling down error and jacobian is done to achieve
# the same effect of tuning the information matrix online.
# Here, however, scale down error for null hypo; i.e.
# null_error /= self.null_std_scale
# is not necessary, since its always zero.
# Zero error and jacobian effectively result in zero information matrix as well.
if self.ignore_junction and (self.in_junction or self.into_junction):
self._null_hypo_left = True
self._null_hypo_right = True
elif not expected_coeffs_list:
self._null_hypo_left = True
self._null_hypo_right = True
else:
# Data association
num_expected_markings = len(self.me_format_expected_markings)
asso_table = np.zeros((2, num_expected_markings+2))
# Left lane marking
errors_left = []
asso_probs = []
meas_likelihoods = []
for exp_coeffs, exp_type in zip(expected_coeffs_list, expected_type_list):
# Compute innovation matrix
expected_c0, expected_c1 = exp_coeffs
H = compute_H(self.px, expected_c0, expected_c1)
innov = H @ self.pose_uncert @ H.T + self.noise_cov
# Compute squared mahalanobis distance
error = np.asarray(exp_coeffs).reshape(
2, -1) - measured_coeffs_left
squared_mahala_dist = error.T @ np.linalg.inv(
innov) @ error
# Semantic likelihood
if self.semantic:
# Conditional probability on type
sem_likelihood = self._conditional_prob_type(
exp_type, measured_type_left)
else:
# Truning off semantic association is equivalent to always
# set semantic likelihood to 1.0
sem_likelihood = 1.0
# Gating (geometric and semantic)
# Reject both geometrically and semantically unlikely associations
# Note:
# Since location distribution across lanes is essentially multimodal,
# geometric gating often used when assuming location is unimodal is not
# very reasonable and will inevitablly reject possible associations.
# Here the geometric gate is set very large so we can preserve associations that
# are a bit far from the current mode (the only mode that exists in the optimization)
# but still possible when the multimodal nature is concerned.
# Or, we can simply give up geometric gating and use semantic gating only.
# The large geometric gate is an inelegant remedy after all.
# if squared_mahala_dist <= self.geo_gate and sem_likelihood > self.sem_gate:
if sem_likelihood > self.sem_gate:
errors_left.append(error)
# Measurement likelihood (based on noise cov)
meas_likelihood = multivariate_normal.pdf(
error.reshape(-1), cov=self.noise_cov)
# Geometric likelihood (based on innov)
geo_likelihood = multivariate_normal.pdf(
error.reshape(-1), cov=innov)
meas_likelihoods.append(meas_likelihood)
asso_prob = geo_likelihood * sem_likelihood
asso_probs.append(asso_prob)
else:
errors_left.append(None)
meas_likelihoods.append(0)
asso_probs.append(0)
asso_probs = np.asarray(asso_probs)
meas_likelihoods = np.asarray(meas_likelihoods)
# Compute weights based on total probability theorem
if asso_probs.sum():
weights_left = (1-self.prob_null) * \
(asso_probs/np.sum(asso_probs))
else:
weights_left = np.zeros(asso_probs.shape)
# Weight measurement likelihoods
weighted_meas_likelihood = weights_left*meas_likelihoods
# Add weight and weighted likelihood of null hypothesis
weights_left = np.insert(weights_left, 0, [self.prob_null, 0])
weighted_meas_likelihood = np.insert(
weighted_meas_likelihood, 0, [null_weighted_meas_likelihood, 0])
asso_table[0, :] = weighted_meas_likelihood
# Right marking
errors_right = []
asso_probs = []
meas_likelihoods = []
for exp_coeffs, exp_type in zip(expected_coeffs_list, expected_type_list):
# Compute innovation matrix
expected_c0, expected_c1 = exp_coeffs
H = compute_H(self.px, expected_c0, expected_c1)
innov = H @ self.pose_uncert @ H.T + self.noise_cov
# Compute squared mahalanobis distance
error = np.asarray(exp_coeffs).reshape(
2, -1) - measured_coeffs_right
squared_mahala_dist = error.T @ np.linalg.inv(
innov) @ error
# Semantic likelihood
if self.semantic:
# Conditional probability on type
sem_likelihood = self._conditional_prob_type(
exp_type, measured_type_right)
else:
# Truning off semantic association is equivalent to always
# set semantic likelihood to 1.0
sem_likelihood = 1.0
# Gating (geometric and semantic)
# Reject both geometrically and semantically unlikely associations
# Note:
# Since location distribution across lanes is essentially multimodal,
# geometric gating often used when assuming location is unimodal is not
# very reasonable and will inevitablly reject possible associations.
# Here the geometric gate is set very large so we can preserve associations that
# are a bit far from the current mode (the only mode that exists in the optimization)
# but still possible when the multimodal nature is concerned.
# Or, we can simply give up geometric gating and use semantic gating only.
# The large geometric gate is an inelegant remedy after all.
# if squared_mahala_dist <= self.geo_gate and sem_likelihood > self.sem_gate:
if sem_likelihood > self.sem_gate:
errors_right.append(error)
# Measurement likelihood (based on noise cov)
meas_likelihood = multivariate_normal.pdf(
error.reshape(-1), cov=self.noise_cov)
# Geometric likelihood (based on innov)
geo_likelihood = multivariate_normal.pdf(
error.reshape(-1), cov=innov)
meas_likelihoods.append(meas_likelihood)
asso_prob = geo_likelihood * sem_likelihood
asso_probs.append(asso_prob)
else:
errors_right.append(None)
meas_likelihoods.append(0)
asso_probs.append(0)
asso_probs = np.asarray(asso_probs)
meas_likelihoods = np.asarray(meas_likelihoods)
# Compute weights based on total probability theorem
if asso_probs.sum():
weights_right = (1-self.prob_null) * \
(asso_probs/np.sum(asso_probs))
else:
weights_right = np.zeros(asso_probs.shape)
# Weight measurement likelihoods
weighted_meas_likelihood = weights_right*meas_likelihoods
# Add weight and weighted likelihood of null hypothesis
weights_right = np.insert(weights_right, 0, [0., self.prob_null])
weighted_meas_likelihood = np.insert(
weighted_meas_likelihood, 0, [0., null_weighted_meas_likelihood, ])
asso_table[1, :] = weighted_meas_likelihood
# GNN association
# This is performed at every optimization step, so this factor is essentially
# a max-mixture factor. It's just now the max operation is replaced by the
# linear sum assignment operation.
# Take log so the association result maximizes the product of likelihoods
# of the associations of the both sides
log_asso_table = np.log(asso_table)
_, col_idc = lsa(log_asso_table, maximize=True)
asso_idx_left = col_idc[0]
asso_idx_right = col_idc[1]
# Left assocation
if asso_idx_left == 0:
# Null hypothesis
self._null_hypo_left = True
else:
self._null_hypo_left = False
chosen_error_left = errors_left[asso_idx_left-2]
self.chosen_expected_coeffs_left = expected_coeffs_list[asso_idx_left-2]
self._scale_left = weights_left[asso_idx_left]
# Right assocation
if asso_idx_right == 1:
# Null hypothesis
self._null_hypo_right = True
else:
self._null_hypo_right = False
chosen_error_right = errors_right[asso_idx_right-2]
self.chosen_expected_coeffs_right = expected_coeffs_list[asso_idx_right-2]
self._scale_right = weights_right[asso_idx_right]
if self._null_hypo_left:
chosen_error_left = null_error
self.chosen_expected_coeffs_left = null_expected_c0c1_left
if self._null_hypo_right:
chosen_error_right = null_error
self.chosen_expected_coeffs_right = null_expected_c0c1_right
chosen_error = np.concatenate((chosen_error_left, chosen_error_right))
return chosen_error
def main(folder_name, idx=None):
argparser = argparse.ArgumentParser(
description='Stop Sign on Road Detection using Ground Truth')
argparser.add_argument('config',
type=argparse.FileType('r'),
help='yaml file for carla configuration')
argparser.add_argument(
'sim_detection_config',
type=argparse.FileType('r'),
help='yaml file for simulated detection configuration')
args = argparser.parse_args()
# Load camera calibration parameters
with open('calib_data.pkl', 'rb') as f:
calib_data = pickle.load(f)
K = calib_data['K']
R = calib_data['R']
x0 = calib_data['x0']
# 3-by-4 calibration matrix for visualization
P = K @ R @ np.concatenate((np.eye(3), -x0), axis=1)
# Read configurations from yaml file
with args.config as f:
carla_config = yaml.safe_load(f)
with args.sim_detection_config as f:
sim_detection_config = yaml.safe_load(f)
dist_raxle_to_fbumper = carla_config['ego_veh']['raxle_to_fbumper']
# Load recorded data
path_to_folder = os.path.join('recordings', folder_name)
with open(os.path.join(path_to_folder, 'sensor_data.pkl'), 'rb') as f:
sensor_data = pickle.load(f)
with open(os.path.join(path_to_folder, 'gt_data.pkl'), 'rb') as f:
gt_data = pickle.load(f)
ss_images = sensor_data['semantic_camera']['ss_image']
traffic_signs = gt_data['static']['traffic_sign']
raxle_locations = gt_data['seq']['pose']['raxle_location']
raxle_orientations = gt_data['seq']['pose']['raxle_orientation']
# Prepare visualization
_, ax = plt.subplots(1, 2)
im = ax[0].imshow(np.ones(ss_images[0].shape).astype(np.uint8),
vmin=0,
vmax=12)
rs_stop_in_img = ax[0].plot([], [], 's')[0]
rs_stop_gt = ax[1].plot([], [], 's', label='rs stop gt')[0]
rs_stop_detect = ax[1].plot([], [], 's', ms=3, label='rs stop detction')[0]
ax[1].set_xlim((30, -30))
ax[1].set_ylim((-5, 60))
ax[1].legend()
plt.show(block=False)
rs_stop_detector = RSStopDetectionSimulator(
traffic_signs, sim_detection_config['rs_stop'])
if idx is not None:
raxle_location = raxle_locations[idx]
raxle_orientation = raxle_orientations[idx]
# Get front bumper's pose
fbumper_location = get_fbumper_location(raxle_location,
raxle_orientation,
dist_raxle_to_fbumper)
fbumper_orientation = raxle_orientation
# Get detection
longi_dist = rs_stop_detector.update_rs_stop(fbumper_location,
fbumper_orientation)
detected_rs_stop_gt = rs_stop_detector.detected_rs_stop_gt
ss_image = ss_images[idx]
if detected_rs_stop_gt is not None:
# put it on road surface (some actors are buried underground)
detected_rs_stop_gt[2] = 0
homo_img_coord = P @ np.append(detected_rs_stop_gt, 1)
u = homo_img_coord[0] / homo_img_coord[2]
v = homo_img_coord[1] / homo_img_coord[2]
rs_stop_in_img.set_data(u, v)
rs_stop_gt.set_data(detected_rs_stop_gt[1], detected_rs_stop_gt[0])
rs_stop_detect.set_data(detected_rs_stop_gt[1], longi_dist)
else:
rs_stop_in_img.set_data([], [])
rs_stop_detect.set_data([], [])
im.set_data(ss_image)
print(detected_rs_stop_gt)
else:
for idx, ss_image in enumerate(ss_images):
raxle_location = raxle_locations[idx]
raxle_orientation = raxle_orientations[idx]
# Get front bumper's pose
fbumper_location = get_fbumper_location(raxle_location,
raxle_orientation,
dist_raxle_to_fbumper)
fbumper_orientation = raxle_orientation
# Get detection
longi_dist = rs_stop_detector.update_rs_stop(
fbumper_location, fbumper_orientation)
detected_rs_stop_gt = rs_stop_detector.detected_rs_stop_gt
ss_image = ss_images[idx]
if detected_rs_stop_gt is not None:
# put it on road surface (some actors are buried underground)
detected_rs_stop_gt[2] = 0
homo_img_coord = P @ np.append(detected_rs_stop_gt, 1)
u = homo_img_coord[0] / homo_img_coord[2]
v = homo_img_coord[1] / homo_img_coord[2]
rs_stop_in_img.set_data(u, v)
rs_stop_gt.set_data(detected_rs_stop_gt[1],
detected_rs_stop_gt[0])
rs_stop_detect.set_data(detected_rs_stop_gt[1], longi_dist)
else:
rs_stop_in_img.set_data([], [])
rs_stop_gt.set_data([], [])
rs_stop_detect.set_data([], [])
im.set_data(ss_image)
print(detected_rs_stop_gt)
plt.pause(0.001)
plt.show()