def xyzrgb_segment_kmeans(pc,colors,histograms,hist_bin_size=4):
"""Input:
- pc is a numpy array of points of size N * 3
- colors is an array of colors of size N * 3
- historgrams is a list of uv histograms
Output (labels,M,cost):
- labels: an N*1 vector of integers 0,...,M
- M: the number of histograms
- cost: a number denoting cost of the histogram.
better values are lower.
"""
assert pc.shape[1]==3
xscale = 2.0
yscale = 2.0
zscale = 1.0
hscale = np.amax(np.amax(pc,0)-np.amin(pc,0))
assert colors.shape[1]==3
features = np.zeros((pc.shape[0],3+len(histograms)))
features[:,0] = pc[:,0]*xscale
features[:,1] = pc[:,1]*yscale
features[:,2] = pc[:,2]*zscale
h = []
for i in xrange(pc.shape[0]):
uv = rgb_to_yuv(*colors[i,:])[1:3]
hi = [eval_uv_hist(hj,uv,hist_bin_size)*hscale for hj in histograms]
h.append(hi)
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)
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
# color_idx = np.append(color_idx, np_cloud[:,3]-i, axis = 0)
# print color_idx
# color_idx = color_idx[color_idx[:] < 307200]
# print len(color_idx)
# # mask = np.zeros((307200,1))
# # for i in range(0,len(color_idx)):
# # if color_idx[i] > 0:
# # mask[color_idx[i]] = 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)
