def clustering_procedure(buffer):
global num_c, clusters_publisher, frame_id, publish_cluster_labels, cluster_labels_publisher
if len(buffer.x) == 0: #the area is empty!
clustersmsg = ClustersMsg()
clustersmsg.header.stamp = rospy.Time.now()
clustersmsg.header.frame_id = frame_id
clustersmsg.x = []
clustersmsg.y = []
clustersmsg.z = []
#empty array_sizes means that anyone listening to this message won't loop through the data
clustersmsg.array_sizes = []
clustersmsg.num_clusters = []
clusters_publisher.publish(clustersmsg)
else:
scan_time = buffer.scan_time
clear_data = np.zeros((len(buffer.x), 3))
for i in range(0,len(buffer.x)):
clear_data[i] = ([buffer.x[i], buffer.y[i], buffer.z[i]])
Eps, cluster_labels= mt.dbscan(clear_data, num_c)
max_label=int(np.amax(cluster_labels))
arr_sz = []
x_ = []
y_ = []
z_ = []
for k in range(1,max_label+1) :
filter = np.where(cluster_labels==k)
if len(filter[0])>40 :
xk = np.array(buffer.x)[filter]
yk = np.array(buffer.y)[filter]
zk = np.array(buffer.z)[filter]
for i in range(0, len(xk)):
x_.append(xk[i])
y_.append(yk[i])
z_.append(zk[i])
arr_sz.append(len(xk))
clustersmsg = ClustersMsg()
clustersmsg.header.stamp = rospy.Time.now()
clustersmsg.header.frame_id = frame_id
#clustersmsg.clusters = cluster_labels
clustersmsg.x = x_
clustersmsg.y = y_
clustersmsg.z = z_
clustersmsg.array_sizes = arr_sz
clustersmsg.num_clusters = []
clustersmsg.scan_time = scan_time
clusters_publisher.publish(clustersmsg)
if publish_cluster_labels:
clusterlabelsmsg = ClusterLabelsMsg()
clusterlabelsmsg.header = clustersmsg.header
clusterlabelsmsg.cluster_labels = cluster_labels
cluster_labels_publisher.publish(clusterlabelsmsg)
def clustering_procedure(buffer):
global num_c, clusters_publisher, frame_id, publish_cluster_labels, cluster_labels_publisher
if len(buffer.x) == 0: #the area is empty!
clustersmsg = ClustersMsg()
clustersmsg.header.stamp = rospy.Time.now()
clustersmsg.header.frame_id = frame_id
clustersmsg.x = []
clustersmsg.y = []
clustersmsg.z = []
#empty array_sizes means that anyone listening to this message won't loop through the data
clustersmsg.array_sizes = []
clustersmsg.num_clusters = []
clusters_publisher.publish(clustersmsg)
else:
scan_time = buffer.scan_time
clear_data = np.zeros((len(buffer.x), 3))
for i in range(0, len(buffer.x)):
clear_data[i] = ([buffer.x[i], buffer.y[i], buffer.z[i]])
arr_sz = []
x_ = []
y_ = []
z_ = []
num_clusters = []
if use_overlap:
Eps, cluster_labels = mt.dbscan(clear_data, num_c)
max_label = int(np.amax(cluster_labels))
for k in range(1, max_label + 1):
filter = np.where(cluster_labels == k)
if len(filter[0]) > 40:
xk = np.array(buffer.x)[filter]
yk = np.array(buffer.y)[filter]
zk = np.array(buffer.z)[filter]
for i in range(0, len(xk)):
x_.append(xk[i])
y_.append(yk[i])
z_.append(zk[i])
arr_sz.append(len(xk))
else:
prev_clusters, cluster_label = oncl.onlineDBscan(clear_data, num_c)
for cl in prev_clusters:
if len(x_) == 0:
x_.append(cl.getPoints()[:, 0])
y_.append(cl.getPoints()[:, 1])
z_.append(cl.getPoints()[:, 2])
else:
x_[0] = np.append(x_[0], cl.getPoints()[:, 0], axis=0)
y_[0] = np.append(y_[0], cl.getPoints()[:, 1], axis=0)
z_[0] = np.append(z_[0], cl.getPoints()[:, 2], axis=0)
if len(num_clusters) == 0:
num_clusters = np.array(cl.getSizes())
arr_sz = [cl.getNumPts()]
else:
num_clusters = np.append(num_clusters,
cl.getSizes(),
axis=0)
arr_sz.append(cl.getNumPts())
if 0 in arr_sz:
arr_sz = [i for i in arr_sz if i != 0]
x_ = x_[0]
y_ = y_[0]
z_ = z_[0]
clustersmsg = ClustersMsg()
clustersmsg.header.stamp = rospy.Time.now()
clustersmsg.header.frame_id = frame_id
#clustersmsg.clusters = cluster_labels
clustersmsg.x = x_
clustersmsg.y = y_
clustersmsg.z = z_
clustersmsg.array_sizes = arr_sz
clustersmsg.num_clusters = num_clusters
clustersmsg.scan_time = scan_time
clusters_publisher.publish(clustersmsg)
if publish_cluster_labels:
clusterlabelsmsg = ClusterLabelsMsg()
clusterlabelsmsg.header = clustersmsg.header
clusterlabelsmsg.cluster_labels = cluster_labels
cluster_labels_publisher.publish(clusterlabelsmsg)
def overlap_trace(clusters_msg):
global trace_array #the centroid point of each cluster at every scan
global trace_count, max_num_slide_window
global trace_results #the position of the clusters at each scan
global cls_results #the set of points of each cluster at every scan
global traced_clusters, first_trace, max_cls
global frame_id, clusters_publisher
global num_clusters
cls = []
error = 100
min_dist = -1.0
index = 0
list_dist = []
temp_list = []
new_cluster = True
xi = np.array(clusters_msg.x)
yi = np.array(clusters_msg.y)
zi = np.array(clusters_msg.z)
scan_time = clusters_msg.scan_time
array_sizes = np.array(clusters_msg.array_sizes)
prev_index = 0
if(len(array_sizes) > 0):
for i in range(0, len(array_sizes)):
xk = []
yk = []
zk = []
for j in range(prev_index, prev_index+array_sizes[i]-1):
xk.append(xi[j])
yk.append(yi[j])
zk.append(zi[j])
cls.append([np.array(xk), np.array(yk), np.array(zk)])
prev_index = array_sizes[i]-1
if len(trace_array) == 0:
for i in range(0, len(cls)):
trace_array.append(get_centroid(cls[i], False))
trace_results.append([i])
cls_results.append([cls[i]])
else:
#condition where the clusters have been reduced
if trace_count:
num_cl = np.where(num_clusters == 0)
num_clusters = np.delete(num_clusters, num_cl, 0)
traced_clusters2 = [x for x in traced_clusters if x != []]
traced_clusters = list(traced_clusters2)
trace_count = False
if len(cls) > len(trace_array):
first = trace_array
second = cls
new_cluster = True
else:
first = cls
second = trace_array
new_cluster = False
for i in range(0, len(first)):
if new_cluster == False:
coord = get_centroid(first[i], True)
for j in range(0, len(second)):
#eucl_dist for every combination
if new_cluster:
coord = get_centroid(second[j], True)
d = dist.euclidean(coord,first[i])
temp_list.append(d)
else:
d = dist.euclidean(coord,second[j])
temp_list.append(d)
list_dist.append(temp_list)
temp_list = []
min_val = -1.0
row = 0
num = 0
temp_num = 0
col = -1
results = []
temp_list = list(list_dist)
length = len(list_dist)
for i in range(0,len(cls)):
results.append(-1)
while num<length:
for i in range(0, len(temp_list)):
if min_val == -1.0:
min_val = min(temp_list[i])
temp_row = i
else:
if min_val > min(temp_list[i]):
min_val = min(temp_list[i])
temp_row = i
for i in range(0, len(list_dist)):
if min_val in list_dist[i]:
row = i
break
if new_cluster:
results[list_dist[row].index(min_val)] = row
else:
results[row] = list_dist[row].index(min_val)
ind = temp_list[temp_row].index(min_val)
del temp_list[temp_row]
for i in range(0, len(temp_list)):
temp_list[i][ind] = error
num = num + 1
col = -1
row =0
min_val = -1.0
# a cluster disappears
if (len(results) < len(trace_array) and len(traced_clusters) != 0):
rm_list = []
#remove the unnecessary clusters
for j in range(0, len(trace_array)):
if j not in results:
rm_list.append(j)
for i in rm_list:
del traced_clusters[i][:]
num_clusters[i] = 0
trace_count = True
#remove previous and add the new ones
del trace_array[:]
for i in range(0, len(cls)):
trace_array.append(get_centroid(cls[i], False))
trace_results.append(results)
cls_results.append(cls)
#the maximum number of clusters
if max_cls < len(results):
max_cls = len(results)
if len(trace_results) == max_num_slide_window:
if first_trace:
create_trace()
first_trace = False
else:
continue_trace()
final_clusters, index_clusters = getClusterSet()
x_ = []
y_ = []
z_ = []
arr_sz = []
for i in range(0, len(final_clusters)):
sz = len(final_clusters[i][0])
arr_sz.append(sz)
for j in range(0, sz):
x_.append(final_clusters[i][0][j])
y_.append(final_clusters[i][1][j])
z_.append(final_clusters[i][2][j])
cls_msg = ClustersMsg()
cls_msg.header.stamp = rospy.Time.now()
cls_msg.header.frame_id = frame_id
cls_msg.x = x_
cls_msg.y = y_
cls_msg.z = z_
cls_msg.array_sizes = arr_sz
cls_msg.scan_time = scan_time
cls_msg.num_clusters = index_clusters
clusters_publisher.publish(cls_msg)
del trace_results[0]
del cls_results[0]
max_cls = len(results)
else:
del trace_results[:]
del cls_results[:]
del traced_clusters[:]
del trace_array[:]
num_clusters = []
first_trace = True
trace_count = False
max_cls =0
clusters_publisher.publish(clusters_msg)