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()