def faceProcess(self):
trackedData = self.getTrackedData()
if trackedData.has_key('data') == False:
return
t = trackedData['t']
points = trackedData['data']
rgbImgVec = self.kinectClient.getRGBImgVec()
faceAngleImg = self.kinectFloorMap.getBlankImg(dtype=np.uint16)
####### only one kinect (so far)
kinectIdx = 0
rgbImgOrig = rgbImgVec[kinectIdx]
if rgbImgOrig == None:
return
rgbImg = rgbImgOrig.copy()
camCoord = self.kinectClient.kinectRGBCamCoordVec[kinectIdx]
croppedImgs = []
mPosVec = []
totalConf = 0
for point in points:
p = point['trackedData']
wPos = [p['x'], p['y'], p['z']]
trackedId = point['trackedId']
rt = self.kinectClient.calcCropCorners(kinectIdx,\
wPos,\
self.faceCropParams['width'], self.faceCropParams['height'],\
self.faceCropParams['zTopMergin'], self.faceCropParams['cropType'])
if rt == None:
continue
iPosPixel, corner_iPosPixels = rt
cv2.circle(rgbImg, (iPosPixel[0], iPosPixel[1]), 6, (0, 0, 255), -1)
cv2.putText(rgbImg, str(trackedId), (iPosPixel[0]+7, iPosPixel[1]), cv2.FONT_HERSHEY_PLAIN, 2.0, (0,0,255))
cv2.polylines(rgbImg, np.array([corner_iPosPixels]), True, (0,255,0), 2)
#人物領域をcroppedImgとして抜出
cropImgSize = (50, 70) #[width, height] pixels
croppedImg = FXImageUtil.cropImage(rgbImgOrig, corner_iPosPixels, (140, 100))
minSize = int(min(cropImgSize)*0.2)
maxSize = int(max(cropImgSize)*0.8)
detAng, detConf = self.detectOrientation.detect(croppedImg, minSize, maxSize)
detAngDeg = math.degrees(detAng)
camDir = np.array(self.kinectClient.getT(kinectIdx)) - np.array(wPos)
camDir[2] = 0
faceCoord = CoordinateTransform3D.CoordinateTransform3D()
#一般的な場合、なぜか(-90, 90, 0)がR=eyeになる。なので、その相対。detAngDegは時計回りに正
facePan = -90 + detAngDeg
faceCoord.setPanTiltRoll(facePan, 90, 0)
faceDir = faceCoord.transformR(camDir)
faceDir = faceDir/np.linalg.norm(faceDir)
faceDirWPos = np.array(wPos) + faceDir * 500.0
faceDirIPos = camCoord.wPos2iPos(faceDirWPos)
faceDirIPosPixels = camCoord.iPos2iPosPixel(faceDirIPos, rgbImg)
if detConf > 10:
angText = "ang:%.0f, conf:%.0f" % (detAngDeg, detConf)
cv2.putText(rgbImg, angText, (iPosPixel[0]+7, iPosPixel[1]+30), cv2.FONT_HERSHEY_PLAIN, 1.5, (0,0,255))
cv2.line(rgbImg, (iPosPixel[0], iPosPixel[1]), (faceDirIPosPixels[0], faceDirIPosPixels[1]), (0,255,255), 3)
#trackedDataに'face'情報を追加
faceData = {}
faceData['angle'] = detAng
faceData['confidence'] = detConf
point['face'] = faceData
mPos = self.kinectFloorMap.wPos2mapPos(wPos)
mPosVec.append(mPos)
if detConf > 0:
faceDirWPos2 = np.array(wPos) + faceDir*10000.
faceDir_mPos = self.kinectFloorMap.wPos2mapPos(faceDirWPos2)
faceAngleImg_person = self.kinectFloorMap.getBlankImg(dtype=np.uint16)
rangeAngle = 30. #degrees
val = detConf
faceAngleImg_person = FXImageUtil.drawSensingDirection(faceAngleImg_person, mPos, faceDir_mPos,\
rangeAngle, val)
faceAngleImg = faceAngleImg + faceAngleImg_person
totalConf = totalConf + detConf
#表示用にcroppedImgsに横につなげる
if np.alen(croppedImgs) > 0:
croppedImgs = np.hstack((croppedImgs, croppedImg))
else:
croppedImgs = np.copy(croppedImg)
self.currentTrackedDataFace = trackedData
dispImg = cv2.resize(rgbImg, (810, 540))
#dispImg = rgbImg
cv2.imshow("rgbFace", dispImg)
#cv2.imshow("croppedImgs", croppedImgs)
self.currentFaceAngleImg = faceAngleImg
if self.currentFaceAngleSumImg == None:
self.currentFaceAngleSumImg = self.currentFaceAngleImg.copy()
else:
processInterval = t - self.currentFaceProcessTime
timeRange = 1.0
processInterval = min (processInterval, timeRange)
w = processInterval / timeRange
self.currentFaceAngleSumImg = cv2.addWeighted(self.currentFaceAngleSumImg, (1.0 - w),\
self.currentFaceAngleImg, w, 0)
self.faceAngleConfMax = max(self.faceAngleConfMax, totalConf)
self.currentFaceProcessTime = t
#表示系
self.faceAngleConfMax = max(self.faceAngleConfMax, totalConf)
if self.faceAngleConfMax > 0:
faceAngleImg8UC3 = FXImageUtil.createColorMap(self.currentFaceAngleSumImg, 0.0, self.faceAngleConfMax)[0]
#faceAngleImg8UC3 = FXImageUtil.createColorMap(self.currentFaceAngleImg, 0.0, self.faceAngleConfMax)[0]
for mPos in mPosVec:
cv2.circle(faceAngleImg8UC3, (int(mPos[0]), int(mPos[1])), 3, (0, 0, 255), -1)
cv2.imshow("faceAngleImg", faceAngleImg8UC3)
def trackProcess(self):
t = FXTimeUtil.getT()
floorImg = self.getFloorImage()
if floorImg == None:
return
viewPort = self.trackInfo['viewPort']
floorImg8UC3, floorImgBin = FXImageUtil.createColorMap(floorImg, 0., viewPort['z']['max'])
noiseReduction = self.trackInfo['noiseReduction']
if noiseReduction > 0:
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (noiseReduction, noiseReduction))
floorImgBin = cv2.morphologyEx(floorImgBin, cv2.MORPH_OPEN, kernel)
floorImgBin = cv2.morphologyEx(floorImgBin, cv2.MORPH_CLOSE, kernel)
binDilate = self.trackInfo['binDilate']
if binDilate > 0:
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (binDilate, binDilate))
floorImgBin = cv2.dilate(floorImgBin, kernel)
binErode = self.trackInfo['binErode']
if binErode > 0:
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (binErode, binErode))
floorImgBin = cv2.erode(floorImgBin, kernel)
floorImgBinOrig = floorImgBin.copy()
contours, hierarchy = cv2.findContours(floorImgBin, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
gaussianSize = self.trackInfo['gaussianSize']
if gaussianSize > 0:
floorImgGaussian = cv2.GaussianBlur(floorImg,(gaussianSize,gaussianSize),0)
else:
floorImgGaussian = floorImg.copy()
#floorImg = floorImgGaussian
#floorImgGaussian8UC3 = FXImageUtil.createColorMap(floorImgGaussian, 0., viewPort['z']['max'])[0]
#cv2.imshow("floorImgGaussian", floorImgGaussian8UC3)
######## audioのマップを作成する
self.audioProcess()
########トラッキング処理
trackedData = {'dataId': 'kinectTracker', 't': t}
points = []
for contour in contours:
if len(contour) < 5:
continue #too small
box = cv2.fitEllipse(contour)
#検出したサイズをmmに (minSize, maxSize)
objectSizeMM = (min(box[1]) * self.trackInfo['pixel2mm'], max(box[1]) * self.trackInfo['pixel2mm'])
#print objectSize
#サイズのチェック
if objectSizeMM[0] < self.trackInfo['objectSize']['min']: #小さい方の大きさ
continue
if objectSizeMM[1] > self.trackInfo['objectSize']['max']: #大きい方の大きさ
continue
ellipseImgBin = np.zeros(floorImgBin.shape).astype(np.uint8)
cv2.ellipse(ellipseImgBin, box, (255), -1)
minMaxLoc = cv2.minMaxLoc(floorImgGaussian, mask=ellipseImgBin)
#人物のシルエット
personBinImg = floorImgBin * ellipseImgBin
#人物のシルエットの面積/楕円の面積
areaSize_ellipse = np.count_nonzero(ellipseImgBin)
areaSize_personBin = np.count_nonzero(personBinImg)
areaSizeRate = 0.
if areaSize_ellipse > 0:
areaSizeRate = float(areaSize_personBin) / float(areaSize_ellipse)
#print box #((centerx, centery), (w,h), angle?)
boxCenter = box[0]
object_height = int(minMaxLoc[1])
maxLoc = minMaxLoc[3]
object_mPos = [maxLoc[0], maxLoc[1]]
#object_mPos = [int(boxCenter[0]), int(boxCenter[1])]
object_wPos = self.kinectFloorMap.mapPos2wPos(object_mPos, object_height)
audioScore = 0.
if self.currentAudioAngleSumImg != None:
audioScore = float(self.currentAudioAngleSumImg[object_mPos[1]][object_mPos[0]])
tData = {}
tData['x'] = object_wPos[0]
tData['y'] = object_wPos[1]
tData['z'] = object_wPos[2]
tData['width'] = objectSizeMM[1] #大きい方
tData['height'] = objectSizeMM[0] #小さい方
mData = {}
mData['x'] = object_mPos[0]
mData['y'] = object_mPos[1]
mData['box'] = box
mData['areaSizeRate'] = areaSizeRate
audioData = {}
audioData['score'] = audioScore
#print areaSizeRate
point = {'trackedData': tData, 'trackedMapData': mData, 'audio': audioData}
points.append(point)
trackedData['data'] = points
self.putTrackedIds(self.currentTrackedData, trackedData)
###### visualize
audioAngleSumImg8UC3 = cv2.cvtColor(self.currentAudioAngleSumImg, cv2.COLOR_GRAY2BGR)
floorImgBin8UC3 = cv2.cvtColor(floorImgBinOrig, cv2.COLOR_GRAY2BGR)
#floorImg8UC3に描画
if trackedData.has_key('data'):
points = trackedData['data']
for point in points:
mPos = [point['trackedMapData']['x'], point['trackedMapData']['y']]
box = point['trackedMapData']['box']
cv2.ellipse(floorImg8UC3, box, (0, 0, 255), 1)
cv2.circle(floorImg8UC3, (mPos[0], mPos[1]), 3, (0,0,255), -1)
trackStr = "(" + point['trackedId'] + ")" + str(object_wPos[2])
cv2.putText(floorImg8UC3, trackStr, (mPos[0]+5, mPos[1]), cv2.FONT_HERSHEY_PLAIN, 0.8, (0,0,255))
#cv2.ellipse(audioAngleSumImg8UC3, box, (0, 0, 255), 1)
cv2.circle(audioAngleSumImg8UC3, (mPos[0], mPos[1]), 3, (0,0,255), -1)
cv2.ellipse(floorImgBin8UC3, box, (0, 0, 255), 1)
cv2.circle(floorImgBin8UC3, (mPos[0], mPos[1]), 3, (0,0,255), -1)
cv2.imshow("floorImg", floorImg8UC3)
cv2.imshow("audioAngleImg", audioAngleSumImg8UC3)
cv2.imshow("floorImgBin", floorImgBin8UC3)
self.currentTrackedData = trackedData
