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 '
