def MatchAllCluster(save, maxdist=200, groups=3, filtparam=2.0): PointsList, DisList, img, depth = MatchAllCapture(0, maxdist) PointsClusterList = [] for i in xrange(len(PointsList)): if depth[PointsList[i].pt[1], PointsList[i].pt[0]] <> 0 and depth[ PointsList[i].pt[1], PointsList[i].pt[0]] < 1000: PointsClusterList.append( [PointsList[i].pt[0], PointsList[i].pt[1]]) Z = np.array(PointsClusterList) # convert to np.float32 Z = np.float32(Z) segregated, centers, distFromCenter, distFromCenterAve = Cluster(Z, 3) #Create List for reduced points segregatedF = [] for i in xrange(groups): segregatedF.append([]) #Remove points which are not close to centroid for j in xrange(groups): for i in range(len(segregated[j])): if distFromCenter[j][i] < filtparam * distFromCenterAve[j]: segregatedF[j].append(segregated[j][i]) for j in xrange(groups): segregatedF[j] = np.array(segregatedF[j]) # Create a centriod depth list FinalCenters = [] for j in xrange(groups): FinalCenters.append([ centers[j][0], centers[j][1], depth[centers[j][1], centers[j][0]] ]) # Convert to world coordinates FinalCentersWC = convertToWorldCoords(FinalCenters) return segregatedF, centers, img, depth, FinalCenters, FinalCentersWC
def MatchAllCluster(save, maxdist=200, filtparam=2.0): PointsList, DisList, img, depth = MatchAllCapture(0, maxdist) PointsClusterList = [] for i in xrange(len(PointsList)): if depth[PointsList[i].pt[1], PointsList[i].pt[0]] <> 0 and depth[ PointsList[i].pt[1], PointsList[i].pt[0]] < 1000: PointsClusterList.append( [PointsList[i].pt[0], PointsList[i].pt[1]]) Z = np.array(PointsClusterList) if len(Z) < 20: print "No Cups" cv2.imshow("Cups Stream", img) return # convert to np.float32 Z = np.float32(Z) # Determine how many cups there are segregated, centers, distFromCenter, distFromCenterAve1 = Cluster(Z, 1) segregated, centers, distFromCenter, distFromCenterAve2 = Cluster(Z, 2) segregated, centers, distFromCenter, distFromCenterAve3 = Cluster(Z, 3) segregated, centers, distFromCenter, distFromCenterAve4 = Cluster(Z, 4) segregated, centers, distFromCenter, distFromCenterAve5 = Cluster(Z, 5) distFromCenterAveList = [ (sum(distFromCenterAve1) / len(distFromCenterAve1)) * 1.0, (sum(distFromCenterAve2) / len(distFromCenterAve2)) * 2.0, (sum(distFromCenterAve3) / len(distFromCenterAve3)) * 3.0, (sum(distFromCenterAve4) / len(distFromCenterAve4)) * 4.0, (sum(distFromCenterAve5) / len(distFromCenterAve5)) * 5.0 ] groups = distFromCenterAveList.index(min(distFromCenterAveList)) + 1 segregated, centers, distFromCenter, distFromCenterAve = Cluster(Z, groups) #Create List for reduced points segregatedF = [] for i in xrange(groups): segregatedF.append([]) #Remove points which are not close to centroid for j in xrange(groups): for i in range(len(segregated[j])): if distFromCenter[j][i] < filtparam * distFromCenterAve[j]: segregatedF[j].append(segregated[j][i]) #remove clusters that are >= 3 points or all superimposed i = 0 while i < groups: if len(segregatedF[i]) <= 5 or np.isnan(np.std(segregatedF[i])): del segregatedF[i], centers[i], distFromCenter[ i], distFromCenterAve[i] groups -= 1 i += 1 for j in xrange(groups): segregatedF[j] = np.array(segregatedF[j]) # Create a centriod depth list FinalCenters = [] for j in xrange(groups): FinalCenters.append([ centers[j][0], centers[j][1], depth[centers[j][1], centers[j][0]] ]) # Convert to world coordinates FinalCentersWC = convertToWorldCoords(FinalCenters) segregated = segregatedF FC = FinalCenters colourList = [(0, 255, 0), (255, 0, 0), (0, 0, 255), (0, 255, 255), (255, 255, 0), (255, 0, 255)] # Seperate the top of each cup in pixel space depthimg = img.copy() depthmask = depth.copy() # Start Cup Classification loop for j in xrange(groups): centx = FC[j][0] centy = FC[j][1] centdepth = FC[j][2] # Choose pixel area likley to contain a cup w = -0.08811 * centdepth + 103.0837 h = -0.13216 * centdepth + 154.6256 h = h cup1 = depthimg[(centy - h):(centy), (centx - w):(centx + w)] cupDepth1 = depthmask[(centy - h):(FC[j][1]), (centx - w):(centx + w)] # Create blank binary images to fill with depth thresholds shape1 = np.zeros(cupDepth1.shape, dtype=np.uint8) # Fill with threshold depths upper = centdepth + 100 lower = centdepth - 50 depthRange = [] midDepthRange = [] for i in xrange(cupDepth1.shape[0]): for k in xrange(cupDepth1.shape[1]): if lower < cupDepth1[i, k] < upper: shape1[i, k] = cupDepth1[i, k] depthRange.append(cupDepth1[i, k]) if i == cupDepth1.shape[0] - 1: midDepthRange.append(k) if len(midDepthRange) < 3: continue cv2.imshow('s**t', shape1) cv2.waitKey(0) midMin = min(midDepthRange) midMax = max(midDepthRange) s3 = [(centx - w) + midMin, centy, centdepth] s4 = [(centx - w) + midMax, centy, centdepth] mid = [s3, s4] midWorld = convertToWorldCoords(mid) CupMidWidth = midWorld[1][0] - midWorld[0][0] CupTopWidth = max(depthRange) - min(depthRange) print "Mid Width", CupMidWidth print "Top Width", CupTopWidth if CupMidWidth > CupTopWidth: cupOrientation = "Upsidedown" cupFill = "Empty" if CupTopWidth > 100: cupType = "Not a Cup" elif CupTopWidth > 58: cupType = "Large" elif CupTopWidth > 49: cupType = "Medium" elif CupTopWidth > 20: cupType = "Small" else: cupType = "Not a Cup" else: cupOrientation = "Upright" cupFill = "Unsure" if CupTopWidth > 100: cupType = "Not a Cup" elif CupTopWidth > 81: cupType = "Large" elif CupTopWidth > 71: cupType = "Medium" elif CupTopWidth > 30: cupType = "Small" else: cupType = "Not a Cup" #Draw the top of the bounding rectangle if cupType <> "Not a Cup": FinalCentersWC[j].append(cupType) FinalCentersWC[j].append(cupOrientation) FinalCentersWC[j].append(cupFill) # Draw the groups deleteList = [] for j in xrange(groups): if len(FinalCentersWC[j]) > 3: centerst = tuple(np.array(centers[j]) + np.array([0, 50])) cv2.putText(img, str(FinalCentersWC[j]), centerst, cv2.FONT_HERSHEY_SIMPLEX, 0.3, colourList[j]) cv2.circle(img, centers[j], 10, colourList[j], -1) cv2.circle(img, centers[j], 2, (0, 0, 0), -1) for i in range(len(segregated[j])): pt_a = (int(segregated[j][i, 0]), int(segregated[j][i, 1])) cv2.circle(img, pt_a, 3, colourList[j]) cv2.line(img, pt_a, centers[j], colourList[j]) else: deleteList.append(j) FinalFinalCentersWC = [ i for j, i in enumerate(FinalCentersWC) if j not in deleteList ] if save == 1: cv2.imwrite('ProcessedImages/ProcessedCluster' + str(ImageNo) + '.jpg', img) if len(FinalFinalCentersWC) <> 0: print FinalFinalCentersWC cv2.imshow("Cups Stream", img)
def MatchAllCluster(save, maxdist=200, filtparam=2.0): PointsList, DisList, img, depth = MatchAllCapture(0,maxdist) PointsClusterList = [] for i in xrange(len(PointsList)): if depth[PointsList[i].pt[1], PointsList[i].pt[0]] <> 0 and depth[PointsList[i].pt[1], PointsList[i].pt[0]] < 1000: PointsClusterList.append([PointsList[i].pt[0], PointsList[i].pt[1]]) Z = np.array(PointsClusterList) if len(Z) < 30: print "No Cups" cv2.imshow("Cups Stream", img) return # convert to np.float32 Z = np.float32(Z) # Determine how many cups there are segregated, centers, distFromCenter, distFromCenterAve1 = Cluster(Z, 1) segregated, centers, distFromCenter, distFromCenterAve2 = Cluster(Z, 2) segregated, centers, distFromCenter, distFromCenterAve3 = Cluster(Z, 3) segregated, centers, distFromCenter, distFromCenterAve4 = Cluster(Z, 4) segregated, centers, distFromCenter, distFromCenterAve5 = Cluster(Z, 5) distFromCenterAveList = [(sum(distFromCenterAve1)/len(distFromCenterAve1))*1.0, (sum(distFromCenterAve2)/len(distFromCenterAve2))*2.0, (sum(distFromCenterAve3)/len(distFromCenterAve3))*3.0, (sum(distFromCenterAve4)/len(distFromCenterAve4))*4.0, (sum(distFromCenterAve5)/len(distFromCenterAve5))*5.0] groups = distFromCenterAveList.index(min(distFromCenterAveList))+1 segregated, centers, distFromCenter, distFromCenterAve = Cluster(Z, groups) #Create List for reduced points segregatedF = [] for i in xrange(groups): segregatedF.append([]) #Remove points which are not close to centroid for j in xrange(groups): for i in range(len(segregated[j])): if distFromCenter[j][i] < filtparam*distFromCenterAve[j]: segregatedF[j].append(segregated[j][i]) #remove clusters that are >= 3 points or all superimposed i = 0 while i < groups: if len(segregatedF[i]) <= 5 or np.isnan(np.std(segregatedF[i])): del segregatedF[i], centers[i], distFromCenter[i], distFromCenterAve[i] groups -= 1 i += 1 for j in xrange(groups): segregatedF[j] = np.array(segregatedF[j]) # Create a centriod depth list FinalCenters = [] for j in xrange(groups): FinalCenters.append([centers[j][0],centers[j][1],depth[centers[j][1],centers[j][0]]]) # Convert to world coordinates FinalCentersWC = convertToWorldCoords(FinalCenters) segregated = segregatedF FC = FinalCenters colourList=[(0, 255, 0), (255, 0, 0), (0, 0, 255), (0, 255, 255), (255, 255, 0), (255, 0, 255)] # Seperate the top of each cup in pixel space depthimg = img.copy() maskimg = depth.copy() # Start Cup Classification loop for j in xrange(groups): # Choose pixel area likley to contain a cup w = -0.08811*FC[j][2]+103.0837 h = -0.13216*FC[j][2]+154.6256 h = h*1.8 cup1 = depthimg[(FC[j][1]-h):(FC[j][1]), (FC[j][0]-w):(FC[j][0]+w)] mask1 = maskimg[(FC[j][1]-h):(FC[j][1]), (FC[j][0]-w):(FC[j][0]+w)] cup2 = depthimg[(FC[j][1]):(FC[j][1]+h), (FC[j][0]-w):(FC[j][0]+w)] mask2 = maskimg[(FC[j][1]):(FC[j][1]+h), (FC[j][0]-w):(FC[j][0]+w)] # Determine the bouding rectangle of the largest contour in the top area gray = cv2.cvtColor(cup1,cv2.COLOR_BGR2GRAY) blur = cv2.GaussianBlur(gray,(5,5),0) blurDepth = cv2.blur(mask1,(5,5)) thresh1 = 200 thresh2 = 400 for i in xrange(cup1.shape[0]): for j in xrange(cup1.shape[1]): if blurDepth[i,j] == 0 or blurDepth[i,j]>1000: gray[i,j] = 0 edges = cv2.Canny(gray,thresh1,thresh2) contours,hierarchy = cv2.findContours(edges,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) if len(contours) == 0: print "No Top Contours" return else: cnt = max(contours, key=len) x,y,w1,h1 = cv2.boundingRect(cnt) # Determine the bouding rectangle of the largest contour in the bottom area gray2 = cv2.cvtColor(cup2,cv2.COLOR_BGR2GRAY) blur2 = cv2.GaussianBlur(gray2,(5,5),0) thresh21 = 200 thresh22 = 400 edges2 = cv2.Canny(blur2,thresh21,thresh22) contours2,hierarchy2 = cv2.findContours(edges2,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) if len(contours2) == 0: print "No Bottom Contours" return else: cnt2 = max(contours2, key=len) x2,y2,w21,h21 = cv2.boundingRect(cnt2) # Use brounding rectangle size to determine cup type and orientation s1 = [(FC[j][0]-w)+x,(FC[j][1]-h)+ y,FC[j][2]] s2 = [(FC[j][0]-w)+x+w1,(FC[j][1]-h)+ y,FC[j][2]] s3 = [(FC[j][0]-w)+x2,(FC[j][1])+ y2 + h21,FC[j][2]] s4 = [(FC[j][0]-w)+x2+w21,(FC[j][1])+ y2 + h21,FC[j][2]] top = [s1,s2] bottom = [s3,s4] topWorld = convertToWorldCoords(top) bottomWorld = convertToWorldCoords(bottom) CupTopWidth = topWorld[1][0]-topWorld[0][0] CupBottomWidth = bottomWorld[1][0]-bottomWorld[0][0] if CupBottomWidth > CupTopWidth: cupOrientation = "Upsidedown" cupFill = "Empty" CupTopWidth = CupBottomWidth if CupTopWidth > 100: cupType = "Not a Cup" elif CupTopWidth > 84.5: cupType = "Large" elif CupTopWidth > 71: cupType = "Medium" elif CupTopWidth > 50: cupType = "Small" else: cupType = "Not a Cup" else: cupOrientation = "Upright" cupFill = "Unsure" if CupTopWidth > 100: cupType = "Not a Cup" elif CupTopWidth > 84.5: cupType = "Large" elif CupTopWidth > 71: cupType = "Medium" elif CupTopWidth > 50: cupType = "Small" else: cupType = "Not a Cup" #Draw the top of the bounding rectangle if cupType <> "Not a Cup": new_cnt = cnt + [int(round((FC[j][0]-w),0)),int(round((FC[j][1]-h),0))] cv2.line(img,(int(round(s1[0],0)),int(round(s1[1],0))),(int(round(s2[0],0)),int(round(s2[1],0))),colourList[j]) cv2.drawContours(img,[new_cnt],0,colourList[j],2) new_cnt2 = cnt2 + [int(round((FC[j][0]-w),0)),int(round((FC[j][1]),0))] cv2.line(img,(int(round(s3[0],0)),int(round(s3[1],0))),(int(round(s4[0],0)),int(round(s4[1],0))),colourList[j]) cv2.line(img,(int(round(s3[0],0)),int(round(s3[1],0))),(int(round(s1[0],0)),int(round(s1[1],0))),colourList[j]) cv2.line(img,(int(round(s4[0],0)),int(round(s4[1],0))),(int(round(s2[0],0)),int(round(s2[1],0))),colourList[j]) cv2.drawContours(img,[new_cnt2],0,colourList[j],2) FinalCentersWC[j].append(cupType) FinalCentersWC[j].append(cupOrientation) FinalCentersWC[j].append(cupFill) # Draw the groups deleteList = [] for j in xrange(groups): if len(FinalCentersWC[j]) > 3: centerst = tuple(np.array(centers[j])+np.array([0,50])) cv2.putText(img,str(FinalCentersWC[j]), centerst, cv2.FONT_HERSHEY_SIMPLEX, 0.3, colourList[j]) cv2.circle(img, centers[j], 10, colourList[j], -1) cv2.circle(img, centers[j], 2, (0,0,0), -1) for i in range(len(segregated[j])): pt_a = (int(segregated[j][i,0]), int(segregated[j][i,1])) cv2.circle(img, pt_a, 3, colourList[j]) else: deleteList.append(j) FinalFinalCentersWC = [i for j, i in enumerate(FinalCentersWC) if j not in deleteList] if save == 1: cv2.imwrite('ProcessedImages/ProcessedCluster'+str(ImageNo)+'.jpg', img) if len(FinalFinalCentersWC)<>0: print FinalFinalCentersWC cv2.imshow("Cups Stream", img)
def MatchAllClusterGlass(save, maxdist=100, filtparam=2.0): PointsList, DisList, img, depth = MatchAllCaptureGlass(0, maxdist) PointsClusterList = [] for i in xrange(len(PointsList)): if True: PointsClusterList.append( [PointsList[i].pt[0], PointsList[i].pt[1]]) Z = np.array(PointsClusterList) if len(Z) < 20: print "No Cups" cv2.imshow("Cups Stream", img) return # convert to np.float32 Z = np.float32(Z) # Determine how many cups there are segregated, centers, distFromCenter, distFromCenterAve1 = Cluster(Z, 1) segregated, centers, distFromCenter, distFromCenterAve2 = Cluster(Z, 2) segregated, centers, distFromCenter, distFromCenterAve3 = Cluster(Z, 3) splitTend = 0.2 distFromCenterAveList = [ (sum(distFromCenterAve1) / len(distFromCenterAve1)) * 1.0 * splitTend, (sum(distFromCenterAve2) / len(distFromCenterAve2)) * 2.0 * splitTend, (sum(distFromCenterAve3) / len(distFromCenterAve3)) * 3.0 * splitTend ] groups = distFromCenterAveList.index(min(distFromCenterAveList)) + 1 segregated, centers, distFromCenter, distFromCenterAve = Cluster(Z, groups) #Create List for reduced points segregatedF = [] for i in xrange(groups): segregatedF.append([]) #Remove points which are not close to centroid for j in xrange(groups): for i in range(len(segregated[j])): if distFromCenter[j][i] < filtparam * distFromCenterAve[j]: segregatedF[j].append(segregated[j][i]) #remove clusters that are >= 3 points or all superimposed i = 0 while i < groups: if len(segregatedF[i]) <= 25 or np.isnan(np.std(segregatedF[i])): del segregatedF[i], centers[i], distFromCenter[ i], distFromCenterAve[i] groups -= 1 i += 1 for j in xrange(groups): segregatedF[j] = np.array(segregatedF[j]) # Create a centriod depth list FinalCenters = [] for j in xrange(groups): FinalCenters.append([ centers[j][0], centers[j][1], depth[centers[j][1], centers[j][0]] ]) # Convert to world coordinates FinalCentersWC = convertToWorldCoords(FinalCenters) segregated = segregatedF FC = FinalCenters colourList = [(0, 255, 0), (255, 0, 0), (0, 0, 255), (0, 255, 255), (255, 255, 0), (255, 0, 255)] print FC """ # Seperate the top of each cup in pixel space depthimg = img.copy() depthmask = depth.copy() # Start Cup Classification loop for j in xrange(groups): centx = FC[j][0] centy = FC[j][1] centdepth = FC[j][2] # Choose pixel area likley to contain a cup w = -0.08811*centdepth+103.0837 h = -0.13216*centdepth+154.6256 h = h cup1 = depthimg[(centy-h):(centy), (centx-w):(centx+w)] cup11 = np.copy(cup1) cupDepth1 = depthmask[(centy-h):(FC[j][1]), (centx-w):(centx+w)] # Create blank binary images to fill with depth thresholds shape1 = np.zeros(cupDepth1.shape,dtype=np.uint8) shape2 = np.zeros(cupDepth1.shape,dtype=np.uint8) # Fill with threshold depths upper = centdepth+100 lower = centdepth-50 depthRange = [] depthRangePos = [] midDepthRange = [] for i in xrange(cupDepth1.shape[0]): for k in xrange(cupDepth1.shape[1]): if lower<cupDepth1[i,k]<upper: shape1[i,k] = cupDepth1[i,k] depthRange.append(cupDepth1[i,k]) depthRangePos.append([k,i]) if i == cupDepth1.shape[0]-1: midDepthRange.append(k) if len(midDepthRange) < 3: continue Maxpos = depthRangePos[depthRange.index(max(depthRange))] Minpos = depthRangePos[depthRange.index(min(depthRange))] Cutoff = max(Maxpos[1],Minpos[1]) midMin = min(midDepthRange) midMax = max(midDepthRange) s3 = [(centx-w)+midMin,centy,centdepth] s4 = [(centx-w)+midMax,centy,centdepth] mid = [s3,s4] midWorld = convertToWorldCoords(mid) CupMidWidth = midWorld[1][0]-midWorld[0][0] CupTopWidth = max(depthRange)-min(depthRange) cv2.circle(cup11, tuple(Maxpos), 3, colourList[j+1]) cv2.circle(cup11, tuple(Minpos), 3, colourList[j+1]) cv2.line(cup11, tuple(Maxpos), tuple(Minpos), colourList[j+1]) #cv2.imshow('MaxMin',cup11) for i in xrange(Cutoff): for k in xrange(cupDepth1.shape[1]): if lower<cupDepth1[i,k]<upper: shape2[i,k] = 255 #cv2.imshow('thresh',shape2) blurThresh = cv2.blur(shape2,(5,5)) #cv2.imshow('blur',blurThresh) thresh1 = 200 thresh2 = 300 edges = cv2.Canny(blurThresh,thresh1,thresh2) #cv2.imshow('edges',edges) contours,hierarchy = cv2.findContours(edges,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) if len(contours)==0: continue cont = np.vstack(contours) hull = cv2.convexHull(cont) cupGray = cv2.cvtColor(cup1,cv2.COLOR_BGR2GRAY) ColourAverage = np.array([0,0,0]) count = 0 for i in xrange(Cutoff): for k in xrange(cupDepth1.shape[1]): if cv2.pointPolygonTest(hull,(k,i), False)>0: ColourAverage += np.array(cupGray[i,k]) count += 1 ColourAverageF = ColourAverage/count whiteColour = max(cupGray[Maxpos[1],Maxpos[0]], cupGray[Minpos[1],Minpos[0]]) fillRatio = float(ColourAverageF[0])/whiteColour #cv2.drawContours(cup1,[hull],0,colourList[j],2) #cv2.imshow('hull',cup1) #cv2.waitKey(0) print "mid width",CupMidWidth print "top width",CupTopWidth if CupMidWidth > CupTopWidth: cupOrientation = "Upsidedown" cupFill = "Empty" if CupTopWidth > 100: cupType = "Not a Cup" elif CupTopWidth > 58: cupType = "Large" elif CupTopWidth > 49: cupType = "Medium" elif CupTopWidth > 20: cupType = "Small" else: cupType = "Not a Cup" else: cupOrientation = "Upright" if fillRatio < 0.8: cupFill = "Full" else: cupFill = "Empty" if CupTopWidth > 100: cupType = "Not a Cup" elif CupTopWidth > 81: cupType = "Large" elif CupTopWidth > 69: cupType = "Medium" elif CupTopWidth > 30: cupType = "Small" else: cupType = "Not a Cup" #Draw the top of the bounding rectangle if cupType <> "Not a Cup": FinalCentersWC[j].append(cupType) FinalCentersWC[j].append(cupOrientation) FinalCentersWC[j].append(cupFill) """ # Draw the groups deleteList = [] for j in xrange(groups): if len(FinalCentersWC[j]) > 1: centerst = tuple(np.array(centers[j]) + np.array([0, 50])) cv2.putText(img, str(FinalCentersWC[j]), centerst, cv2.FONT_HERSHEY_SIMPLEX, 0.3, colourList[j]) cv2.circle(img, centers[j], 10, colourList[j], -1) cv2.circle(img, centers[j], 2, (0, 0, 0), -1) for i in range(len(segregated[j])): pt_a = (int(segregated[j][i, 0]), int(segregated[j][i, 1])) cv2.circle(img, pt_a, 3, colourList[j]) cv2.line(img, pt_a, centers[j], colourList[j]) rpt1 = tuple(segregated[j].min(axis=0)) rpt2 = tuple(segregated[j].max(axis=0)) cv2.rectangle(img, rpt1, rpt2, colourList[j]) else: deleteList.append(j) FinalFinalCentersWC = [ i for j, i in enumerate(FinalCentersWC) if j not in deleteList ] if save == 1: cv2.imwrite('ProcessedImages/ProcessedCluster' + str(ImageNo) + '.jpg', img) if len(FinalFinalCentersWC) <> 0: print FinalFinalCentersWC cv2.imshow("Cups Stream", img)
def MatchAllCluster(save, maxdist=200, filtparam=2.0, SplitTend=0.8): PointsList, DisList, img, depth = MatchAllCapture(0, maxdist) PointsClusterList = [] for i in xrange(len(PointsList)): if depth[PointsList[i].pt[1], PointsList[i].pt[0]] <> 0 and depth[ PointsList[i].pt[1], PointsList[i].pt[0]] < 1000: PointsClusterList.append( [PointsList[i].pt[0], PointsList[i].pt[1]]) Z = np.array(PointsClusterList) if len(Z) < 50: print "No Cups" cv2.imshow("Cups Stream", img) return # convert to np.float32 Z = np.float32(Z) # Determine how many cups there are segregated, centers, distFromCenter, distFromCenterAve1 = Cluster(Z, 1) segregated, centers, distFromCenter, distFromCenterAve2 = Cluster(Z, 2) segregated, centers, distFromCenter, distFromCenterAve3 = Cluster(Z, 3) segregated, centers, distFromCenter, distFromCenterAve4 = Cluster(Z, 4) segregated, centers, distFromCenter, distFromCenterAve5 = Cluster(Z, 5) distFromCenterAveList = [ (sum(distFromCenterAve1) / len(distFromCenterAve1)) * 1.0, (sum(distFromCenterAve2) / len(distFromCenterAve2)) * (1.0 + 1.0 * SplitTend), (sum(distFromCenterAve3) / len(distFromCenterAve3)) * (1.0 + 2.0 * SplitTend), (sum(distFromCenterAve4) / len(distFromCenterAve4)) * (1.0 + 3.0 * SplitTend), (sum(distFromCenterAve5) / len(distFromCenterAve5)) * (1.0 + 4.0 * SplitTend) ] groups = distFromCenterAveList.index(min(distFromCenterAveList)) + 1 segregated, centers, distFromCenter, distFromCenterAve = Cluster(Z, groups) #remove clusters that are <= 20 points or all superimposed i = 0 while i < groups: if len(segregated[i]) <= 20 or np.isnan(np.std(segregated[i])): del segregated[i], centers[i], distFromCenter[ i], distFromCenterAve[i] groups -= 1 i += 1 #Create List for reduced points segregatedF = [] for i in xrange(groups): segregatedF.append([]) #Remove points which are not close to centroid for j in xrange(groups): for i in range(len(segregated[j])): if distFromCenter[j][i] < filtparam * distFromCenterAve[j]: segregatedF[j].append(segregated[j][i]) for j in xrange(groups): segregatedF[j] = np.array(segregatedF[j]) # Create a centriod depth list FinalCenters = [] for j in xrange(groups): FinalCenters.append([ centers[j][0], centers[j][1], depth[centers[j][1], centers[j][0]] ]) # Convert to world and then object coordinates FinalCentersCC = convertToWorldCoords(FinalCenters) Corners = np.load('CalibrationImages/Caliboutput/corners.npy') PixCorners = np.load('CalibrationImages/Caliboutput/PixCorners.npy') FinalCentersWC = transformCoords(FinalCentersCC, Corners) #Draw the coordinate system cv2.line(img, tuple(PixCorners[1][:2]), tuple(PixCorners[0][:2]), (255, 0, 0), 3) cv2.line(img, tuple(PixCorners[1][:2]), tuple(PixCorners[2][:2]), (0, 0, 255), 3) segregated = segregatedF FC = FinalCenters colourList = [(0, 255, 0), (255, 0, 0), (0, 0, 255), (0, 255, 255), (255, 255, 0), (255, 0, 255)] # Seperate the top of each cup in pixel space depthimg = img.copy() depthmask = depth.copy() CupMidTopPixList = [] # Start Cup Classification loop for j in xrange(groups): centx = FC[j][0] centy = FC[j][1] centdepth = FC[j][2] # Choose pixel area likley to contain a cup w = -0.08811 * centdepth + 103.0837 h = -0.13216 * centdepth + 154.6256 h = h * 1.3 cup1 = depthimg[(centy - h):(centy), (centx - w):(centx + w)] cup11 = np.copy(cup1) cupDepth1 = depthmask[(centy - h):(FC[j][1]), (centx - w):(centx + w)] cupDepth2 = np.copy(cupDepth1) # Create blank binary images to fill with depth thresholds shape1 = np.zeros(cupDepth1.shape, dtype=np.uint8) shape2 = np.zeros(cupDepth1.shape, dtype=np.uint8) shape3 = np.zeros(cupDepth1.shape, dtype=np.uint8) #Show the voids within the ROI for i in xrange(cupDepth1.shape[0]): for k in xrange(cupDepth1.shape[1]): if cupDepth2[i, k] == 0: shape3[i, k] = 255 #cv2.imshow('depth',shape3) #Find the edges of the middle of the cup using the voids try: del midLeft except NameError: pass i = 0 Flag = True while Flag == True and i < cupDepth1.shape[0]: for k in xrange(cupDepth1.shape[1] / 2, 0, -1): if cupDepth2[cupDepth1.shape[0] - 1 - i, k] == 0: midLeft = k + 1 Flag = False break i += 1 #Catch non cups try: midLeft except NameError: #print "well, midLeft WASN'T defined after all!" continue leftDepth = cupDepth2[cupDepth1.shape[0] - 1 - i, midLeft] try: del midRight except NameError: pass i = 0 Flag = True while Flag == True and i < cupDepth1.shape[0]: for k in xrange(cupDepth1.shape[1] / 2, cupDepth1.shape[1]): if cupDepth2[cupDepth1.shape[0] - 1 - i, k] == 0: midRight = k - 1 Flag = False break i += 1 try: midRight except NameError: #print "well, midRight WASN'T defined after all!" continue rightDepth = cupDepth2[cupDepth1.shape[0] - 1 - i, midRight] # Fill with threshold depths upper = centdepth + 100 lower = centdepth - 50 depthRange = [] depthRangePos = [] for i in xrange(cupDepth1.shape[0]): for k in xrange(cupDepth1.shape[1]): if lower < cupDepth1[i, k] < upper: shape1[i, k] = cupDepth1[i, k] depthRange.append(cupDepth1[i, k]) depthRangePos.append([k, i]) MinDepth = min(depthRange) Minpos = depthRangePos[depthRange.index(min(depthRange))] Cutoff = Minpos[1] for i in xrange(Cutoff): for k in xrange(cupDepth1.shape[1]): if lower < cupDepth1[i, k] < upper: shape2[i, k] = 255 #cv2.imshow('thresh',shape2) q = 0 runFlag = True while runFlag is True and q < Cutoff: for p in xrange(cupDepth1.shape[1]): if shape2[q, p] == 255: Maxpos = [p, q] MaxDepth = cupDepth1[q, p] runFlag = False break q += 1 try: Maxpos except NameError: #print "well, Maxpos WASN'T defined after all!" continue cv2.circle(cup11, tuple(Maxpos), 3, colourList[j + 1]) cv2.circle(cup11, tuple(Minpos), 3, colourList[j + 1]) cv2.line(cup11, tuple(Maxpos), tuple(Minpos), colourList[j + 1]) cv2.circle(cup11, (midLeft, cupDepth1.shape[0]), 3, colourList[j + 1]) cv2.circle(cup11, (midRight, cupDepth1.shape[0]), 3, colourList[j + 1]) cv2.line(cup11, (midLeft, cupDepth1.shape[0]), (midLeft, cupDepth1.shape[0]), colourList[j + 1]) #cv2.imshow('MaxMin',cup11) globMin0 = int(round(centx - w + Minpos[0], 0)) globMin1 = int(round(centy - h + Minpos[1], 0)) globMax0 = int(round(centx - w + Maxpos[0], 0)) globMax1 = int(round(centy - h + Maxpos[1], 0)) globLeft = int(round(centx - w + midLeft, 0)) globRight = int(round(centx - w + midRight, 0)) CupMidTopPix = np.mean([[globMin0, globMin1], [globMax0, globMax1]], axis=0) CupMidTopPix = [int(CupMidTopPix[0]), int(CupMidTopPix[1])] CupMidTopPixList.append(CupMidTopPix) """ #Draw cup key points cv2.circle(img, (globMax0,globMax1), 2, (255,0,0), -1) cv2.circle(img, (globMin0,globMin1), 2, (255,0,0), -1) cv2.circle(img, (globLeft,centy), 5, (255,0,0), -1) cv2.circle(img, (globRight,centy), 5, (255,0,0), -1) """ FrontTopCup = [globMin0, globMin1, MinDepth] BackTopCup = [globMax0, globMax1, MaxDepth] LeftCup = [globLeft, centy, leftDepth] RightCup = [globRight, centy, rightDepth] CupTopPoints = convertToWorldCoords([FrontTopCup, BackTopCup]) CupTopWidth = np.linalg.norm( np.array(CupTopPoints[0]) - np.array(CupTopPoints[1])) CupMidPoints = convertToWorldCoords([LeftCup, RightCup]) CupMidWidth = np.linalg.norm( np.array(CupMidPoints[0]) - np.array(CupMidPoints[1])) CupMidTop = [np.mean(CupTopPoints, axis=0)] CupMidTopWorld = transformCoords(CupMidTop, Corners) blurThresh = cv2.blur(shape2, (5, 5)) #cv2.imshow('blur',blurThresh) thresh1 = 200 thresh2 = 300 edges = cv2.Canny(blurThresh, thresh1, thresh2) #cv2.imshow('edges',edges) contours, hierarchy = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) if len(contours) == 0: continue cont = np.vstack(contours) hull = cv2.convexHull(cont) cupGray = cv2.cvtColor(cup1, cv2.COLOR_BGR2GRAY) #cv2.imshow('gray',cupGray) ColourAverage = np.array([0, 0, 0]) count = 0 for i in xrange(Cutoff): for k in xrange(cupDepth1.shape[1]): if cv2.pointPolygonTest(hull, (k, i), False) > 0: ColourAverage += np.array(cupGray[i, k]) count += 1 ColourAverageF = ColourAverage / count whiteColour = max(cupGray[Maxpos[1], Maxpos[0]], cupGray[Minpos[1], Minpos[0]]) fillRatio = float(ColourAverageF[0]) / whiteColour cv2.drawContours(cup1, [hull], 0, colourList[j], 2) cv2.imshow('hull', cup1) #print "CupTopWidth",CupTopWidth,"CupMidWidth",CupMidWidth if CupMidWidth > CupTopWidth: cupOrientation = "Upsidedown" cupFill = "Empty" if CupTopWidth > 100: cupType = "Not a Cup" elif CupTopWidth > 58: cupType = "Large" elif CupTopWidth > 49: cupType = "Medium" elif CupTopWidth > 20: cupType = "Small" else: cupType = "Not a Cup" else: cupOrientation = "Upright" if fillRatio < 0.8: cupFill = "Full" else: cupFill = "Empty" if CupTopWidth > 100: cupType = "Not a Cup" elif CupTopWidth > 81: cupType = "Large" elif CupTopWidth > 69: cupType = "Medium" elif CupTopWidth > 30: cupType = "Small" else: cupType = "Not a Cup" #Draw the top of the bounding rectangle if cupType <> "Not a Cup": FinalCentersWC[j][0:3] = CupMidTopWorld[0] FinalCentersWC[j].append(cupType) FinalCentersWC[j].append(cupOrientation) FinalCentersWC[j].append(cupFill) # Draw the groups deleteList = [] for j in xrange(groups): if len(FinalCentersWC[j]) > 3: centerst = tuple(np.array(centers[j]) + np.array([0, 50])) cv2.putText(img, str(FinalCentersWC[j]), centerst, cv2.FONT_HERSHEY_SIMPLEX, 0.3, colourList[j]) for i in range(len(segregated[j])): pt_a = (int(segregated[j][i, 0]), int(segregated[j][i, 1])) cv2.circle(img, pt_a, 3, colourList[j]) cv2.line(img, pt_a, centers[j], colourList[j]) if j >= len(CupMidTopPixList): continue cv2.circle(img, tuple(CupMidTopPixList[j]), 10, colourList[j], -1) cv2.circle(img, tuple(CupMidTopPixList[j]), 2, (0, 0, 0), -1) cv2.circle(img, centers[j], 2, (0, 0, 0), -1) else: deleteList.append(j) FinalFinalCentersWC = [ i for j, i in enumerate(FinalCentersWC) if j not in deleteList ] if save == 1: cv2.imwrite('ProcessedImages/ProcessedCluster' + str(ImageNo) + '.jpg', img) if len(FinalFinalCentersWC) <> 0: print FinalFinalCentersWC cv2.imshow("Cups Stream", img)
def MatchAllCluster(save, maxdist=200, filtparam=2.0): PointsList, DisList, img, depth = MatchAllCapture(0, maxdist) PointsClusterList = [] for i in xrange(len(PointsList)): if depth[PointsList[i].pt[1], PointsList[i].pt[0]] <> 0 and depth[ PointsList[i].pt[1], PointsList[i].pt[0]] < 1000: PointsClusterList.append( [PointsList[i].pt[0], PointsList[i].pt[1]]) Z = np.array(PointsClusterList) print "points list length ", len(Z) if len(Z) < 30: print "holy ducking shit" # convert to np.float32 Z = np.float32(Z) # Determine how many cups there are segregated, centers, distFromCenter, distFromCenterAve1 = Cluster(Z, 1) segregated, centers, distFromCenter, distFromCenterAve2 = Cluster(Z, 2) segregated, centers, distFromCenter, distFromCenterAve3 = Cluster(Z, 3) segregated, centers, distFromCenter, distFromCenterAve4 = Cluster(Z, 4) segregated, centers, distFromCenter, distFromCenterAve5 = Cluster(Z, 5) distFromCenterAveList = [ (sum(distFromCenterAve1) / len(distFromCenterAve1)) * 1.0, (sum(distFromCenterAve2) / len(distFromCenterAve2)) * 2.0, (sum(distFromCenterAve3) / len(distFromCenterAve3)) * 3.0, (sum(distFromCenterAve4) / len(distFromCenterAve4)) * 4.0, (sum(distFromCenterAve5) / len(distFromCenterAve5)) * 5.0 ] groups = distFromCenterAveList.index(min(distFromCenterAveList)) + 1 segregated, centers, distFromCenter, distFromCenterAve = Cluster(Z, groups) #Create List for reduced points segregatedF = [] for i in xrange(groups): segregatedF.append([]) #Remove points which are not close to centroid for j in xrange(groups): for i in range(len(segregated[j])): if distFromCenter[j][i] < filtparam * distFromCenterAve[j]: segregatedF[j].append(segregated[j][i]) for j in xrange(groups): segregatedF[j] = np.array(segregatedF[j]) print "-" * 20 print "groups ", groups #remove clusters that are 3 points or smaller for i in xrange(groups): print len(segregatedF[i]) print "std ", np.std(segregatedF[j]) if len(segregatedF[i]) <= 5 or np.isnan(np.std(segregatedF[j])): print "HOLY F*****G SHIT" print "-" * 20 # Create a centriod depth list FinalCenters = [] for j in xrange(groups): FinalCenters.append([ centers[j][0], centers[j][1], depth[centers[j][1], centers[j][0]] ]) # Convert to world coordinates FinalCentersWC = convertToWorldCoords(FinalCenters) return segregatedF, centers, img, depth, FinalCenters, FinalCentersWC, groups
def MatchAllCluster(save, maxdist=200, filtparam=2.0): PointsList, DisList, img, depth = MatchAllCapture(0, maxdist) PointsClusterList = [] for i in xrange(len(PointsList)): if depth[PointsList[i].pt[1], PointsList[i].pt[0]] <> 0 and depth[ PointsList[i].pt[1], PointsList[i].pt[0]] < 1000: PointsClusterList.append( [PointsList[i].pt[0], PointsList[i].pt[1]]) Z = np.array(PointsClusterList) if len(Z) < 30: print "No Cups" return # convert to np.float32 Z = np.float32(Z) # Determine how many cups there are segregated, centers, distFromCenter, distFromCenterAve1 = Cluster(Z, 1) segregated, centers, distFromCenter, distFromCenterAve2 = Cluster(Z, 2) segregated, centers, distFromCenter, distFromCenterAve3 = Cluster(Z, 3) segregated, centers, distFromCenter, distFromCenterAve4 = Cluster(Z, 4) segregated, centers, distFromCenter, distFromCenterAve5 = Cluster(Z, 5) distFromCenterAveList = [ (sum(distFromCenterAve1) / len(distFromCenterAve1)) * 1.0, (sum(distFromCenterAve2) / len(distFromCenterAve2)) * 2.0, (sum(distFromCenterAve3) / len(distFromCenterAve3)) * 3.0, (sum(distFromCenterAve4) / len(distFromCenterAve4)) * 4.0, (sum(distFromCenterAve5) / len(distFromCenterAve5)) * 5.0 ] groups = distFromCenterAveList.index(min(distFromCenterAveList)) + 1 segregated, centers, distFromCenter, distFromCenterAve = Cluster(Z, groups) #Create List for reduced points segregatedF = [] for i in xrange(groups): segregatedF.append([]) #Remove points which are not close to centroid for j in xrange(groups): for i in range(len(segregated[j])): if distFromCenter[j][i] < filtparam * distFromCenterAve[j]: segregatedF[j].append(segregated[j][i]) #remove clusters that are >= 3 points or all superimposed i = 0 while i < groups: if len(segregatedF[i]) <= 5 or np.isnan(np.std(segregatedF[i])): del segregatedF[i], centers[i], distFromCenter[ i], distFromCenterAve[i] groups -= 1 i += 1 for j in xrange(groups): segregatedF[j] = np.array(segregatedF[j]) # Create a centriod depth list FinalCenters = [] for j in xrange(groups): FinalCenters.append([ centers[j][0], centers[j][1], depth[centers[j][1], centers[j][0]] ]) # Convert to world coordinates FinalCentersWC = convertToWorldCoords(FinalCenters) segregated = segregatedF FC = FinalCenters colourList = [(0, 255, 0), (255, 0, 0), (0, 0, 255), (0, 255, 255), (255, 255, 0), (255, 0, 255)] # Seperate the top of each cup in pixel space depthimg = img.copy() depthmask = depth.copy() # Start Cup Classification loop for j in xrange(groups): centx = FC[j][0] centy = FC[j][1] centdepth = FC[j][2] # Choose pixel area likley to contain a cup w = -0.08811 * centdepth + 103.0837 h = -0.13216 * centdepth + 154.6256 h = h cup1 = depthimg[(centy - h):(centy), (centx - w):(centx + w)] cupDepth1 = depthmask[(centy - h):(FC[j][1]), (centx - w):(centx + w)] cup2 = depthimg[(centy):(centy + h / 4), (centx - w):(centx + w)] cupDepth2 = depthmask[(centy):(centy + h / 4), (centx - w):(centx + w)] # Create blank binary images to fill with depth thresholds shape1 = np.zeros(cupDepth1.shape, dtype=np.uint8) shape2 = np.zeros(cupDepth2.shape, dtype=np.uint8) # Colour in upper threshold depths upper = centdepth + 80 lower = centdepth - 30 for i in xrange(cupDepth1.shape[0]): for k in xrange(cupDepth1.shape[1]): if lower < cupDepth1[i, k] < upper: shape1[i, k] = 255 # Colour in upper threshold depths upper = centdepth + 80 lower = centdepth for i in xrange(cupDepth2.shape[0]): for k in xrange(cupDepth2.shape[1]): if lower < cupDepth2[i, k] < upper: shape2[i, k] = 255 #cv2.imshow('depth',shape2) #cv2.waitKey(0) # Apply a median filter to the depth thresholds shape1blur = cv2.blur(shape1, (5, 5)) shape2blur = cv2.blur(shape2, (5, 5)) #cv2.imshow('blur',shape2blur) #cv2.waitKey(0) # Determine the bouding rectangle of the largest contour in the top area thresh1 = 200 thresh2 = 400 edges = cv2.Canny(shape1blur, thresh1, thresh2) contours, hierarchy = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) cont = np.vstack(contours) if len(cont) == 0: print "No Top Contours" return else: hull = cv2.convexHull(cont) x, y, w1, h1 = cv2.boundingRect(hull) # Determine the bouding rectangle of the largest contour in the bottom area thresh21 = 200 thresh22 = 400 edges2 = cv2.Canny(shape2blur, thresh21, thresh22) #cv2.imshow('edges',edges2) #cv2.waitKey(0) contours2, hierarchy2 = cv2.findContours(edges2, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) for cnt in contours2: cv2.drawContours(cup2, [cnt], 0, colourList[j], 2) #cv2.imshow('cnt',cup2) #cv2.waitKey(0) cont2 = np.vstack(contours2) if len(cont2) == 0: print "No Bottom Contours" return else: hull2 = cv2.convexHull(cont2) x2, y2, w21, h21 = cv2.boundingRect(hull2) #cv2.drawContours(cup2,[hull2],0,colourList[j+1],2) #cv2.imshow('cntMax',cup2) #cv2.waitKey(0) # Use brounding rectangle size to determine cup type and orientation s1 = [(centx - w) + x, (centy - h) + y, centdepth] s2 = [(centx - w) + x + w1, (centy - h) + y, centdepth] s3 = [(centx - w) + x2, (centy) + y2 + h21, centdepth] s4 = [(centx - w) + x2 + w21, (centy) + y2 + h21, centdepth] top = [s1, s2] bottom = [s3, s4] topWorld = convertToWorldCoords(top) bottomWorld = convertToWorldCoords(bottom) CupTopWidth = topWorld[1][0] - topWorld[0][0] CupBottomWidth = bottomWorld[1][0] - bottomWorld[0][0] if CupBottomWidth > CupTopWidth: cupOrientation = "Upsidedown" cupFill = "Empty" CupTopWidth = CupBottomWidth if CupTopWidth > 100: cupType = "Not a Cup" elif CupTopWidth > 84.5: cupType = "Large" elif CupTopWidth > 71: cupType = "Medium" elif CupTopWidth > 50: cupType = "Small" else: cupType = "Not a Cup" else: cupOrientation = "Upright" cupFill = "Unsure" print CupTopWidth if CupTopWidth > 100: cupType = "Not a Cup" elif CupTopWidth > 79: cupType = "Large" elif CupTopWidth > 60: cupType = "Medium" elif CupTopWidth > 50: cupType = "Small" else: cupType = "Not a Cup" #Draw the top of the bounding rectangle if cupType <> "Not a Cup": new_cnt = hull + [ int(round((centx - w), 0)), int(round((centy - h), 0)) ] cv2.line(img, (int(round(s1[0], 0)), int(round(s1[1], 0))), (int(round(s2[0], 0)), int(round(s2[1], 0))), colourList[j]) cv2.drawContours(img, [new_cnt], 0, colourList[j], 2) new_cnt2 = hull2 + [ int(round((FC[j][0] - w), 0)), int(round((FC[j][1]), 0)) ] cv2.line(img, (int(round(s3[0], 0)), int(round(s3[1], 0))), (int(round(s4[0], 0)), int(round(s4[1], 0))), colourList[j]) cv2.line(img, (int(round(s3[0], 0)), int(round(s3[1], 0))), (int(round(s1[0], 0)), int(round(s1[1], 0))), colourList[j]) cv2.line(img, (int(round(s4[0], 0)), int(round(s4[1], 0))), (int(round(s2[0], 0)), int(round(s2[1], 0))), colourList[j]) cv2.drawContours(img, [new_cnt2], 0, colourList[j], 2) FinalCentersWC[j].append(cupType) FinalCentersWC[j].append(cupOrientation) FinalCentersWC[j].append(cupFill) # Draw the groups deleteList = [] for j in xrange(groups): if len(FinalCentersWC[j]) > 3: centerst = tuple(np.array(centers[j]) + np.array([0, 50])) cv2.putText(img, str(FinalCentersWC[j]), centerst, cv2.FONT_HERSHEY_SIMPLEX, 0.3, colourList[j]) cv2.circle(img, centers[j], 10, colourList[j], -1) cv2.circle(img, centers[j], 2, (0, 0, 0), -1) for i in range(len(segregated[j])): pt_a = (int(segregated[j][i, 0]), int(segregated[j][i, 1])) cv2.circle(img, pt_a, 3, colourList[j]) else: deleteList.append(j) FinalFinalCentersWC = [ i for j, i in enumerate(FinalCentersWC) if j not in deleteList ] if save == 1: cv2.imwrite('ProcessedImages/ProcessedCluster' + str(ImageNo) + '.jpg', img) if len(FinalFinalCentersWC) <> 0: print FinalFinalCentersWC cv2.imshow("Cups Stream", img)