def getAudioMapImg(self):
audioAngleImgTotal = self.kinectFloorMap.getBlankImg(dtype=np.uint16)
audioDataVec = self.kinectClient.getAudioDataVec()
for k in xrange(self.kinectClient.getKinectNum()):
audioData = audioDataVec[k]
if audioData == None:
continue
sourceAngle = audioData['sourceAngle']
sourceConfidence = audioData['sourceConfidence']
if sourceConfidence == 0:
continue
kinect_wPos = np.array(self.kinectClient.getT(k))
kinect_cTransform = self.kinectClient.getCoorinateTransform(k)
distance = 10000. #10(m)
audioDirPos = [math.tan(math.radians(- sourceAngle))*distance, 0., distance]
audioDirPos = kinect_cTransform.transformR(audioDirPos)
audioDst_wPos = kinect_wPos + np.array(audioDirPos)
kinect_mPos = self.kinectFloorMap.wPos2mapPos(kinect_wPos)
audioDst_mPos = self.kinectFloorMap.wPos2mapPos(audioDst_wPos)
audioAngleImg = self.kinectFloorMap.getBlankImg(dtype=np.uint8)
val = int(255. * sourceConfidence) #最大255
#val = sourceConfidence
if val == 0:
continue
minRangeAngle = 10.
rangeAngle = minRangeAngle + (90. - minRangeAngle) * (1.0-sourceConfidence) #minRangeAngle-90度。confidence==1で20度
audioAngleImg = FXImageUtil.drawSensingDirection(audioAngleImg, kinect_mPos, audioDst_mPos,\
rangeAngle, val)
audioAngleImgTotal = audioAngleImgTotal + audioAngleImg
audioAngleImgTotal = audioAngleImgTotal / self.kinectClient.getKinectNum() #kinect台数で割る
audioAngleImgTotal = audioAngleImgTotal.astype(np.uint8)
return audioAngleImgTotal
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
def diffCheckProcess(self):
interval = self.param['interval']
while True:
sTime = FXTimeUtil.getT()
if self.movementValMean < self.param['movementCheck']['thr']:
if self.debugLevel >= 2:
print "no movements. diff check"
img = self.getImg()
if img==None:
continue
imgGray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
imgGray = cv2.GaussianBlur(imgGray, (5,5),0)
lapImg = cv2.Laplacian(imgGray, cv2.CV_32F, ksize=3)
#lapImg = lapImg.astype(np.uint8)
#conjunction
#kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))
#lapImg = cv2.dilate(lapImg, kernel, iterations=2)
#lapImg = cv2.erode(lapImg, kernel, iterations=2)
if self.diffCheckImg == None:
self.diffCheckImg = lapImg
diffImg = cv2.absdiff(lapImg, self.diffCheckImg)
ret, diffImg = cv2.threshold(diffImg, 10, 255, cv2.THRESH_BINARY)
diffImg = diffImg.astype(np.uint8)
#noise reduction
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
diffImg = cv2.erode(diffImg, kernel, iterations=2)
diffImg = cv2.dilate(diffImg, kernel, iterations=2)
diffImg = cv2.bitwise_and(diffImg, self.diffCheckMask)
if self.debugLevel >= 1:
dispImg = diffImg.copy()
dispImg = FXImageUtil.scaleImage(diffImg, 0.25)
cv2.imshow("diffImg", dispImg)
val = cv2.mean(diffImg)
val = float(val[0])
self.diffVal = val
self.diffCheckImg = lapImg
if self.debugLevel >= 2:
print "HD difference val: %f" % self.diffVal
if useRoyWeb:
ps.send('diff', self.diffVal, 'v', 'diffVal')
if self.diffVal > self.param['thr']:
print "difference is detected"
thumbSize = self.param['dstThumb']['size']
thumbImg = cv2.resize(img, (thumbSize[0], thumbSize[1]))
if self.thumbRectFlag:
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (100, 100))
diffImgArea = cv2.dilate(diffImg, kernel)
diffImgArea = cv2.erode(diffImgArea, kernel)
contours, hierarchy = cv2.findContours(diffImgArea, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
#get max rectangle
areas = [cv2.contourArea(contour) for contour in contours]
cnt_max = [contours[areas.index(max(areas))]][0]
rect = cv2.boundingRect(cnt_max)
#draw rectangle
offset = 20
pt1 = (int((rect[0]-offset) * self.thumbScale), int((rect[1]-offset) * self.thumbScale))
pt2 = (int((rect[0]+rect[2]+offset) * self.thumbScale), int((rect[1]+rect[3]+offset) * self.thumbScale))
cv2.rectangle(thumbImg, pt1, pt2, self.thumbRectColor, thickness=self.thumbRectThickness)
self.handleWBCapture(img, thumbImg)
if self.debugLevel >= 1:
dispImg = FXImageUtil.scaleImage(img, 0.25)
cv2.imshow("captured wb", dispImg)
dispImg = FXImageUtil.scaleImage(diffImg, 0.25)
cv2.imshow("captured wb diff", dispImg)
else:
if self.debugLevel >= 2:
print "movement is detected. skip diff check"
duration = FXTimeUtil.getT() - sTime
if duration < interval:
time.sleep(interval - duration)
duration = interval
cv2.waitKey(1)
