def objectMatch1(cloud,histogram_dict): """ Input: Numpy array of cloud with RGB of current object, dictionary of all the object in the shelf Output: ID of specific point cloud """ uv = [color.rgb_to_yuv(*rgb)[1:3] for rgb in cloud[:,4:7]] hist = color.make_uv_hist(uv) scores = dict([ (obj, dtw(svd(hist)[1].reshape(-1, 1),svd(histogram)[1].reshape(-1, 1),dist=lambda t13, t14: norm(svd(hist)[1].reshape(-1, 1) - svd(histogram)[1].reshape(-1, 1), ord=1))) for (obj, histogram) in histogram_dict.items()]) sorted_score = sorted(scores.items(), key=operator.itemgetter(1),reverse = True) obj = sorted_score[0][0] score = sorted_score[0][1] if DEBUG_PERCEPTION: print 'found object ' + str((obj-1)/NUM_HIST_PER_OBJECT+1) + '\nscore is ' + str(score)+ '\ncloud com is ' print com(cloud) print "\n" return (obj-1)/NUM_HIST_PER_OBJECT+1,score
features[:,3:] = np.array(h) naive_labeling = [] for i in xrange(pc.shape[0]): hi,index = max((v,j) for (j,v) in enumerate(h[i])) naive_labeling.append(index) labels,quality = kmeans(features,len(histograms),initial=naive_labeling) return (labels,len(histograms),quality) if __name__ == '__main__': N1 = 3000 N2 = 2000 pc1 = [[random.uniform(0.1,0.2),random.uniform(0.5,0.7),random.uniform(1.3,1.7)] for i in xrange(N1)] pc2 = [[random.uniform(0.13,0.18),random.uniform(0.7,0.8),random.uniform(1.3,1.7)] for i in xrange(N2)] rgb1 = [[random.uniform(0.4,0.8),random.uniform(0.2,0.3),random.uniform(0.0,1.0)] for i in xrange(N1)] rgb2 = [[random.uniform(0.2,0.5),random.uniform(0.1,0.8),random.uniform(0.3,0.6)] for i in xrange(N2)] hist1 = make_uv_hist([rgb_to_yuv(*c)[1:3] for c in rgb1]) hist2 = make_uv_hist([rgb_to_yuv(*c)[1:3] for c in rgb2]) testxyz = pc1 + pc2 testrgb = rgb1 + rgb2 labels,K,cost = xyzrgb_segment_kmeans(np.array(testxyz),np.array(testrgb),[hist1,hist2]) print "Cost:",cost TP,TN,FP,FN = 0,0,0,0 for i in xrange(0,N1): if labels[i] == 0: TP += 1 else: FN += 1 for i in xrange(N1,N1+N2): if labels[i] == 1: TN += 1
# final_cloud = cloud[color_idx.astype(int)] # print final_cloud # print len(final_cloud) np.savez("test", final_cloud[:, 0], final_cloud[:, 1], final_cloud[:, 2]) np.savez("object", final_cloud[:, 0], final_cloud[:, 1], final_cloud[:, 2]) y, u, v = color.rgb_to_yuv(final_cloud[:, 4], final_cloud[:, 5], final_cloud[:, 6]) # print 'y', y # print 'u', u # print 'v', v uv = [color.rgb_to_yuv(*rgb)[1:3] for rgb in final_cloud[:, 4:7]] # print uv # # uv = [u,v] # print uv hist = color.make_uv_hist(uv) # print hist # a = hist.sum() # print a old_hist = np.load("ref2.npz") old_hist = old_hist["arr_0"] # # print old_hist.sum() np.savez(SAVE_LOCATION, hist) # # # print old_hist score = 2 * np.minimum(hist, old_hist).sum() - np.maximum(hist, old_hist).sum() print score # # print len(cloud) # # plane1 = perception.segmentation(cloud) # # np.savez('subfinished', plane1[:,0],plane1[:,1],plane1[:,2]) # # plane = perception.segmentation(np_cloud) # print'finish sub '