def run(self):
# Read data from new file
while len(self.filenames) > 0:
if len(self.filenames) > 0 and self.xyz_stack is None:
data_file = self.filenames.pop()
print 'New video:', data_file
self.deviceID = data_file
self.xyz_stack, self.skeleton_stack = read_EVAL_depth_ims(
self.base_dir + data_file)
framecount = self.xyz_stack.shape[-1]
for i in xrange(framecount):
self.depthIm = self.xyz_stack[:, :, 2, i].copy()
self.posIm = self.xyz_stack[:, :, :, i]
self.mask = (np.abs(self.depthIm - self.background_model) >
500) * (self.depthIm != 0)
self.mask = extract_people(self.depthIm, self.mask, 1000,
500)[0]
if self.enable_bg_subtraction:
self.depthIm *= self.mask
self.posIm *= self.mask[:, :, None]
self.users = [] #[msr_to_kinect_skel(self.skel_stack[i])]
yield
def run(self): # Read data from new file while len(self.filenames) > 0: if len(self.filenames) > 0 and self.xyz_stack is None: data_file = self.filenames.pop() print 'New video:', data_file self.deviceID = data_file self.xyz_stack, self.skeleton_stack = read_EVAL_depth_ims(self.base_dir+data_file) framecount = self.xyz_stack.shape[-1] for i in xrange(framecount): self.depthIm = self.xyz_stack[:,:,2,i].copy() self.posIm = self.xyz_stack[:,:,:,i] self.mask = (np.abs(self.depthIm - self.background_model) > 500) * (self.depthIm != 0) self.mask = extract_people(self.depthIm, self.mask, 1000, 500)[0] if self.enable_bg_subtraction: self.depthIm *= self.mask self.posIm *= self.mask[:,:,None] self.users = []#[msr_to_kinect_skel(self.skel_stack[i])] yield
def visualize_data(name):
'''
'''
depth_file = name + "depth.bin"
color_file = name + "rgb.avi"
skeleton_file = name + "skeleton.txt"
''' Read data from each video/sequence '''
try:
depthIms, maskIms = read_MSR_depth_ims(depth_file)
colorIms = read_MSR_color_ims(color_file)
skels_world, skels_im = read_MSR_skeletons(skeleton_file)
except:
print "Error reading data"
return -1
framecount = np.minimum(depthIms.shape[0], colorIms.shape[0])
''' View all data'''
for frame in xrange(framecount):
depth = depthIms[frame]
mask = maskIms[frame] > 0
color = colorIms[frame]
# color = np.dstack([color[:,:,2],color[:,:,1],color[:,:,0]])
''' Skeleton in world (w) and image (i) coordinates '''
skel_w = skels_world[frame]
skel_i = world2depth(skel_w, rez=[240,320])
''' Calculate hogs '''
grayIm = (rgb2gray(color) * 255).astype(np.uint8)
person_mask, bounding_boxes, labels = extract_people(grayIm, mask>0)
rez = grayIm[bounding_boxes[0]].shape
''' Plot skeletons on color image'''
# color = color*(mask[:,:,None]==0)
# from pylab import *
# embed()
# blurIm = (grayIm*mask).astype(np.float)
# blurIm = color*mask[:,:,None]
# blurIm = cv2.GaussianBlur(color, (15,15), 10)
# blurIm = cv2.GaussianBlur(mask[:,:,None]*color, (9,9), 5)
# blurIm = cv2.blur(mask[:,:,None]*color, (5,5))
# cv2.imshow("b", blurIm/float(blurIm.max()))
''' Visualization '''
# color = display_skeletons(color, skel_i, color=(0,200,0))
# blurIm = display_skeletons(blurIm, skel_i, color=(0,200,0))
cv2.imshow("Depth", depth/float(depth.max()))
cv2.imshow("RGB", color)
# cv2.imshow("RGB_blur", color*(mask[:,:,None]==0) + blurIm*(mask[:,:,None]>0))
# cv2.imshow("RGB masked", color*(mask[:,:,None]>0))
ret = cv2.waitKey(10)
if ret >= 0:
break
def visualize_data(name):
'''
'''
depth_file = name + "depth.bin"
color_file = name + "rgb.avi"
skeleton_file = name + "skeleton.txt"
''' Read data from each video/sequence '''
try:
depthIms, maskIms = read_MSR_depth_ims(depth_file)
colorIms = read_MSR_color_ims(color_file)
skels_world, skels_im = read_MSR_skeletons(skeleton_file)
except:
print "Error reading data"
return -1
framecount = np.minimum(depthIms.shape[0], colorIms.shape[0])
''' View all data'''
for frame in xrange(framecount):
depth = depthIms[frame]
mask = maskIms[frame] > 0
color = colorIms[frame]
# color = np.dstack([color[:,:,2],color[:,:,1],color[:,:,0]])
''' Skeleton in world (w) and image (i) coordinates '''
skel_w = skels_world[frame]
skel_i = world2depth(skel_w, rez=[240, 320])
''' Calculate hogs '''
grayIm = (rgb2gray(color) * 255).astype(np.uint8)
person_mask, bounding_boxes, labels = extract_people(grayIm, mask > 0)
rez = grayIm[bounding_boxes[0]].shape
''' Plot skeletons on color image'''
# color = color*(mask[:,:,None]==0)
# from pylab import *
# embed()
# blurIm = (grayIm*mask).astype(np.float)
# blurIm = color*mask[:,:,None]
# blurIm = cv2.GaussianBlur(color, (15,15), 10)
# blurIm = cv2.GaussianBlur(mask[:,:,None]*color, (9,9), 5)
# blurIm = cv2.blur(mask[:,:,None]*color, (5,5))
# cv2.imshow("b", blurIm/float(blurIm.max()))
''' Visualization '''
# color = display_skeletons(color, skel_i, color=(0,200,0))
# blurIm = display_skeletons(blurIm, skel_i, color=(0,200,0))
cv2.imshow("Depth", depth / float(depth.max()))
cv2.imshow("RGB", color)
# cv2.imshow("RGB_blur", color*(mask[:,:,None]==0) + blurIm*(mask[:,:,None]>0))
# cv2.imshow("RGB masked", color*(mask[:,:,None]>0))
ret = cv2.waitKey(10)
if ret >= 0:
break
# sequenceFrameNames = files[1200:1300]
sequenceFrameNames = files[::500]
# sequenceFrameNames = files[7200:7230] #:7300
imgs = []
for i in sequenceFrameNames:
imgs.append(scipy.misc.imread(dataDir+str(i)+'.jpg'))
imgs = np.array(imgs)
''' Get posMat from individual image '''
# t=7
t=13
im = imgs[t]
objectNum = 0
# posMatFull = posImage2XYZ(im, 500, 1250)
posMatFull = posImage2XYZ(im, 500, 2000)
imLabels, objSlices, objInds = extract_people(posMatFull[:,:,2], 10000, True)
if len(objInds)!=0:
t += 1
assert len(objInds)!=0, "Error: No objects"
posMat = posMatFull[objSlices[objectNum]]
for i in range(3):
posMat[:,:,i] *= (imLabels[objSlices[objectNum]]==objInds[objectNum])
posMat = removeNoise(posMat, thresh=500)
xyz = posMat[(posMat[:,:,2]>0)*(posMat[:,:,0]!=0),:]
''' Get geodesic extrema '''
t1 = time.time()
regions, regionXYZ, regionLabels, edgeDict = regionGraph(posMat, pixelSize=1500)
regionPos = [x[2] for x in regionLabels[1:]]
regionPos.insert(0, [0,0])
if t > 0:
if bodyPos == -1 or len(bodyPos) == 7:
AllBodyPos.append(bodyPos)
bodyTimes.append(sequenceFrameNames[t-1])
else:
print "Error. Redoing the last frame."
t -= 1
else:
AllBodyPos = []
bodyPos = []
im = imgs[t]
objectNum = 0
posMatFull = posImage2XYZ(im, 500, 2000)
imLabels, objSlices, objInds = extract_people(posMatFull[:,:,2], 10000, True)
if len(objInds)==0:
print"No humans"
bodyPos = -1
else:
posMat = posMatFull[objSlices[objectNum]]
for i in range(3):
posMat[:,:,i] *= (imLabels[objSlices[objectNum]]==objInds[objectNum])
posMat = removeNoise(posMat, thresh=500)
fig = figure(2)
fig.canvas.mpl_connect('button_press_event', onclick)
ax = fig.add_subplot(111)
ax.imshow(posMat[:,:,2])
def main(get_depth, get_color, get_skeleton, get_mask, calculate_features, visualize, save_anonomized, device):
dev = device
ret = 0
backgroundTemplates = np.empty([1,1,1])
backgroundModel = None
backgroundCount = 20
bgPercentage = .05
prevDepthIm = None
day_dirs = os.listdir('depth/')
day_dirs = [x for x in day_dirs if x[0]!='.']
day_dirs.sort(key=lambda x: int(x))
hour_index = 0
minute_index=0
allFeatures = []
coms = []
orns = []
play_speed = 1
new_date_entered = False
framerate = 0
frame_prev = 0
frame_prev_time = time()
day_index = 0
while day_index < len(day_dirs):
if new_date_entered:
try:
day_index = day_dirs.index(day_new)
except:
print "Day not found"
day_index = 0
dayDir = day_dirs[day_index]
hour_dirs = os.listdir('depth/'+dayDir)
hour_dirs = [x for x in hour_dirs if x[0]!='.']
hour_dirs.sort(key=lambda x: int(x))
'''Hours'''
''' Check for new Hours index '''
if not new_date_entered:
if play_speed >= 0 and ret != keys_frame_left:
hour_index = 0
else:
hour_index = len(hour_dirs)-1
else:
try:
hour_index = hour_dirs.index(hour_new)
except:
print "Hour was not found"
hour_index = 0
while hour_index < len(hour_dirs):
hourDir = hour_dirs[hour_index]
minute_dirs = os.listdir('depth/'+dayDir+'/'+hourDir)
minute_dirs = [x for x in minute_dirs if x[0]!='.']
minute_dirs.sort(key=lambda x: int(x))
'''Minutes'''
''' Check for new minute index '''
if not new_date_entered:
if play_speed >= 0 and ret != keys_frame_left:
minute_index = 0
else:
minute_index = len(minute_dirs)-1
else:
try:
minute_index = minute_dirs.index(minute_new)
except:
print "Minute was not found"
minute_index = 0
''' Loop through this minute '''
while minute_index < len(minute_dirs):
minute_dir = minute_dirs[minute_index]
# Prevent from reading hidden files
if minute_dir[0] == '.':
continue
depth_files = []
skelFiles = []
# For each available device:
devices = os.listdir('depth/'+dayDir+'/'+hourDir+'/'+minute_dir)
devices = [x for x in devices if x[0]!='.' and x.find('tmp')<0]
devices.sort()
deviceID = "device_{0:d}".format(dev+1)
if not os.path.isdir('depth/'+dayDir+'/'+hourDir+'/'+minute_dir+'/'+deviceID):
continue
''' Sort files '''
if get_depth:
depthTmp = os.listdir('depth/'+dayDir+'/'+hourDir+'/'+minute_dir+'/'+deviceID)
tmpSort = [int(x.split('_')[-3])*100 + int(formatFileString(x.split('_')[-2])) for x in depthTmp]
depthTmp = np.array(depthTmp)[np.argsort(tmpSort)].tolist()
depth_files.append([x for x in depthTmp if x.find('.png')>=0])
if get_skeleton:
skelTmp = os.listdir('skel/'+dayDir+'/'+hourDir+'/'+minute_dir+'/'+deviceID)
tmpSort = [int(x.split('_')[-4])*100 + int(formatFileString(x.split('_')[-3])) for x in skelTmp]
skelTmp = np.array(skelTmp)[np.argsort(tmpSort)].tolist()
skelFiles.append([x for x in skelTmp if x.find('.dat')>=0])
if len(depth_files) == 0:
continue
if play_speed >= 0 and ret != keys_frame_left:
frame_id = 0
else:
frame_id = len(depth_files[dev])-1
while frame_id < len(depth_files[0]):
# while frame_id < len(depth_files[dev]):
depthFile = depth_files[0][frame_id]
# try:
if 1:
''' Load Depth '''
if get_depth:
depthIm = sm.imread('depth/'+dayDir+'/'+hourDir+'/'+minute_dir+'/'+deviceID+'/'+depthFile)
depthIm = np.array(depthIm, dtype=np.uint16)
''' Load Color '''
if get_color:
colorFile = 'color_'+depthFile[6:-4]+'.jpg'
colorIm = sm.imread('color/'+dayDir+'/'+hourDir+'/'+minute_dir+'/'+deviceID+'/'+colorFile)
# colorIm_g = colorIm.mean(-1, dtype=np.uint8)
colorIm_g = skimage.img_as_ubyte(skimage.color.rgb2gray(colorIm))
# colorIm_lab = skimage.color.rgb2lab(colorIm).astype(np.uint8)
''' Load Skeleton Data '''
if get_skeleton:
skelFile = 'skel_'+depthFile[6:-4]+'_.dat'
if os.path.isfile('skel/'+dayDir+'/'+hourDir+'/'+minute_dir+'/'+deviceID+'/'+skelFile):
with open('skel/'+dayDir+'/'+hourDir+'/'+minute_dir+'/'+deviceID+'/'+skelFile, 'rb') as inFile:
users = pickle.load(inFile)
else:
print "No user file:", skelFile
coms = [users[x]['com'] for x in users.keys() if users[x]['com'][2] > 0.0]
jointCount = 0
for i in users.keys():
user = users[i]
timestamp = depthFile[:-4].split('_')[1:] # Day, hour, minute, second, millisecond, Frame number in this second
depthIm = np.minimum(depthIm.astype(np.float), 5000)
fill_image(depthIm)
'''Background model'''
if backgroundModel is None:
bgSubtraction = AdaptiveMixtureOfGaussians(depthIm, maxGaussians=3, learningRate=0.01, decayRate=0.02, variance=300**2)
backgroundModel = bgSubtraction.getModel()
if get_color:
prevColorIm = colorIm_g.copy()
continue
else:
bgSubtraction.update(depthIm)
backgroundModel = bgSubtraction.getModel()
foregroundMask = bgSubtraction.get_foreground(thresh=50)
''' Find people '''
if get_skeleton:
ret = plotUsers(depthIm, users, device=deviceID, vis=True)
if get_mask:
foregroundMask, userBoundingBoxes, userLabels = extract_people(depthIm, foregroundMask, minPersonPixThresh=1500, gradientFilter=True, gradThresh=100)
''' Calculate user features '''
if calculate_features and get_color:
''' Color Optical Flow '''
flow = getFlow(prevColorIm, colorIm_g)
prevColorIm = colorIm_g.copy()
userCount = len(userBoundingBoxes)
for i in xrange(userCount):
userBox = userBoundingBoxes[i]
userMask = foregroundMask==i+1
allFeatures.append(computeUserFeatures(colorIm, depthIm, flow, userBox, time=timestamp, mask=userMask, windowSize=[96,72], visualise=False))
''' Or get CoM + orientation '''
if get_mask and not calculate_features:
userCount = len(userBoundingBoxes)
for i in xrange(userCount):
userBox = userBoundingBoxes[i]
userMask = foregroundMask==i+1
com, ornBasis = calculateBasicPose(depthIm, userMask)
coms.append(com)
orns.append(ornBasis[1])
allFeatures.append({'com':com, "orn":ornBasis, 'time':timestamp})
''' Visualization '''
if visualize:
tmpSecond = depthFile.split("_")[-3]
if len(tmpSecond) == 0:
tmpSecond = '0'+tmpSecond
if get_depth:
vv.imshow("Depth", depthIm/6000.)
vv.putText("Depth", "Day "+dayDir+" Time "+hourDir+":"+minute_dir+":"+tmpSecond, (5,220), size=15)
vv.putText("Depth", "Play speed: "+str(play_speed)+"x", (5,15), size=15)
vv.putText("Depth", str(int(framerate))+" fps", (275,15), size=15)
if get_color:
vv.putText(colorIm, "Day "+dayDir+" Time "+hourDir+":"+minute_dir+" Dev#"+str(dev), (10,220))
vv.imshow("I_orig", colorIm)
if get_mask:
# vv.imshow("I", colorIm*foregroundMask[:,:,np.newaxis])
vv.imshow("I_masked", colorIm + (255-colorIm)*(((foregroundMask)[:,:,np.newaxis])))
if get_mask:
vv.imshow("Mask", foregroundMask.astype(np.float)/float(foregroundMask.max()))
# vv.imshow("BG Model", backgroundModel.astype(np.float)/float(backgroundModel.max()))
''' Multi-camera map '''
if 0 and len(coms) > 0:
mapRez = [200,200]
mapIm = np.zeros(mapRez)
coms_np = np.array(coms)
xs = np.minimum(np.maximum(mapRez[0]+((coms_np[:,2]+500)/3000.*mapRez[0]).astype(np.int), 0),mapRez[0]-1)
ys = np.minimum(np.maximum(((coms_np[:,0]+500)/1500.*mapRez[0]).astype(np.int), 0), mapRez[1]-1)
mapIm[xs, ys] = 255
vv.imshow("Map", mapIm)
# scatter(coms_np[:,0], -coms_np[:,2])
'''3D Vis'''
if 0:
# figure = mlab.figure(1, fgcolor=(1,1,1), bgcolor=(0,0,0))
# from pyKinectTools.utils.DepthUtils import *
pts = depthIm2XYZ(depthIm).astype(np.int)
interval = 25
figure.scene.disable_render = True
mlab.clf()
# ss = mlab.points3d(-pts[::interval,0], pts[::interval,1], pts[::interval,2], colormap='Blues', vmin=1000., vmax=5000., mode='2dvertex')
ss = mlab.points3d(pts[::interval,0], pts[::interval,1], pts[::interval,2], 5.-(np.minimum(pts[::interval,2], 5000)/float((-pts[:,2]).max()))/1000., scale_factor=25., colormap='Blues')#, mode='2dvertex')
# , scale_factor=25.
mlab.view(azimuth=0, elevation=0, distance=3000., focalpoint=(0,0,0), figure=figure)#, reset_roll=False)
# mlab.roll(90)
currentView = mlab.view()
figure.scene.disable_render = False
mlab.draw()
# mlab.show()
# ss = mlab.points3d(pts[::interval,0], pts[::interval,1], pts[::interval,2], color=col, scale_factor=5)
# ss = mlab.points3d(pts[:,0], pts[:,1], pts[:,2], color=(1,1,1), scale_factor=5)
# ss = mlab.points3d(pts[:,0], pts[:,1], pts[:,2])
''' Playback control: Look at keyboard input '''
ret = vv.waitKey()
if frame_id - frame_prev > 0:
framerate = (frame_id - frame_prev) / (time() - frame_prev_time)
frame_prev = frame_id
frame_prev_time = time()
new_date_entered = False
if ret > 0:
# player_controls(ret)
# print "Ret is",ret
if ret == keys_ESC:
break
elif ret == keys_space:
print "Enter the following into the command line"
tmp = raw_input("Enter date: ")
day_new = tmp
tmp = raw_input("Enter hour: ")
hour_new = tmp
tmp = raw_input("Enter minute: ")
minute_new = tmp
print "New date:", day_new, hour_new, minute_new
new_date_entered = True
break
elif ret == keys_down_arrow:
play_speed = 0
elif ret == keys_left_arrow:
play_speed -= 1
elif ret == keys_right_arrow:
play_speed += 1
elif ret == keys_i:
embed()
elif ret == keys_frame_left:
frame_id -= 1
elif ret == keys_frame_right:
frame_id += 1
elif ret == keys_help:
display_help()
frame_id += play_speed
if save_anonomized and get_mask:
save_dir = 'color_masked/'+dayDir+'/'+hourDir+'/'+minute_dir+'/'+devices[dev]+'/'
createDirectory(save_dir)
sm.imsave(save_dir+'colorM_'+depthFile[6:-4]+'.jpg', colorIm*(1-foregroundMask))
# except:
# print "Erroneous frame"
# if visualize:
# vv.imshow("D", depthIm.astype(np.float)/5000)
# ret = vv.waitKey(10)
# End seconds
if ret == keys_ESC or new_date_entered:
break
if frame_id >= len(depth_files[0]):
minute_index += 1
elif frame_id < 0:
minute_index -= 1
break
# End hours
if ret == keys_ESC or new_date_entered:
break
if minute_index >= len(minute_dirs):
hour_index += 1
elif minute_index < 0:
hour_index -= 1
break
# End days
if ret == keys_ESC:
break
if new_date_entered:
break
if hour_index >= len(hour_dirs):
day_index += 1
elif hour_index < 0:
day_index -= 1
if day_index < 0:
day_index = 0
if ret == keys_ESC or day_index > len(day_dirs):
break
np.save("/media/Data/r40_cX_", allFeatures)
embed()
def compute_features(name, vis=False, person_rez=[144, 72]):
'''
---Parameters---
filename : base filename for depth/color/skel
vis: turn visualize on?
person_rez : all hog/hof features should be the same dimensions, so we resize the people to this resolution
---Return---
features: features in a dictionary
'''
''' Get filenames '''
depth_file = name + "depth.bin"
color_file = name + "rgb.avi"
skeleton_file = name + "skeleton.txt"
''' Read data from each video/sequence '''
try:
depthIms, maskIms = read_MSR_depth_ims(depth_file)
colorIms = read_MSR_color_ims(color_file)
skels_world, skels_im = read_MSR_skeletons(skeleton_file)
except:
print "Error reading data"
return -1
#print depthIms.shape, colorIms.shape
framecount = np.minimum(depthIms.shape[0], colorIms.shape[0])
dataset_features = {
'framecount': framecount,
'hog': [],
'hof': [],
'skel_image': [],
'skel_world': []
}
grayIm_prev = None
''' View all data'''
for frame in xrange(framecount):
depth = depthIms[frame]
mask = maskIms[frame]
color = colorIms[frame]
# Skeleton in world (w) and image (i) coordinates
skel_w = skels_world[frame]
skel_i = world2depth(skel_w, rez=[240, 320])
''' Calculate hogs '''
grayIm = (rgb2gray(color) * 255).astype(np.uint8)
hogIm = np.zeros_like(depth)
person_mask, bounding_boxes, labels = extract_people(grayIm, mask > 0)
rez = grayIm[bounding_boxes[0]].shape
#hog_input_im = sm.imresize(grayIm[bounding_boxes[0]], person_rez)
hog_input_im = cv2.resize(grayIm[bounding_boxes[0]],
(person_rez[1], person_rez[0]))
hogData, hogImBox = hog(hog_input_im, orientations=4, visualise=True)
#hogIm[bounding_boxes[0]] = sm.imresize(hogImBox, [rez[0],rez[1]])
hogIm[bounding_boxes[0]] = cv2.resize(hogImBox, (rez[1], rez[0]))
# hogIm[bounding_boxes[0]] = hogImBox
hogIm *= person_mask
''' Calculate HOF '''
hofIm = np.zeros_like(depth)
if grayIm_prev is None:
grayIm_prev = np.copy(grayIm)
continue
else:
flow = getFlow(grayIm_prev[bounding_boxes[0]],
grayIm[bounding_boxes[0]])
rez = flow.shape
bounding_boxes = (bounding_boxes[0][0], bounding_boxes[0][1],
slice(0, 2))
#hof_input_im = np.dstack([sm.imresize(flow[0], [person_rez[0],person_rez[1]]),
# sm.imresize(flow[1], [person_rez[0],person_rez[1]])])i
hof_input_im = np.dstack([
cv2.resize(flow[0], (person_rez[1], person_rez[0])),
cv2.resize(flow[1], (person_rez[1], person_rez[0]))
])
hofData, hofImBox = hof(hof_input_im,
orientations=5,
visualise=True)
#hofIm[bounding_boxes[:2]] = sm.imresize(hofImBox, [rez[0],rez[1]])
hofIm[bounding_boxes[:2]] = cv2.resize(hogImBox, (rez[1], rez[0]))
hofIm *= person_mask
grayIm_prev = np.copy(grayIm)
''' Add features '''
dataset_features['hog'] += [hogData]
dataset_features['hof'] += [hofData]
dataset_features['skel_image'] += [skel_i]
dataset_features['skel_world'] += [skel_w]
''' Plot skeletons on color image'''
if vis:
color = display_skeletons(color, skel_i)
''' Visualization '''
cv2.imshow("Depth", depth / float(depth.max()))
cv2.imshow("HOG", hogIm / float(hogIm.max()))
cv2.imshow("RGB", color)
cv2.imshow("RGB masked", color * (mask[:, :, None] > 0))
cv2.imshow("HOF", hofIm / float(hofIm.max()))
ret = cv2.waitKey(10)
if ret >= 0:
break
print "Done calculating ", name
return dataset_features
def main(get_depth, get_color, get_skeleton, get_mask, calculate_features,
visualize, save_anonomized, device):
dev = device
ret = 0
backgroundTemplates = np.empty([1, 1, 1])
backgroundModel = None
backgroundCount = 20
bgPercentage = .05
prevDepthIm = None
day_dirs = os.listdir('depth/')
day_dirs = [x for x in day_dirs if x[0] != '.']
day_dirs.sort(key=lambda x: int(x))
hour_index = 0
minute_index = 0
allFeatures = []
coms = []
orns = []
play_speed = 1
new_date_entered = False
framerate = 0
frame_prev = 0
frame_prev_time = time()
day_index = 0
while day_index < len(day_dirs):
if new_date_entered:
try:
day_index = day_dirs.index(day_new)
except:
print "Day not found"
day_index = 0
dayDir = day_dirs[day_index]
hour_dirs = os.listdir('depth/' + dayDir)
hour_dirs = [x for x in hour_dirs if x[0] != '.']
hour_dirs.sort(key=lambda x: int(x))
'''Hours'''
''' Check for new Hours index '''
if not new_date_entered:
if play_speed >= 0 and ret != keys_frame_left:
hour_index = 0
else:
hour_index = len(hour_dirs) - 1
else:
try:
hour_index = hour_dirs.index(hour_new)
except:
print "Hour was not found"
hour_index = 0
while hour_index < len(hour_dirs):
hourDir = hour_dirs[hour_index]
minute_dirs = os.listdir('depth/' + dayDir + '/' + hourDir)
minute_dirs = [x for x in minute_dirs if x[0] != '.']
minute_dirs.sort(key=lambda x: int(x))
'''Minutes'''
''' Check for new minute index '''
if not new_date_entered:
if play_speed >= 0 and ret != keys_frame_left:
minute_index = 0
else:
minute_index = len(minute_dirs) - 1
else:
try:
minute_index = minute_dirs.index(minute_new)
except:
print "Minute was not found"
minute_index = 0
''' Loop through this minute '''
while minute_index < len(minute_dirs):
minute_dir = minute_dirs[minute_index]
# Prevent from reading hidden files
if minute_dir[0] == '.':
continue
depth_files = []
skelFiles = []
# For each available device:
devices = os.listdir('depth/' + dayDir + '/' + hourDir + '/' +
minute_dir)
devices = [
x for x in devices if x[0] != '.' and x.find('tmp') < 0
]
devices.sort()
deviceID = "device_{0:d}".format(dev + 1)
if not os.path.isdir('depth/' + dayDir + '/' + hourDir + '/' +
minute_dir + '/' + deviceID):
continue
''' Sort files '''
if get_depth:
depthTmp = os.listdir('depth/' + dayDir + '/' + hourDir +
'/' + minute_dir + '/' + deviceID)
tmpSort = [
int(x.split('_')[-3]) * 100 +
int(formatFileString(x.split('_')[-2]))
for x in depthTmp
]
depthTmp = np.array(depthTmp)[np.argsort(tmpSort)].tolist()
depth_files.append(
[x for x in depthTmp if x.find('.png') >= 0])
if get_skeleton:
skelTmp = os.listdir('skel/' + dayDir + '/' + hourDir +
'/' + minute_dir + '/' + deviceID)
tmpSort = [
int(x.split('_')[-4]) * 100 +
int(formatFileString(x.split('_')[-3]))
for x in skelTmp
]
skelTmp = np.array(skelTmp)[np.argsort(tmpSort)].tolist()
skelFiles.append(
[x for x in skelTmp if x.find('.dat') >= 0])
if len(depth_files) == 0:
continue
if play_speed >= 0 and ret != keys_frame_left:
frame_id = 0
else:
frame_id = len(depth_files[dev]) - 1
while frame_id < len(depth_files[0]):
# while frame_id < len(depth_files[dev]):
depthFile = depth_files[0][frame_id]
# try:
if 1:
''' Load Depth '''
if get_depth:
depthIm = sm.imread('depth/' + dayDir + '/' +
hourDir + '/' + minute_dir +
'/' + deviceID + '/' +
depthFile)
depthIm = np.array(depthIm, dtype=np.uint16)
''' Load Color '''
if get_color:
colorFile = 'color_' + depthFile[6:-4] + '.jpg'
colorIm = sm.imread('color/' + dayDir + '/' +
hourDir + '/' + minute_dir +
'/' + deviceID + '/' +
colorFile)
# colorIm_g = colorIm.mean(-1, dtype=np.uint8)
colorIm_g = skimage.img_as_ubyte(
skimage.color.rgb2gray(colorIm))
# colorIm_lab = skimage.color.rgb2lab(colorIm).astype(np.uint8)
''' Load Skeleton Data '''
if get_skeleton:
skelFile = 'skel_' + depthFile[6:-4] + '_.dat'
if os.path.isfile('skel/' + dayDir + '/' +
hourDir + '/' + minute_dir +
'/' + deviceID + '/' + skelFile):
with open(
'skel/' + dayDir + '/' + hourDir +
'/' + minute_dir + '/' + deviceID +
'/' + skelFile, 'rb') as inFile:
users = pickle.load(inFile)
else:
print "No user file:", skelFile
coms = [
users[x]['com'] for x in users.keys()
if users[x]['com'][2] > 0.0
]
jointCount = 0
for i in users.keys():
user = users[i]
timestamp = depthFile[:-4].split(
'_'
)[1:] # Day, hour, minute, second, millisecond, Frame number in this second
depthIm = np.minimum(depthIm.astype(np.float), 5000)
fill_image(depthIm)
'''Background model'''
if backgroundModel is None:
bgSubtraction = AdaptiveMixtureOfGaussians(
depthIm,
maxGaussians=3,
learningRate=0.01,
decayRate=0.02,
variance=300**2)
backgroundModel = bgSubtraction.getModel()
if get_color:
prevColorIm = colorIm_g.copy()
continue
else:
bgSubtraction.update(depthIm)
backgroundModel = bgSubtraction.getModel()
foregroundMask = bgSubtraction.get_foreground(
thresh=50)
''' Find people '''
if get_skeleton:
ret = plotUsers(depthIm,
users,
device=deviceID,
vis=True)
if get_mask:
foregroundMask, userBoundingBoxes, userLabels = extract_people(
depthIm,
foregroundMask,
minPersonPixThresh=1500,
gradientFilter=True,
gradThresh=100)
''' Calculate user features '''
if calculate_features and get_color:
''' Color Optical Flow '''
flow = getFlow(prevColorIm, colorIm_g)
prevColorIm = colorIm_g.copy()
userCount = len(userBoundingBoxes)
for i in xrange(userCount):
userBox = userBoundingBoxes[i]
userMask = foregroundMask == i + 1
allFeatures.append(
computeUserFeatures(colorIm,
depthIm,
flow,
userBox,
time=timestamp,
mask=userMask,
windowSize=[96, 72],
visualise=False))
''' Or get CoM + orientation '''
if get_mask and not calculate_features:
userCount = len(userBoundingBoxes)
for i in xrange(userCount):
userBox = userBoundingBoxes[i]
userMask = foregroundMask == i + 1
com, ornBasis = calculateBasicPose(
depthIm, userMask)
coms.append(com)
orns.append(ornBasis[1])
allFeatures.append({
'com': com,
"orn": ornBasis,
'time': timestamp
})
''' Visualization '''
if visualize:
tmpSecond = depthFile.split("_")[-3]
if len(tmpSecond) == 0:
tmpSecond = '0' + tmpSecond
if get_depth:
vv.imshow("Depth", depthIm / 6000.)
vv.putText("Depth",
"Day " + dayDir + " Time " +
hourDir + ":" + minute_dir + ":" +
tmpSecond, (5, 220),
size=15)
vv.putText("Depth",
"Play speed: " + str(play_speed) +
"x", (5, 15),
size=15)
vv.putText("Depth",
str(int(framerate)) + " fps",
(275, 15),
size=15)
if get_color:
vv.putText(
colorIm,
"Day " + dayDir + " Time " + hourDir +
":" + minute_dir + " Dev#" + str(dev),
(10, 220))
vv.imshow("I_orig", colorIm)
if get_mask:
# vv.imshow("I", colorIm*foregroundMask[:,:,np.newaxis])
vv.imshow(
"I_masked", colorIm + (255 - colorIm) *
(((foregroundMask)[:, :, np.newaxis])))
if get_mask:
vv.imshow(
"Mask",
foregroundMask.astype(np.float) /
float(foregroundMask.max()))
# vv.imshow("BG Model", backgroundModel.astype(np.float)/float(backgroundModel.max()))
''' Multi-camera map '''
if 0 and len(coms) > 0:
mapRez = [200, 200]
mapIm = np.zeros(mapRez)
coms_np = np.array(coms)
xs = np.minimum(
np.maximum(
mapRez[0] +
((coms_np[:, 2] + 500) / 3000. *
mapRez[0]).astype(np.int), 0),
mapRez[0] - 1)
ys = np.minimum(
np.maximum(((coms_np[:, 0] + 500) / 1500. *
mapRez[0]).astype(np.int), 0),
mapRez[1] - 1)
mapIm[xs, ys] = 255
vv.imshow("Map", mapIm)
# scatter(coms_np[:,0], -coms_np[:,2])
'''3D Vis'''
if 0:
# figure = mlab.figure(1, fgcolor=(1,1,1), bgcolor=(0,0,0))
# from pyKinectTools.utils.DepthUtils import *
pts = depthIm2XYZ(depthIm).astype(np.int)
interval = 25
figure.scene.disable_render = True
mlab.clf()
# ss = mlab.points3d(-pts[::interval,0], pts[::interval,1], pts[::interval,2], colormap='Blues', vmin=1000., vmax=5000., mode='2dvertex')
ss = mlab.points3d(
pts[::interval, 0],
pts[::interval, 1],
pts[::interval, 2],
5. -
(np.minimum(pts[::interval, 2], 5000) /
float((-pts[:, 2]).max())) / 1000.,
scale_factor=25.,
colormap='Blues') #, mode='2dvertex')
# , scale_factor=25.
mlab.view(azimuth=0,
elevation=0,
distance=3000.,
focalpoint=(0, 0, 0),
figure=figure) #, reset_roll=False)
# mlab.roll(90)
currentView = mlab.view()
figure.scene.disable_render = False
mlab.draw()
# mlab.show()
# ss = mlab.points3d(pts[::interval,0], pts[::interval,1], pts[::interval,2], color=col, scale_factor=5)
# ss = mlab.points3d(pts[:,0], pts[:,1], pts[:,2], color=(1,1,1), scale_factor=5)
# ss = mlab.points3d(pts[:,0], pts[:,1], pts[:,2])
''' Playback control: Look at keyboard input '''
ret = vv.waitKey()
if frame_id - frame_prev > 0:
framerate = (frame_id - frame_prev) / (
time() - frame_prev_time)
frame_prev = frame_id
frame_prev_time = time()
new_date_entered = False
if ret > 0:
# player_controls(ret)
# print "Ret is",ret
if ret == keys_ESC:
break
elif ret == keys_space:
print "Enter the following into the command line"
tmp = raw_input("Enter date: ")
day_new = tmp
tmp = raw_input("Enter hour: ")
hour_new = tmp
tmp = raw_input("Enter minute: ")
minute_new = tmp
print "New date:", day_new, hour_new, minute_new
new_date_entered = True
break
elif ret == keys_down_arrow:
play_speed = 0
elif ret == keys_left_arrow:
play_speed -= 1
elif ret == keys_right_arrow:
play_speed += 1
elif ret == keys_i:
embed()
elif ret == keys_frame_left:
frame_id -= 1
elif ret == keys_frame_right:
frame_id += 1
elif ret == keys_help:
display_help()
frame_id += play_speed
if save_anonomized and get_mask:
save_dir = 'color_masked/' + dayDir + '/' + hourDir + '/' + minute_dir + '/' + devices[
dev] + '/'
createDirectory(save_dir)
sm.imsave(
save_dir + 'colorM_' + depthFile[6:-4] + '.jpg',
colorIm * (1 - foregroundMask))
# except:
# print "Erroneous frame"
# if visualize:
# vv.imshow("D", depthIm.astype(np.float)/5000)
# ret = vv.waitKey(10)
# End seconds
if ret == keys_ESC or new_date_entered:
break
if frame_id >= len(depth_files[0]):
minute_index += 1
elif frame_id < 0:
minute_index -= 1
break
# End hours
if ret == keys_ESC or new_date_entered:
break
if minute_index >= len(minute_dirs):
hour_index += 1
elif minute_index < 0:
hour_index -= 1
break
# End days
if ret == keys_ESC:
break
if new_date_entered:
break
if hour_index >= len(hour_dirs):
day_index += 1
elif hour_index < 0:
day_index -= 1
if day_index < 0:
day_index = 0
if ret == keys_ESC or day_index > len(day_dirs):
break
np.save("/media/Data/r40_cX_", allFeatures)
embed()
def main(visualize=True):
n_cameras = 1
cam = KinectPlayer(base_dir='./',
device=1,
bg_subtraction=True,
get_depth=True,
get_color=True,
get_skeleton=False,
background_model='box',
background_param=3200)
# embed()
cloud = pcl.PointCloud()
angle_measurements = []
dist_thresh = 10
max_iter = 500
n_planes = 3
framerate = 1
cam.play_speed = 300
while cam.next(framerate):
# mask = cam.get_person() > 0
# embed()
id_im, id_slices, ids, id_px = extract_people(cam.depthIm,
gradThresh=50)
# id_max = np.argmin([np.abs(cam.depthIm[0]/2 - (x[1].stop+x[1].start)/2.) for x in id_slices])
id_max = np.argmin([
np.abs(320 / 2 - (x[1].stop + x[1].start) / 2.) for x in id_slices
])
mask = id_im == (id_max + 1)
cam.depthIm *= mask
planes_idx = []
planes_models = []
plane_im = np.repeat(cam.depthIm[:, :, None], 3,
-1).reshape([-1, 3]).copy()
# plane_im_out = np.repeat(cam.depthIm[:,:,None]*0, 3, -1).reshape([-1,3]).copy()
plane_im_out = np.zeros(
[cam.depthIm.shape[0] * cam.depthIm.shape[1], 3], np.float)
# Find planes
for i in range(n_planes):
# Get valid points
pts = cam.camera_model.im2PosIm(plane_im[:, 0].reshape(
cam.depthIm.shape)).reshape([-1, 3])
nonzero_idx = np.nonzero((pts[:, 2] > 0) * (pts[:, 2] != 1000))
cloud.from_array(pts[nonzero_idx].astype(np.float32))
# Find plane
indices, model = find_plane(cloud, dist_thresh, max_iter)
print "Plane:", model, len(indices)
# if len(indices) < 5000:
# continue
planes_idx += [indices]
planes_models += [model]
# Remove labeled points and highlight on image
tmp = plane_im[nonzero_idx]
tmp[(np.array(indices), )] = num_to_rgb[i] * 1000
plane_im[nonzero_idx] = tmp
# plane_im_out[nonzero_idx[(np.array(indices),)]] = tmp
plane_im_out[nonzero_idx[0][indices]] = num_to_rgb[i] * 1000
angle_measurements = []
for combos in it.combinations(planes_models, 2):
angle_measurements += [
np.arccos(np.dot(combos[0][:3], combos[1][:3])) * 180. / np.pi
]
print "Current angle:", angle_measurements
# angle_measurements += [np.arccos(np.dot(planes_models[0][:3], planes_models[1][:3]))*180./np.pi]
# print "Current angle:", angle_measurements[-1]
# print "Average angle:", np.mean(angle_measurements)
print ""
# tmp = plane_im[nonzero_idx]
# tmp[(np.array(planes_idx[1]),)] = np.array([0, 2000, 0])
# plane_im[nonzero_idx] = tmp
plane_im = plane_im.reshape([240, 320, 3])
plane_im_out = plane_im_out.reshape([240, 320, 3])
# cv2.imshow("dd", plane_im/float(plane_im.max()))
cv2.imshow("dd", (plane_im_out / float(plane_im_out.max())) * 100. +
cam.colorIm / 300.)
# cv2.waitKey(50)
# cam.visualize(color=True, depth=True, text=True, colorize=True, depth_bounds=[500,3500])
cam.visualize(color=False,
depth=True,
text=True,
colorize=False,
depth_bounds=[500, 3500])
# embed()
embed()
print 'Done'
def main(get_depth, get_color, get_skeleton, visualize):
# fill = True
fill = False
get_color = True
# VP = KinectPlayer(base_dir='./', device=device, get_depth=get_depth, get_color=get_color, get_skeleton=get_skeleton, get_mask=get_mask)
# VP = KinectPlayer(base_dir='./', device=1, bg_subtraction=True, get_depth=get_depth, get_color=get_color, get_skeleton=get_skeleton, fill_images=fill)
VP = KinectPlayer(base_dir='./', device=2, bg_subtraction=True, get_depth=True, get_color=True, get_skeleton=False, fill_images=fill)
cam_count = 1
if cam_count == 2:
VP2 = KinectPlayer(base_dir='./', device=2, bg_subtraction=True, get_depth=True, get_color=True, get_skeleton=False, fill_images=fill)
# Transformation matrix from first to second camera
# data = pickle.load(open("./Registration.dat", 'r'))
# transform_c1_to_c2 = data['transform']
# print 'aaaaaaaaaaaaaaaa'
# embed()
while VP.next():
# VP.update_background()
# Transform skels from cam1 to cam2
if get_skeleton:
VP_skels = [np.array(VP.users[s]['jointPositions'].values()) for s in VP.users.keys()]
if cam_count == 2:
VP2.sync_cameras(VP)
if get_skeleton:
VP2_skels = transform_skels(VP_skels, transform_c1_to_c2)
VP.colorIm = VP.colorIm[:,:,[2,1,0]]
# im_tmp = np.dstack([VP.depthIm/float(VP.depthIm.max()),\
# VP.depthIm/float(VP.depthIm.max()),\
# VP.colorIm.mean(-1)/255])
# im = felzenszwalb(im_tmp, scale=255)
# im = felzenszwalb(VP.depthIm/float(VP.depthIm.max()), scale=255)
# im = VP.depthIm/float(VP.depthIm.max()) * VP.
# im = felzenszwalb(im, scale=255)
# cv2.imshow("seg", im/float(im.max()))
# cv2.waitKey(10)
# embed()
# Geodesic extrema
if 0:
if VP.foregroundMask is not None:
im = VP.depthIm * (VP.foregroundMask>0)
# cv2.imshow("im--", im/float(im.max()))
if 1:#(im>0).sum() > 1000:
# Extract person
labelMask, boxes, labels, px_counts= extract_people(im, VP.foregroundMask, minPersonPixThresh=3000)
# print labels, px_counts
if len(labels) > 0:
max_ind = np.argmax(px_counts)
mask = labelMask==max_ind+1
im[-mask] = 0
edge_thresh = 200#10
gradients = np.gradient(im)
mag = np.sqrt(gradients[0]**2+gradients[1]**2)
im[mag>edge_thresh] = 0
# Segmentation experiment
# im_s = VP.depthIm/float(VP.depthIm.max())
# im_s = felzenszwalb(im, scale=255)
# cv2.imshow("seg", im_s/float(im_s.max()))
x,y = nd.center_of_mass(im)
if im[x,y] == 0:
tmp = np.nonzero(im>0)
x,y = [tmp[0][0], tmp[1][0]]
# min_map = geodesic_extrema_MPI(im, centroid=[x,y], iterations=15)
extrema = geodesic_extrema_MPI(im, centroid=[x,y], iterations=15)
# embed()
# for e, i in zip(extrema, range(20)):
# if 0 < i < 6:
# box_color = [255, 0, 0]
# elif i==0:
# box_color = [0, 0, 255]
# else:
# box_color = [255,255,255]
# VP.colorIm[e[0]-4:e[0]+5, e[1]-4:e[1]+5] = box_color
# cv2.imshow("ExtremaB", min_map/float(min_map.max()))
# cv2.waitKey(500)
cv2.waitKey(20)
# cv2.imshow("bg model", VP.backgroundModel/float(VP.backgroundModel.max()))
# cv2.imshow("foregroundMask", ((VP.foregroundMask>0)*255).astype(np.uint8))
if visualize:
if cam_count == 2:
for s in VP2_skels:
VP2.depthIm = display_skeletons(VP2.depthIm, s, (VP2.depthIm.max(),), skel_type='Kinect')
VP2.visualize()
VP2.playback_control()
VP.visualize()
def compute_features(name, vis=False, person_rez=[144,72]):
'''
---Parameters---
filename : base filename for depth/color/skel
vis: turn visualize on?
person_rez : all hog/hof features should be the same dimensions, so we resize the people to this resolution
---Return---
features: features in a dictionary
'''
''' Get filenames '''
depth_file = name + "depth.bin"
color_file = name + "rgb.avi"
skeleton_file = name + "skeleton.txt"
''' Read data from each video/sequence '''
try:
depthIms, maskIms = read_MSR_depth_ims(depth_file)
colorIms = read_MSR_color_ims(color_file)
skels_world, skels_im = read_MSR_skeletons(skeleton_file)
except:
print "Error reading data"
return -1
#print depthIms.shape, colorIms.shape
framecount = np.minimum(depthIms.shape[0], colorIms.shape[0])
dataset_features = {'framecount':framecount, 'hog':[], 'hof':[], 'skel_image':[], 'skel_world':[]}
grayIm_prev = None
''' View all data'''
for frame in xrange(framecount):
depth = depthIms[frame]
mask = maskIms[frame]
color = colorIms[frame]
# Skeleton in world (w) and image (i) coordinates
skel_w = skels_world[frame]
skel_i = world2depth(skel_w, rez=[240,320])
''' Calculate hogs '''
grayIm = (rgb2gray(color) * 255).astype(np.uint8)
hogIm = np.zeros_like(depth)
person_mask, bounding_boxes, labels = extract_people(grayIm, mask>0)
rez = grayIm[bounding_boxes[0]].shape
#hog_input_im = sm.imresize(grayIm[bounding_boxes[0]], person_rez)
hog_input_im = cv2.resize(grayIm[bounding_boxes[0]], (person_rez[1], person_rez[0]))
hogData, hogImBox = hog(hog_input_im, orientations=4, visualise=True)
#hogIm[bounding_boxes[0]] = sm.imresize(hogImBox, [rez[0],rez[1]])
hogIm[bounding_boxes[0]] = cv2.resize(hogImBox, (rez[1],rez[0]))
# hogIm[bounding_boxes[0]] = hogImBox
hogIm *= person_mask
''' Calculate HOF '''
hofIm = np.zeros_like(depth)
if grayIm_prev is None:
grayIm_prev = np.copy(grayIm)
continue
else:
flow = getFlow(grayIm_prev[bounding_boxes[0]], grayIm[bounding_boxes[0]])
rez = flow.shape
bounding_boxes = (bounding_boxes[0][0], bounding_boxes[0][1], slice(0,2))
#hof_input_im = np.dstack([sm.imresize(flow[0], [person_rez[0],person_rez[1]]),
# sm.imresize(flow[1], [person_rez[0],person_rez[1]])])i
hof_input_im = np.dstack([cv2.resize(flow[0], (person_rez[1],person_rez[0])), cv2.resize(flow[1], (person_rez[1], person_rez[0]))])
hofData, hofImBox = hof(hof_input_im, orientations=5, visualise=True)
#hofIm[bounding_boxes[:2]] = sm.imresize(hofImBox, [rez[0],rez[1]])
hofIm[bounding_boxes[:2]] = cv2.resize(hogImBox, (rez[1],rez[0]))
hofIm *= person_mask
grayIm_prev = np.copy(grayIm)
''' Add features '''
dataset_features['hog'] += [hogData]
dataset_features['hof'] += [hofData]
dataset_features['skel_image'] += [skel_i]
dataset_features['skel_world'] += [skel_w]
''' Plot skeletons on color image'''
if vis:
color = display_skeletons(color, skel_i)
''' Visualization '''
cv2.imshow("Depth", depth/float(depth.max()))
cv2.imshow("HOG", hogIm/float(hogIm.max()))
cv2.imshow("RGB", color)
cv2.imshow("RGB masked", color*(mask[:,:,None]>0))
cv2.imshow("HOF", hofIm/float(hofIm.max()))
ret = cv2.waitKey(10)
if ret >= 0:
break
print "Done calculating ", name
return dataset_features
def main(visualize=True):
n_cameras = 1
cam = KinectPlayer(base_dir='./', device=1, bg_subtraction=True, get_depth=True,
get_color=True, get_skeleton=False, background_model='box', background_param=3200)
# embed()
cloud = pcl.PointCloud()
angle_measurements = []
dist_thresh = 10
max_iter = 500
n_planes = 3
framerate = 1
cam.play_speed = 300
while cam.next(framerate):
# mask = cam.get_person() > 0
# embed()
id_im, id_slices, ids, id_px = extract_people(cam.depthIm, gradThresh=50)
# id_max = np.argmin([np.abs(cam.depthIm[0]/2 - (x[1].stop+x[1].start)/2.) for x in id_slices])
id_max = np.argmin([np.abs(320/2 - (x[1].stop+x[1].start)/2.) for x in id_slices])
mask = id_im==(id_max+1)
cam.depthIm *= mask
planes_idx = []
planes_models = []
plane_im = np.repeat(cam.depthIm[:,:,None], 3, -1).reshape([-1,3]).copy()
# plane_im_out = np.repeat(cam.depthIm[:,:,None]*0, 3, -1).reshape([-1,3]).copy()
plane_im_out = np.zeros([cam.depthIm.shape[0]*cam.depthIm.shape[1], 3], np.float)
# Find planes
for i in range(n_planes):
# Get valid points
pts = cam.camera_model.im2PosIm(plane_im[:,0].reshape(cam.depthIm.shape)).reshape([-1,3])
nonzero_idx = np.nonzero((pts[:,2]>0)*(pts[:,2]!=1000) )
cloud.from_array(pts[nonzero_idx].astype(np.float32))
# Find plane
indices, model = find_plane(cloud, dist_thresh, max_iter)
print "Plane:", model, len(indices)
# if len(indices) < 5000:
# continue
planes_idx += [indices]
planes_models += [model]
# Remove labeled points and highlight on image
tmp = plane_im[nonzero_idx]
tmp[(np.array(indices),)] = num_to_rgb[i]*1000
plane_im[nonzero_idx] = tmp
# plane_im_out[nonzero_idx[(np.array(indices),)]] = tmp
plane_im_out[nonzero_idx[0][indices]] = num_to_rgb[i]*1000
angle_measurements = []
for combos in it.combinations(planes_models, 2):
angle_measurements += [np.arccos(np.dot(combos[0][:3], combos[1][:3]))*180./np.pi]
print "Current angle:", angle_measurements
# angle_measurements += [np.arccos(np.dot(planes_models[0][:3], planes_models[1][:3]))*180./np.pi]
# print "Current angle:", angle_measurements[-1]
# print "Average angle:", np.mean(angle_measurements)
print ""
# tmp = plane_im[nonzero_idx]
# tmp[(np.array(planes_idx[1]),)] = np.array([0, 2000, 0])
# plane_im[nonzero_idx] = tmp
plane_im = plane_im.reshape([240,320,3])
plane_im_out = plane_im_out.reshape([240,320,3])
# cv2.imshow("dd", plane_im/float(plane_im.max()))
cv2.imshow("dd", (plane_im_out/float(plane_im_out.max()))*100. + cam.colorIm/300.)
# cv2.waitKey(50)
# cam.visualize(color=True, depth=True, text=True, colorize=True, depth_bounds=[500,3500])
cam.visualize(color=False, depth=True, text=True, colorize=False, depth_bounds=[500,3500])
# embed()
embed()
print 'Done'