Example #1
0
    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
Example #2
0
    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)
Example #3
0
    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)