def generate_intersections(lines):
"""
Runs extract_intersections on all combinations of lines
Writes the resulting intersections to file as well as returning
Args:
lines: the lines from the shapefile
Returns:
inters: intersections - a list of point, dict tuples
the dict contains the newly created ids of the
intersecting segments
"""
inters = []
i = 0
# Total combinations of two road segments
def nCr(n, r):
f = math.factorial
return f(n) / f(r) / f(n - r)
tot = nCr(len(lines), 2)
# Look at all pairs of segments to extract intersections
for segment1, segment2 in itertools.combinations(lines, 2):
track(i, 10000, tot)
if segment1[1].intersects(segment2[1]):
inter = segment1[1].intersection(segment2[1])
inters.extend(
extract_intersections(inter, {
'id_1': segment1[0],
'id_2': segment2[0]
}))
i += 1
return inters
def get_candidates(buffered, buffered_index, lines):
"""
Gets candidate matches: lines that overlap the buffer of
lines from the other map
Args:
buffered - a list of tuples containing the buffer,
the linestring, and the properties for the lines for one map
buffered_index - the rtree index
lines - the lines for the other map
Returns:
a list of dicts containing a line, the properties,
and the candidate overlapping lines
"""
results = []
print "Getting candidate overlapping lines"
# Go through each line from the osm map
for i, line in enumerate(lines):
overlapping = []
util.track(i, 1000, len(lines))
# First, get candidates from new map that overlap the buffer
# from the original map
for idx in buffered_index.intersection(line[0].bounds):
buffer = buffered[idx][0]
# If the new line overlaps the old line
# then check whether it is entirely within the old buffer
if buffer.intersects(line[0]):
# Add the linestring and the features to the overlap list
overlapping.append((buffered[idx][1], buffered[idx][2]))
results.append({
'line': line[0],
'properties': line[1],
'candidates': overlapping,
})
return results
def get_int_mapping(lines, buffered, buffered_index):
"""
Gets the mappings between intersections
Args:
lines - the set of lines in an intersection
buffered - the buffered lines for the intersections in the other map
buffered_index - the rtree index
"""
print "Getting intersection mappings"
line_results = []
# Go through each line from the osm map
for i, line in enumerate(lines):
util.track(i, 1000, len(lines))
line_buffer = line[0].buffer(10)
best_match = {}
best_overlap = 0
for idx in buffered_index.intersection(line_buffer.bounds):
buffer = buffered[idx][0]
# If the new buffered intersection intersects the old one
# figure out how much overlap, and take the best one
if buffer.intersects(line[0]):
total_area = unary_union([line_buffer, buffer]).area
overlap = max(line_buffer.area / total_area,
buffer.area / total_area)
if overlap > best_overlap and overlap > .20:
best_overlap = overlap
best_match = buffered[idx][2]
line_results.append([line[0], line[1], best_match])
total = len(line_results)
percent_matched = 100 - 100 * float(
len([x for x in line_results if not x[2]])) / float(total)
print "Found matches for " + str(percent_matched) + "% of intersections"
return line_results
def process_frame(self, im_gray):
tracked_keypoints, _ = util.track(self.im_prev, im_gray,
self.active_keypoints)
(center, scale_estimate, rotation_estimate,
tracked_keypoints) = self.estimate(tracked_keypoints)
# Detect keypoints, compute descriptors
keypoints_cv = self.detector.detect(im_gray)
keypoints_cv, features = self.descriptor.compute(im_gray, keypoints_cv)
# Create list of active keypoints
active_keypoints = zeros((0, 3))
# Get the best two matches for each feature
matches_all = self.matcher.knnMatch(features, self.features_database,
2)
# Get all matches for selected features
if not any(isnan(center)):
selected_matches_all = self.matcher.knnMatch(
features, self.selected_features, len(self.selected_features))
# For each keypoint and its descriptor
if len(keypoints_cv) > 0:
transformed_springs = scale_estimate * util.rotate(
self.springs, -rotation_estimate)
for i in range(len(keypoints_cv)):
# Retrieve keypoint location
location = np.array(keypoints_cv[i].pt)
# First: Match over whole image
# Compute distances to all descriptors
matches = matches_all[i]
distances = np.array([m.distance for m in matches])
# Convert distances to confidences, do not weight
combined = 1 - distances / self.DESC_LENGTH
classes = self.database_classes
# Get best and second best index
bestInd = matches[0].trainIdx
secondBestInd = matches[1].trainIdx
# Compute distance ratio according to Lowe
ratio = (1 - combined[0]) / (1 - combined[1])
# Extract class of best match
keypoint_class = classes[bestInd]
# If distance ratio is ok and absolute distance is ok and keypoint class is not background
if ratio < self.THR_RATIO and combined[
0] > self.THR_CONF and keypoint_class != 0:
# Add keypoint to active keypoints
new_kpt = append(location, keypoint_class)
active_keypoints = append(active_keypoints,
array([new_kpt]),
axis=0)
# In a second step, try to match difficult keypoints
# If structural constraints are applicable
if not any(isnan(center)):
# Compute distances to initial descriptors
matches = selected_matches_all[i]
distances = np.array([m.distance for m in matches])
# Re-order the distances based on indexing
idxs = np.argsort(np.array([m.trainIdx for m in matches]))
distances = distances[idxs]
# Convert distances to confidences
confidences = 1 - distances / self.DESC_LENGTH
# Compute the keypoint location relative to the object center
relative_location = location - center
# Compute the distances to all springs
displacements = util.L2norm(transformed_springs -
relative_location)
# For each spring, calculate weight
weight = displacements < self.THR_OUTLIER # Could be smooth function
combined = weight * confidences
classes = self.selected_classes
# Sort in descending order
sorted_conf = argsort(combined)[::-1] # reverse
# Get best and second best index
bestInd = sorted_conf[0]
secondBestInd = sorted_conf[1]
# Compute distance ratio according to Lowe
ratio = (1 - combined[bestInd]) / (1 -
combined[secondBestInd])
# Extract class of best match
keypoint_class = classes[bestInd]
# If distance ratio is ok and absolute distance is ok and keypoint class is not background
if ratio < self.THR_RATIO and combined[
bestInd] > self.THR_CONF and keypoint_class != 0:
# Add keypoint to active keypoints
new_kpt = append(location, keypoint_class)
# Check whether same class already exists
if active_keypoints.size > 0:
same_class = np.nonzero(
active_keypoints[:, 2] == keypoint_class)
active_keypoints = np.delete(active_keypoints,
same_class,
axis=0)
active_keypoints = append(active_keypoints,
array([new_kpt]),
axis=0)
# If some keypoints have been tracked
if tracked_keypoints.size > 0:
# Extract the keypoint classes
tracked_classes = tracked_keypoints[:, 2]
# If there already are some active keypoints
if active_keypoints.size > 0:
# Add all tracked keypoints that have not been matched
associated_classes = active_keypoints[:, 2]
missing = ~np.in1d(tracked_classes, associated_classes)
active_keypoints = append(active_keypoints,
tracked_keypoints[missing, :],
axis=0)
# Else use all tracked keypoints
else:
active_keypoints = tracked_keypoints
# Update object state estimate
_ = active_keypoints
self.center = center
self.scale_estimate = scale_estimate
self.rotation_estimate = rotation_estimate
self.tracked_keypoints = tracked_keypoints
self.active_keypoints = active_keypoints
self.im_prev = im_gray
self.keypoints_cv = keypoints_cv
_ = time.time()
self.tl = (nan, nan)
self.tr = (nan, nan)
self.br = (nan, nan)
self.bl = (nan, nan)
self.bb = array([nan, nan, nan, nan])
self.has_result = False
if not any(
isnan(self.center)
) and self.active_keypoints.shape[0] > self.num_initial_keypoints / 10:
self.has_result = True
tl = util.array_to_int_tuple(center + scale_estimate * util.rotate(
self.center_to_tl[None, :], rotation_estimate).squeeze())
tr = util.array_to_int_tuple(center + scale_estimate * util.rotate(
self.center_to_tr[None, :], rotation_estimate).squeeze())
br = util.array_to_int_tuple(center + scale_estimate * util.rotate(
self.center_to_br[None, :], rotation_estimate).squeeze())
bl = util.array_to_int_tuple(center + scale_estimate * util.rotate(
self.center_to_bl[None, :], rotation_estimate).squeeze())
min_x = min((tl[0], tr[0], br[0], bl[0]))
min_y = min((tl[1], tr[1], br[1], bl[1]))
max_x = max((tl[0], tr[0], br[0], bl[0]))
max_y = max((tl[1], tr[1], br[1], bl[1]))
self.tl = tl
self.tr = tr
self.bl = bl
self.br = br
self.bb = np.array([min_x, min_y, max_x - min_x, max_y - min_y])
def process_frame(self, im_gray): tracked_keypoints, status = util.track(self.im_prev, im_gray, self.active_keypoints) (center, scale_estimate, rotation_estimate, tracked_keypoints) = self.estimate(tracked_keypoints) #Detect keypoints, compute descriptors keypoints_cv = self.detector.detect(im_gray) keypoints_cv, features = self.descriptor.compute(im_gray, keypoints_cv) #Create list of active keypoints active_keypoints = zeros((0,3)) #For each keypoint and its descriptor if len(keypoints_cv) > 0: for (keypoint_cv, feature) in zip(keypoints_cv, features): #Retrieve keypoint location location = np.array(keypoint_cv.pt) #First: Match over whole image #Compute distances to all descriptors matches = self.matcher.match(self.features_database, feature[None,:]) distances = np.array([m.distance for m in matches]) #Convert distances to confidences, do not weight combined = 1 - distances / self.DESC_LENGTH classes = self.database_classes #Sort in descending order sorted_conf = argsort(combined)[::-1] #reverse #Get best and second best index bestInd = sorted_conf[0] secondBestInd = sorted_conf[1] #Compute distance ratio according to Lowe ratio = (1-combined[bestInd]) / (1-combined[secondBestInd]) #Extract class of best match keypoint_class = classes[bestInd] #If distance ratio is ok and absolute distance is ok and keypoint class is not background if ratio < self.THR_RATIO and combined[bestInd] > self.THR_CONF and keypoint_class != 0: #Add keypoint to active keypoints new_kpt = append(location, keypoint_class) active_keypoints = append(active_keypoints, array([new_kpt]), axis=0) #In a second step, try to match difficult keypoints #If structural constraints are applicable if not any(isnan(center)): #Compute distances to initial descriptors matches = self.matcher.match(self.selected_features, feature[None,:]) distances = np.array([m.distance for m in matches]) #Convert distances to confidences confidences = 1 - distances / self.DESC_LENGTH #Compute the keypoint location relative to the object center relative_location = location - center #Compute the distances to all springs displacements = util.L2norm(scale_estimate * util.rotate(self.springs, -rotation_estimate) - relative_location) #For each spring, calculate weight weight = displacements < self.THR_OUTLIER #Could be smooth function combined = weight * confidences classes = self.selected_classes #Sort in descending order sorted_conf = argsort(combined)[::-1] #reverse #Get best and second best index bestInd = sorted_conf[0] secondBestInd = sorted_conf[1] #Compute distance ratio according to Lowe ratio = (1-combined[bestInd]) / (1-combined[secondBestInd]) #Extract class of best match keypoint_class = classes[bestInd] #If distance ratio is ok and absolute distance is ok and keypoint class is not background if ratio < self.THR_RATIO and combined[bestInd] > self.THR_CONF and keypoint_class != 0: #Add keypoint to active keypoints new_kpt = append(location, keypoint_class) #Check whether same class already exists if active_keypoints.size > 0: same_class = np.nonzero(active_keypoints[:,2] == keypoint_class) active_keypoints = np.delete(active_keypoints, same_class, axis=0) active_keypoints = append(active_keypoints, array([new_kpt]), axis=0) #If some keypoints have been tracked if tracked_keypoints.size > 0: #Extract the keypoint classes tracked_classes = tracked_keypoints[:,2] #If there already are some active keypoints if active_keypoints.size > 0: #Add all tracked keypoints that have not been matched associated_classes = active_keypoints[:,2] missing = ~np.in1d(tracked_classes, associated_classes) active_keypoints = append(active_keypoints, tracked_keypoints[missing,:], axis=0) #Else use all tracked keypoints else: active_keypoints = tracked_keypoints #Update object state estimate active_keypoints_before = active_keypoints self.center = center self.scale_estimate = scale_estimate self.rotation_estimate = rotation_estimate self.tracked_keypoints = tracked_keypoints self.active_keypoints = active_keypoints self.im_prev = im_gray self.keypoints_cv = keypoints_cv toc = time.time() self.tl = (nan,nan) self.tr = (nan,nan) self.br = (nan,nan) self.bl = (nan,nan) self.bb = array([nan,nan,nan,nan]) self.has_result = False if not any(isnan(self.center)) and self.active_keypoints.shape[0] > self.num_initial_keypoints / 10: self.has_result = True tl = util.array_to_int_tuple(center + scale_estimate*util.rotate(self.center_to_tl[None,:], rotation_estimate).squeeze()) tr = util.array_to_int_tuple(center + scale_estimate*util.rotate(self.center_to_tr[None,:], rotation_estimate).squeeze()) br = util.array_to_int_tuple(center + scale_estimate*util.rotate(self.center_to_br[None,:], rotation_estimate).squeeze()) bl = util.array_to_int_tuple(center + scale_estimate*util.rotate(self.center_to_bl[None,:], rotation_estimate).squeeze()) min_x = min((tl[0],tr[0],br[0],bl[0])) min_y = min((tl[1],tr[1],br[1],bl[1])) max_x = max((tl[0],tr[0],br[0],bl[0])) max_y = max((tl[1],tr[1],br[1],bl[1])) self.tl = tl self.tr = tr self.bl = bl self.br = br self.bb = np.array([min_x, min_y, max_x - min_x, max_y - min_y])
def track_update(self, t, dt, z_k, size_k):
# here the format of z_k is
# t: time of receiving the observations
# dt: time between this time and last time of receiving observations
# z_k: a list of position data, expressed as [[x1, y1], [x2, y2], ...]
# FoV analysis, to classifiy obs data into 2 parts, inside FoV or outside
z_k, outside_z_k, size_k = self.obs_fov(z_k, size_k)
# adaptive motion model here
self.F = np.matrix([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0],
[0, 0, 0, 1]])
next_index_list = []
ellips_inputs_k = []
bb_output_k = []
# 1. extract all obs inside elliposidual gates of initialized and confirmed tracks
obs_matrix = np.zeros((len(self.track_list_next_index), len(z_k)))
for i in range(len(self.track_list_next_index)):
k = self.track_list_next_index[i]
kf = self.track_list[k].kf
S = np.dot(np.dot(kf.H, kf.P_k_k_min), kf.H.T) + kf.R
pred_z = np.dot(kf.H, kf.x_k_k_min)
#### usage for plotting ellipse
self.track_list[k].S = S
self.track_list[k].pred_z = pred_z
for j in range(len(z_k)):
z = z_k[j]
z_til = pred_z - np.matrix(z).T
obs_matrix[i][j] = self.elliposidualGating(z_til, S)
# create a mitrix to check the relationship between obs and measurements
obs_sum = obs_matrix.sum(axis=0)
# matrix operation, exclude all observations whose sum is 0, which means its not in any gate
index = np.where(obs_sum > 0)[0]
obs_outside_index = np.where(obs_sum == 0)[0]
# obs matrix inside the gate -- obs_matrix_gate
obs_matrix_gate = obs_matrix[:, index]
# 2. deal with observations inside all gates
# i. initialize each observation, find out how many gates each observation is in
obs_class = []
# now in obs_class, each element is a class with its .track includes all track numbers from 0 - m_k
# then we need to analyse the gate. First for jth obs z_j in its ith gate we need to calculate
# g_ij = N(z_j - z_ij|0, S_ij) here z_ij and S_ij is the updated KF given observation z_j
for i in range(len(z_k)):
if i in index:
try:
a_mea = measurement(z_k[i], i)
except:
print(i, z_k)
for j in np.where(obs_matrix[:, i] != 0)[0]:
# track is the track id for obs i, in obs_matrix the column value is not 0
track_id = self.track_list_next_index[j]
a_mea.inside_track(track_id)
temp_kf = copy.deepcopy(self.track_list[track_id].kf)
temp_kf.update(z_k[i])
var = multivariate_normal(mean=np.squeeze(
temp_kf.z_bar.reshape(2).tolist()[0]),
cov=temp_kf.S_k)
a_mea.g_ij.append(var.pdf(z_k[i]))
obs_class.append(a_mea)
else:
obs_class.append([])
# ii. for each gate/track, analysis if there exists observations joint different tracks, find out the relation between different tracks
track_class = []
for i in range(len(self.track_list_next_index)):
k = self.track_list_next_index[i]
a_track = self.track_list[k] # pointer to track_list class
a_track.get_measurement(np.where(obs_matrix[i] != 0)[0])
a_track.measurement.append(-1)
track_class.append(a_track)
for i in range(len(track_class)):
# Case 1. if there is no obs inside track gate,
# make Kalman's update x_k_k= x_k_k_min, P_k_k = P_k_k_min
if track_class[i].deleted:
continue
if len(track_class[i].measurement) == 1:
# there is only false alarm in observation, then only do prediction
kf = track_class[i].kf
# apparently this is wrong, we need to make observation in Kalman update
# x_k_k_min, then update the covariance.
kf.x_k_k = kf.x_k_k_min
kf.P_k_k = kf.P_k_k_min
# kf.update([kf.x_k_k_min[0, 0], kf.x_k_k_min[1, 0]])
track_class[i].update(t, kf, False)
# only keep the confirmed ones
if track_class[i].confirmed:
# ellips_inputs_k.append([track_class[i].S, track_class[i].pred_z])
ellips_inputs_k.append(track_class[i])
# ellips_inputs_k.append([track_class[i].id, track_class[i].kf.x_k_k, track_class[i].kf.P_k_k])
bb_output_k.append(track_class[i].bb_box_size)
if not (track_class[i].deleted or track_class[i].abandoned):
next_index_list.append(self.track_list_next_index[i])
continue
# Case 2. there is obs inside the track gate
# calculate the beta for ith track
# need the number of measurements inside the gate
beta = self.cal_beta(len(track_class[i].measurement) - 1)
table_key = [self.track_list_next_index[i]]
# begin find all observations inside this gate that is related to other gates (joint)
for obs_id in track_class[i].measurement:
if obs_id != -1:
obs = obs_class[obs_id]
table_key += obs.track
table_key = list(set(table_key))
# for each track, figure out how many observations inside the track
# inverse the table
table_key_inv = []
for j in table_key:
table_key_inv.append(self.track_list_next_index.index(j))
table_key_matrix = obs_matrix[table_key_inv]
######################
# if (table_key_matrix == table_key_matrix[0]).all():
# # there are overlaps of tracks, we only remain the one has oldest history
if (table_key_matrix == table_key_matrix[0]
).all() and len(table_key_matrix) > 2:
# there are overlaps of tracks, we only remain the one has oldest history
seed = min(table_key)
for key in table_key:
if key != seed:
self.track_list[key].deletion(t)
##### !!!!!!!!
# in order to avoid multiple tracks tracking same target, we need to analysis the
# obs_matrix
obs_num_tracks = obs_matrix.sum(axis=1)[table_key_inv]
# number of joint tracks
N_T = len(table_key)
# number of observations total
total_obs = []
for track_id in table_key:
a_track = self.track_list[track_id]
total_obs += a_track.measurement
total_obs = list(set(total_obs))
N_o = len(total_obs) - 1
common_factor = common_fact(beta, self.P_D, N_T, N_o)
# iii. after merged all related tracks, we generat a hypothesis matrix/table based on the table
# generated by the table_key
obs_num_tracks_ = obs_num_tracks + 1
total_row = int(obs_num_tracks_.prod())
# create title for the table
hyp_matrix = {}
for a_key in table_key:
hyp_matrix[str(a_key)] = []
hyp_matrix["p"] = []
for row_num in range(total_row):
key_num = len(table_key)
col_num = 0
# build one row of hypothesis
while key_num > 0:
if col_num == len(table_key) - 1:
obs_id = int(row_num)
product = 1
else:
product = obs_num_tracks_[(col_num + 1):].prod()
obs_id = int(row_num // product)
value = self.track_list[
table_key[col_num]].measurement[obs_id]
key = str(table_key[col_num])
hyp_matrix[key].append(value)
row_num = row_num % product
col_num += 1
key_num -= 1
# now we want to calculate the probability of this row's hypothesis
hyp_list = []
prob = common_factor
for key in hyp_matrix.keys():
if key != 'p':
hyp_list.append(hyp_matrix[key][-1])
# print('hyp_list, ', hyp_list)
# calculate the prob of this hypothesis
if checkIfDuplicates(hyp_list):
# this is not one vaild hypothesis.
prob = 0
else:
# this is the valid hypothesis, we should calculate the prob
for key in hyp_matrix.keys():
if key != 'p':
track_id = int(key)
obs_id = hyp_matrix[key][-1]
# print('obs id ', obs_id)
if obs_id == -1:
prob *= (1 - self.P_D) * beta
else:
# print(obs_class[obs_id].table, print(obs_class[obs_id].id))
index = obs_class[obs_id].track.index(track_id)
prob *= self.P_D * obs_class[obs_id].g_ij[index]
hyp_matrix['p'].append(prob)
# iv. Then gather the prob in this track, and update the kF
obs_in_i_track = track_class[i].measurement
obs_in_i_track_prob = []
hyp_in_i_track = np.array(hyp_matrix[str(
self.track_list_next_index[i])])
hyp_in_i_track_prob = np.array(hyp_matrix['p'])
for obs in obs_in_i_track:
index_ = np.where(hyp_in_i_track == obs)
w_ij_list = hyp_in_i_track_prob[index_]
obs_in_i_track_prob.append(w_ij_list.sum())
# then normalize all w_ij s
obs_in_i_track_prob_norm = normalize(obs_in_i_track_prob)
# well, we then just need to update the KF of ith track
kf = track_class[i].kf
x_k = []
P_k = []
bb_k = []
for obs in obs_in_i_track:
if obs == -1:
x_k.append(kf.x_k_k)
P_k.append(kf.P_k_k)
else:
z = np.array(z_k[obs]).T
# update the kf
temp_kf = copy.deepcopy(kf)
temp_kf.update(z)
x_k.append(temp_kf.x_k_k)
P_k.append(temp_kf.P_k_k)
bb_k.append(size_k[obs])
x_k_pda = 0 * temp_kf.x_k_k
P_k_pda = 0 * temp_kf.P_k_k
for j in range(len(obs_in_i_track_prob_norm)):
x_k_pda += obs_in_i_track_prob_norm[j] * x_k[j]
for j in range(len(obs_in_i_track_prob_norm)):
P_k_pda += obs_in_i_track_prob_norm[j] * (
P_k[j] + np.dot(x_k_pda - x_k[j], x_k_pda.T - x_k[j].T))
# update this to kf
kf.x_k_k = x_k_pda
kf.P_k_k = P_k_pda
if np.linalg.det(kf.P_k_k[0:2, 0:2]) > self.common_P:
# print("track get deleted here~~")
track_class[i].update(t, kf, False)
track_class[i].deleted = True
continue
track_class[i].update(t, kf, True)
# only keep the confirmed ones
if track_class[i].confirmed:
# ellips_inputs_k.append([track_class[i].id, track_class[i].kf.x_k_k, track_class[i].kf.P_k_k])
ellips_inputs_k.append(track_class[i])
# pick the bounding box which is the cloest to the associated x_k_k
bb_output_k.append(bb_k[self.find_cloest(x_k_pda, x_k[1:])])
track_class[i].bb_box_size = bb_output_k[-1]
# save the activated ones for next recursion
if not (track_class[i].deleted or track_class[i].abandoned):
next_index_list.append(self.track_list_next_index[i])
# 3. deal with observations outside all gates
# now initialize all observations outside of the gate
for i in obs_outside_index:
z = z_k[i] + [0, 0]
x0 = np.matrix(z).T
kf = EKFcontrol(self.F, self.H, x0, self.P, self.Q, self.R)
id_ = len(self.track_list)
new_track = track(t, id_, kf, self.DeletionThreshold,
self.ConfirmationThreshold)
new_track.kf.predict()
new_track.bb_box_size = size_k[i] # bb size initialization
self.track_list.append(new_track)
next_index_list.append(id_)
# swap the track list to the new list.
self.track_list_next_index = next_index_list
return ellips_inputs_k, bb_output_k
def process_frame(self, im_gray):
tracked_keypoints, status, flow = util.track(self.im_prev, im_gray,
self.active_keypoints,
1.0)
(center, scale_estimate, rotation_estimate, tracked_keypoints,
flow) = self.estimate(tracked_keypoints, flow)
# creat mask
mask = np.zeros_like(im_gray, dtype=np.uint8)
if tracked_keypoints.size > 0 and not any(isnan(center)):
tl = util.array_to_float_tuple(
center + scale_estimate * util.rotate(
self.center_to_tl[None, :], rotation_estimate).squeeze())
tr = util.array_to_float_tuple(
center + scale_estimate * util.rotate(
self.center_to_tr[None, :], rotation_estimate).squeeze())
br = util.array_to_float_tuple(
center + scale_estimate * util.rotate(
self.center_to_br[None, :], rotation_estimate).squeeze())
bl = util.array_to_float_tuple(
center + scale_estimate * util.rotate(
self.center_to_bl[None, :], rotation_estimate).squeeze())
cv.fillPoly(
mask,
np.array([tl, tr, br, bl], dtype=np.int32).reshape(-1, 4, 2),
(255, 255, 255))
mask = cv.dilate(mask, np.ones((19, 19), np.uint8), iterations=2)
# Detect keypoints, compute descriptors
keypoints_cv = self.detector.detect(im_gray, mask=mask)
else:
# Detect keypoints, compute descriptors
keypoints_cv = self.detector.detect(im_gray)
keypoints_cv, features = self.descriptor.compute(im_gray, keypoints_cv)
# # double check for transformation estimation
# mask = np.zeros_like(self.im_prev, dtype=np.uint8)
# cv.fillPoly(mask, np.array([self.tl, self.tr, self.br, self.bl], dtype=np.int32).reshape(-1, 4, 2), (255, 255, 255))
# keypoints_cv_prev = self.detector.detect(self.im_prev, mask=mask)
# keypoints_cv_prev, features_prev = self.descriptor.compute(self.im_prev, keypoints_cv_prev)
# H, status = self.estimate_homography(keypoints_cv_prev, features_prev, keypoints_cv, features)
# if H is not None:
# tl = util.array_to_float_tuple(cv.perspectiveTransform(np.float32(self.tl).reshape(1, 1, -1), H).reshape(-1))
# tr = util.array_to_float_tuple(cv.perspectiveTransform(np.float32(self.tr).reshape(1, 1, -1), H).reshape(-1))
# br = util.array_to_float_tuple(cv.perspectiveTransform(np.float32(self.br).reshape(1, 1, -1), H).reshape(-1))
# bl = util.array_to_float_tuple(cv.perspectiveTransform(np.float32(self.bl).reshape(1, 1, -1), H).reshape(-1))
# Create list of active keypoints
active_keypoints = zeros((0, 3))
# Get all matches for selected features
if not any(isnan(center)):
selected_matches_all = self.matcher.knnMatch(
features, self.selected_features, len(self.selected_features))
# import pdb
# pdb.set_trace()
# For each keypoint and its descriptor
matched_ratio = 0.0
if len(keypoints_cv) > 0:
transformed_springs = scale_estimate * util.rotate(
self.springs, -rotation_estimate)
for i in range(len(keypoints_cv)):
# Retrieve keypoint location
location = np.array(keypoints_cv[i].pt)
# If structural constraints are applicable
if not any(isnan(center)):
# Compute distances to initial descriptors
matches = selected_matches_all[i]
distances = np.array([m.distance for m in matches])
# Re-order the distances based on indexing
idxs = np.argsort(np.array([m.trainIdx for m in matches]))
distances = distances[idxs]
# Convert distances to confidences
confidences = 1 - distances / self.DESC_LENGTH
# Compute the keypoint location relative to the object center
relative_location = location - center
# Compute the distances to all springs
displacements = util.L2norm(transformed_springs -
relative_location)
# For each spring, calculate weight
weight = displacements < self.THR_OUTLIER # Could be smooth function
combined = weight * confidences
classes = self.selected_classes
# Sort in descending order
sorted_conf = argsort(combined)[::-1] # reverse
# Get best and second best index
bestInd = sorted_conf[0]
secondBestInd = sorted_conf[1]
# Compute distance ratio according to Lowe
ratio = (1 - combined[bestInd] +
1e-8) / (1 - combined[secondBestInd] + 1e-8)
# Extract class of best match
keypoint_class = classes[bestInd]
# If distance ratio is ok and absolute distance is ok and keypoint class is not background
if ratio < self.THR_RATIO and combined[
bestInd] > self.THR_CONF and keypoint_class != 0:
matched_ratio += 1
# Add keypoint to active keypoints
new_kpt = append(location, keypoint_class)
# Check whether same class already exists
if active_keypoints.size > 0:
same_class = np.nonzero(
active_keypoints[:, 2] == keypoint_class)
active_keypoints = np.delete(active_keypoints,
same_class,
axis=0)
active_keypoints = append(active_keypoints,
array([new_kpt]),
axis=0)
# If some keypoints have been tracked
if tracked_keypoints.size > 0:
# Extract the keypoint classes
tracked_classes = tracked_keypoints[:, 2]
# If there already are some active keypoints
if active_keypoints.size > 0:
# Add all tracked keypoints that have not been matched
associated_classes = active_keypoints[:, 2]
missing = ~np.in1d(tracked_classes, associated_classes)
active_keypoints = append(active_keypoints,
tracked_keypoints[missing, :],
axis=0)
# Else use all tracked keypoints
else:
active_keypoints = tracked_keypoints
# Update object state estimate
_ = active_keypoints
self.center = center
self.scale_estimate = scale_estimate
self.rotation_estimate = rotation_estimate
self.tracked_keypoints = tracked_keypoints
self.active_keypoints = active_keypoints
self.keypoints_cv = keypoints_cv
_ = time.time()
self.bb = array([nan, nan, nan, nan])
self.has_result = False
if not any(
isnan(self.center)
) and self.active_keypoints.shape[0] > self.num_initial_keypoints / 10:
self.has_result = True
min_x = min((tl[0], tr[0], br[0], bl[0]))
min_y = min((tl[1], tr[1], br[1], bl[1]))
max_x = max((tl[0], tr[0], br[0], bl[0]))
max_y = max((tl[1], tr[1], br[1], bl[1]))
self.tl = tl
self.tr = tr
self.bl = bl
self.br = br
self.bb = np.array([min_x, min_y, max_x - min_x, max_y - min_y])
self.matched_ratio = matched_ratio / len(self.selected_features)
self.im_prev = im_gray
def get_mapping(lines, features):
"""
Attempts to map one or more segments of the second map to the first map
Args:
lines - a list of dicts containing the line from the first map,
the properties, the candidate overlapping lines from the new map,
and nearby segments on the original map because we may want to
combine two for the purposes of mapping
"""
print len(lines)
result_counts = [0, 0]
buff = 5
# keep track of which new segments matched at which size buffer
buff_match = {}
new_id = 0
while buff <= 20:
print "Looking at buffer " + str(buff)
for i in range(len(lines)):
util.track(i, 1000, len(lines))
if 'matches' in lines[i].keys():
continue
matched_candidates = []
for j, candidate in enumerate(lines[i]['candidates']):
if 'id' not in lines[i]['candidates'][j][1].keys():
lines[i]['candidates'][j][1]['id'] = new_id
new_id += 1
match = True
for coord in candidate[0].coords:
if not Point(coord).within(lines[i]['line'].buffer(buff)):
match = False
if match:
matched_candidates.append((candidate, buff))
if lines[i]['candidates'][j][1]['id'] \
not in buff_match.keys():
buff_match[lines[i]['candidates'][j][1]['id']] = buff
if matched_candidates:
lines[i]['matches'] = matched_candidates
buff *= 2
# Now go through the lines that still aren't matched
# this time, see if they are a subset of any of their candidates
for i in range(len(lines)):
matched_candidates = []
if 'matches' in lines[i].keys():
continue
for j, candidate in enumerate(lines[i]['candidates']):
if 'id' not in lines[i]['candidates'][j][1].keys():
lines[i]['candidates'][j][1]['id'] = new_id
new_id += 1
match = True
for coord in lines[i]['line'].coords:
if not Point(coord).within(candidate[0].buffer(20)):
match = False
if match:
matched_candidates.append((candidate, 20))
if lines[i]['candidates'][j][1]['id'] not in buff_match.keys():
buff_match[lines[i]['candidates'][j][1]['id']] = buff
if matched_candidates:
lines[i]['matches'] = matched_candidates
# Remove matches that matched better on a different segment
# But only if there's a match for that segment already
for i in range(len(lines)):
if 'matches' in lines[i].keys():
matches = lines[i]['matches']
new_matches = []
for (m, buff) in matches:
if buff_match[m[1]['id']] == buff:
new_matches.append(m)
if new_matches:
lines[i]['matches'] = new_matches
else:
# Remove buffer info
lines[i]['matches'] = [m[0] for m in lines[i]['matches']]
orig = []
matched = []
for i, line in enumerate(lines):
if 'matches' in line.keys() and line['matches']:
result_counts[0] += 1
orig.append((line['line'], line['properties']))
# Every single match for this line
add_match_features(line, features)
# Add each matching line
# Only used for debugging purposes, take out eventually
for m in line['matches']:
matched.append((m[0], m[1]))
else:
for f in features:
line['properties'][f] = 0
result_counts[1] += 1
percent_matched = float(
result_counts[0]) / (float(result_counts[0] + result_counts[1])) * 100
print 'Found matches for ' + str(percent_matched) + '% of segments'
print result_counts