def phdMerge(self):
if self.parameters.phd_parameters["merge_threshold"] <= 0:
return
result_wt_list = []
result_state_list = []
result_covariance_list = []
num_remaining_components = len(self._weights_)
while num_remaining_components:
max_wt_index = self._weights_.argmax()
max_wt_state = self._states_[max_wt_index]
mahalanobis_dist = misctools.mahalanobis(max_wt_state.state, max_wt_state.covariance, self._states_.state())
merge_list_indices = np.where(mahalanobis_dist <= self.parameters.phd_parameters["merge_threshold"])[0]
merge_list_states = self._states_[merge_list_indices]
# .select(merge_list_indices, INPLACE=False)
merged_wt, merged_x, merged_P = misctools.merge_states(
self._weights_[merge_list_indices], merge_list_states.state, merge_list_states.covariance
)
result_wt_list += [merged_wt]
result_state_list += [merged_x]
result_covariance_list += [merged_P]
# phdmisctools.delete_from_list(
self._states_.delete(merge_list_indices)
self._weights_ = np.delete(self._weights_, merge_list_indices)
num_remaining_components = len(self._weights_)
self._states_.set(np.array(result_state_list), np.array(result_covariance_list))
self._weights_ = np.array(result_wt_list)
def merge(self):
self.flags.LOCK.acquire()
try:
if (self.vars.merge_threshold < 0) or (self.weights.shape[0] < 2):
return
merged_wts = []
merged_sts = []
merged_cvs = []
num_remaining_components = self.weights.shape[0]
while num_remaining_components:
max_wt_index = self.weights.argmax()
max_wt_state = self.states[np.newaxis, max_wt_index]
max_wt_cov = self.covs[np.newaxis, max_wt_index]
mahalanobis_fn = misctools.approximate_mahalanobis
mahalanobis_dist = mahalanobis_fn(max_wt_state, max_wt_cov,
self.states)
merge_list_indices = ( np.where(mahalanobis_dist <=
self.vars.merge_threshold)[0] )
retain_idx = ( np.where(mahalanobis_dist >
self.vars.merge_threshold)[0] )
new_wt, new_st, new_cv = misctools.merge_states(
self.weights[merge_list_indices],
self.states[merge_list_indices],
self.covs[merge_list_indices])
merged_wts += [new_wt]
merged_sts += [new_st]
merged_cvs += [new_cv]
# Remove merged states from the list
#retain_idx = misctools.gen_retain_idx(self.weights.shape[0],
# merge_list_indices)
self.weights = self.weights[retain_idx]
self.states = self.states[retain_idx]
self.covs = self.covs[retain_idx]
num_remaining_components = self.weights.shape[0]
self.flags.ESTIMATE_IS_VALID = False
self.set_states(np.array(merged_wts),
np.array(merged_sts), np.array(merged_cvs))
assert self.weights.shape[0] == self.states.shape[0] == self.covs.shape[0], "Lost states!!"
finally:
self.flags.LOCK.release()
def merge_fov(self):
self.flags.LOCK.acquire()
try:
if (self.vars.merge_threshold < 0) or (self.weights.shape[0] < 2):
return
sensor = self.sensors.dummy_camera
detected_idx = sensor.pdf_detection(self.parent_ned, self.parent_rpy,
self.states)
undetected_idx = np.where(detected_idx <= 0.5)[0]
detected_idx = np.where(detected_idx > 0.5)[0]
#undetected_idx = misctools.gen_retain_idx(self.weights.shape[0],
# detected_idx)
if not detected_idx.shape[0]:
return
merged_wts = []
merged_sts = []
merged_cvs = []
# Save samples which are not going to be merged
unmerged_wts = self.weights[undetected_idx]
unmerged_sts = self.states[undetected_idx]
unmerged_cvs = self.covs[undetected_idx]
# Remove unmerged samples from the state
self.weights = self.weights[detected_idx]
self.states = self.states[detected_idx]
self.covs = self.covs[detected_idx]
num_remaining_components = self.weights.shape[0]
while num_remaining_components:
max_wt_index = self.weights.argmax()
max_wt_state = self.states[np.newaxis, max_wt_index]
max_wt_cov = self.covs[np.newaxis, max_wt_index]
mahalanobis_fn = misctools.approximate_mahalanobis
mahalanobis_dist = mahalanobis_fn(max_wt_state, max_wt_cov,
self.states)
merge_list_indices = ( np.where(mahalanobis_dist <=
self.vars.merge_threshold)[0] )
retain_idx = ( np.where(mahalanobis_dist >
self.vars.merge_threshold)[0] )
new_wt, new_st, new_cv = misctools.merge_states(
self.weights[merge_list_indices],
self.states[merge_list_indices],
self.covs[merge_list_indices])
merged_wts += [new_wt]
merged_sts += [new_st]
merged_cvs += [new_cv]
# Remove merged states from the list
#retain_idx = misctools.gen_retain_idx(self.weights.shape[0],
# merge_list_indices)
self.weights = self.weights[retain_idx]
self.states = self.states[retain_idx]
self.covs = self.covs[retain_idx]
num_remaining_components = self.weights.shape[0]
self.flags.ESTIMATE_IS_VALID = False
self.set_states(np.hstack((unmerged_wts, np.array(merged_wts))),
np.vstack((unmerged_sts, np.array(merged_sts))),
np.vstack((unmerged_cvs, np.array(merged_cvs))))
assert self.weights.shape[0] == self.states.shape[0] == self.covs.shape[0], "Lost states!!"
finally:
self.flags.LOCK.release()
