Пример #1
0
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)
Пример #2
0
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)
Пример #3
0
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)