git-svn-id: http://newslabx.csie.ntu.edu.tw/svn/Ginger@29 5747cdd2-2146-426f-b2b0-0570f90b98ed

trunk/code/readGridEye.py

@@ -69,52 +69,13 @@ class GridEye():
                             print ('something wrong', len(data))
                 except Exception as e:
                     print (e)
-                    
-class Distance():
-    def __init__(self, serialPort, baudrate):
-        self.port = serial.Serial(serialPort, baudrate)
-        self.distance = 200
-        self.reading = True
-        self.thread = threading.Thread(target = self.reader)
-        self.thread.setDaemon(True)
-        self.lock = threading.Lock()
-
-    def start(self):
-        self.port.reset_input_buffer()
-        self.thread.start()
-        
-    def stop(self):
-        self.reading = False
-        self.thread.join()
-        
-    def reader(self):
-        while (self.reading):
-            line = b''
-            while (self.reading):
-                c = self.port.read()
-                if c == b'\r':
-                    c = self.port.read()
-                    break
-                if c == b'\n':
-                    break
-                line += c
-            if b'Distance' in line:
-                try:
-                    dist = float(line.decode('utf-8').split(':')[1])
-                    print (dist)
-                    if dist > 200.0:
-                        dist = 200.0
-                    self.lock.acquire()
-                    self.distance = dist
-                    self.lock.release()
-                except ValueError as e:
-                    print ('error', e)
-                    
+              
         
 if __name__ == '__main__':
     import cv2
     import numpy as np
     import math
+    import json
     def exponential(img, value):
         tmp = cv2.pow(img.astype(np.double), value)*(255.0/(255.0**value))
         return tmp.astype(np.uint8)
@@ -126,9 +87,12 @@ if __name__ == '__main__':
     distance2Object = 60.0 #cm
     ADJUST_BACK = 5
     EXPONENTAL_VALUE = 0.4
-    
+    PRODUCTION_THRESHOLD = 100
+    MIN_EXIST_TIME = 0.5
+    W_ARRAY = np.array([list(range(SIZE*2)) for x in range(SIZE*2)])
+    H_ARRAY = np.array([[x]*(SIZE*2) for x in range(SIZE*2)])
 
-    grideye = GridEye('COM25', 115200)
+    grideye = GridEye('COM18', 115200)
     grideye.start()
     grideye2 = GridEye('COM24', 115200)
     grideye2.start()
@@ -142,6 +106,9 @@ if __name__ == '__main__':
     cv2.imshow('sample', np.zeros((SIZE*3,SIZE*2), np.uint8))
     cnt = 0
     avers = []
+    hasPos = False
+    endTime = 0
+    startTime = 0
     while True:
         if grideye.frame1 and grideye.frame2 and grideye2.frame1 and grideye2.frame2:
             grideye.lock.acquire()
@@ -157,7 +124,11 @@ if __name__ == '__main__':
                 distance2Object = 200
             grideye2.lock.release()
             grideye.lock.release()
-            
+            with open('log.txt', 'a') as f:
+                f.write(json.dumps(frames[0].time)+'\n')
+                for frame in frames:
+                    f.write(json.dumps(frame.data)+'\n')
+                #print (json.dumps(frames))
             imgs = []
             for frame in frames:
                 img = (np.array(frame.data)-15)*10
@@ -186,7 +157,7 @@ if __name__ == '__main__':
             out[:SIZE, SIZE:SIZE*2] = imgs[1]
             out[SIZE:SIZE*2, :SIZE] = imgs[2]
             out[SIZE:SIZE*2, SIZE:SIZE*2] = imgs[3]
-            
+            '''
             try:
                 overlap_w = int(SIZE - (distanceBetweenSensors_w / (2*distance2Object*math.tan(30.0/180.0*math.pi))) * SIZE)
             except:
@@ -234,6 +205,7 @@ if __name__ == '__main__':
                     maxProduct = product
                     overlap_w = i
                     
+            tmp = maxProduct
             maxProduct = 0
             overlap_h = 0
             for i in range(80, 128):
@@ -243,6 +215,7 @@ if __name__ == '__main__':
                 if product > maxProduct:
                     maxProduct = product
                     overlap_h = i
+            maxProduct = (tmp + maxProduct)/2
             
             tmp = np.zeros((SIZE, SIZE*2-overlap_w), dtype=np.uint16)
             tmp[:, :SIZE] = imgs[0]
@@ -259,11 +232,11 @@ if __name__ == '__main__':
             merge[-SIZE:, :] += tmp2
             merge[(SIZE-overlap_h):SIZE, :] = merge[(SIZE-overlap_h):SIZE, :]/2
             
-            
+                
             offset_w = int(overlap_w/2)
             offset_h = int(overlap_h/2)
             out[SIZE*2+offset_h:SIZE*4-overlap_h+offset_h, SIZE*2+offset_w: SIZE*4-overlap_w+offset_w] = merge
-            
+            '''
             # offset = int(overlap2/2)
             # tmp = np.zeros((SIZE, SIZE*2-overlap2), dtype=np.uint16)
             # tmp[:, :SIZE] = img
@@ -276,31 +249,35 @@ if __name__ == '__main__':
             out = out.astype(np.uint8)
             out = exponential(out, EXPONENTAL_VALUE)
             
+            out = cv2.cvtColor(out,cv2.COLOR_GRAY2BGR)
+            if False and maxProduct > PRODUCTION_THRESHOLD:
+                print ('XDDDD',maxProduct)
+                position = [0,0]
+                rows,cols = merge.shape
+                position[0] = sum(sum(H_ARRAY[:rows,:cols]*merge))/sum(sum(merge))
+                position[1] = sum(sum(W_ARRAY[:rows,:cols]*merge))/sum(sum(merge))
+                pos_w = distanceBetweenSensors_w/(SIZE-overlap_w)*position[0]
+                pos_h = distanceBetweenSensors_h/(SIZE-overlap_h)*position[1]
+                cv2.circle(out, (SIZE*2+offset_w+int(position[1]), SIZE*2+offset_h+int(position[0])), 10, (255,0,0), 5)
+                if not hasPos:
+                    startPos = [pos_w, pos_h]
+                    startTime = frames[0].time
+                    hasPos = True
+                endPos = [pos_w, pos_h]
+                endTime = frames[0].time
+            elif hasPos:
+                if endTime - startTime > MIN_EXIST_TIME:
+                    print (startPos, endPos)
+                    print ('speed:', ((endPos[0]-startPos[0])**2+(endPos[1]-startPos[1])**2)**0.5/(endTime - startTime))
+                    print ('time:', endTime-startTime)
+                hasPos = False
+            if endTime - startTime > MIN_EXIST_TIME:
+                speed = ((endPos[0]-startPos[0])**2+(endPos[1]-startPos[1])**2)**0.5/(endTime - startTime)
+                cv2.putText(out, f'{speed:.2f}',
+                        (0, SIZE*2),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2, cv2.LINE_AA)
+                
             cv2.imshow('sample', out)
-            videoWriter.write(cv2.cvtColor(out,cv2.COLOR_GRAY2BGR))
-            try:
-                sift = cv2.xfeatures2d.SIFT_create()
-                img1 = exponential(imgs[0], EXPONENTAL_VALUE)
-                img2 = exponential(imgs[1], EXPONENTAL_VALUE)
-                kp_1, desc_1 = sift.detectAndCompute(img1, None)
-                kp_2, desc_2 = sift.detectAndCompute(img2, None)
-
-                index_params = dict(algorithm=0, trees=5)
-                search_params = dict()
-                flann = cv2.FlannBasedMatcher(index_params, search_params)
-                matches = flann.knnMatch(desc_1, desc_2, k=2)
-
-                good_points = []
-                ratio = 0.8
-                for m, n in matches:
-                    if m.distance < ratio*n.distance:
-                        good_points.append(m)
-                result = cv2.drawMatches(img1, kp_1, img2, kp_2, good_points, None)
-                cv2.imshow("result", result)
-                print (result.shape)
-                siftVideoWriter.write(result)
-            except:
-                pass
+            videoWriter.write(out)
         key = cv2.waitKey(1)
         if key == ord('q'):
             break

trunk/code/readGridEyeData.py

@@ -0,0 +1,224 @@
+import json
+
+class Frame():
+    def __init__(self, time, data):
+        self.time = time
+        self.data = data
+        
+class GridEyeData():
+    def __init__(self, filePath):
+        self.f = open(filePath, 'r')
+        self.frames = [None]*4
+    def readFrame(self):
+        time = self.f.readline()
+        if not time:
+            return False
+        time = float(time)
+        for i in range(4):
+            data = json.loads(self.f.readline())
+            self.frames[i] = Frame(time, data)
+        return True
+        
+if __name__ == '__main__':
+    import cv2
+    import numpy as np
+    import sys
+    def exponential(img, value):
+        tmp = cv2.pow(img.astype(np.double), value)*(255.0/(255.0**value))
+        return tmp.astype(np.uint8)
+      
+    SIZE = 128
+    AVERAGE_FRAME = 10
+    distanceBetweenSensors_w = 2.6 #cm
+    distanceBetweenSensors_h = 2.6 #cm
+    distance2Object = 60.0 #cm
+    ADJUST_BACK = 5
+    EXPONENTAL_VALUE = 0.4
+    PRODUCTION_THRESHOLD = 10
+    MIN_EXIST_TIME = 0.1
+    cnt = 0
+    avers = []
+    
+    fourcc = cv2.VideoWriter_fourcc(*'XVID')
+    videoWriter = cv2.VideoWriter('output.avi', fourcc, 10.0, (SIZE*2,SIZE*4))
+    cv2.imshow('sample', np.zeros((SIZE*4,SIZE*2), np.uint8))
+    gridEye = GridEyeData(sys.argv[1])
+    
+    hasLastFrame = False
+    hasPos = False
+    innerHasPos = False
+    endTime = 0
+    startTime = 0
+    innerEndTime = 0
+    innerStartTime = 0
+    path = []
+    speed = 0
+    while gridEye.readFrame():
+        frames = gridEye.frames
+        imgs = []
+        for frame in frames:
+            img = (np.array(frame.data)-15)*10
+            img = cv2.resize(img.astype(np.uint8), (SIZE,SIZE), interpolation = cv2.INTER_LINEAR) # INTER_LINEAR, INTER_CUBIC
+            imgs.append(img)
+            avers.append(np.zeros((SIZE,SIZE), np.uint16))
+        if cnt < AVERAGE_FRAME:
+            cnt += 1
+            for i in range(len(imgs)):
+                avers[i] += imgs[i]
+            if cnt == AVERAGE_FRAME:
+                for i in range(len(avers)):
+                    avers[i] = avers[i]/AVERAGE_FRAME
+                    avers[i] = avers[i].astype(np.uint8)
+                    avers[i] += ADJUST_BACK
+            continue
+            
+        for i in range(len(imgs)):
+            imgs[i] = cv2.subtract(imgs[i], avers[i])
+        out = np.full((SIZE*4, SIZE*2), 255, dtype=np.uint16)
+        out[:SIZE, :SIZE] = imgs[0]
+        out[:SIZE, SIZE:SIZE*2] = imgs[1]
+        out[SIZE:SIZE*2, :SIZE] = imgs[2]
+        out[SIZE:SIZE*2, SIZE:SIZE*2] = imgs[3]
+        
+        # production
+        '''
+        maxProduct = 0
+        overlap_w = 0
+        for i in range(80, 128):
+            product = sum(imgs[0][:,SIZE-i:].astype(np.uint32)*imgs[1][:,:i].astype(np.uint32))
+            product += sum(imgs[2][:,SIZE-i:].astype(np.uint32)*imgs[3][:,:i].astype(np.uint32))
+            product = sum(product) / len(product)
+            if product > maxProduct:
+                maxProduct = product
+                overlap_w = i
+        tmp = maxProduct
+        maxProduct = 0
+        overlap_h = 0
+        for i in range(80, 128):
+            product = sum(imgs[0][SIZE-i:, :].astype(np.uint32)*imgs[2][:i,:].astype(np.uint32))
+            product += sum(imgs[1][SIZE-i:, :].astype(np.uint32)*imgs[3][:i,:].astype(np.uint32))
+            product = sum(product) / len(product)
+            if product > maxProduct:
+                maxProduct = product
+                overlap_h = i
+        maxProduct = (tmp + maxProduct)/2
+        
+        # fixed overlap_h
+        '''
+        maxProduct = 0
+        overlaps = 125
+        overlap_w = overlaps
+        overlap_h = overlaps
+        product = sum(imgs[0][:,SIZE-overlaps:].astype(np.uint32)*imgs[1][:,:overlaps].astype(np.uint32))
+        product += sum(imgs[2][:,SIZE-overlaps:].astype(np.uint32)*imgs[3][:,:overlaps].astype(np.uint32))
+        product = sum(product) / len(product)
+        maxProduct = product
+        tmp = maxProduct
+        maxProduct = 0
+        product = sum(imgs[0][SIZE-overlaps:, :].astype(np.uint32)*imgs[2][:overlaps,:].astype(np.uint32))
+        product += sum(imgs[1][SIZE-overlaps:, :].astype(np.uint32)*imgs[3][:overlaps,:].astype(np.uint32))
+        product = sum(product) / len(product)
+        maxProduct = product
+        maxProduct = (tmp + maxProduct)/2
+        
+        
+        if maxProduct > PRODUCTION_THRESHOLD:
+            tmp = np.zeros((SIZE, SIZE*2-overlap_w), dtype=np.uint16)
+            tmp[:, :SIZE] = imgs[0]
+            tmp[:, -SIZE:] += imgs[1]
+            tmp[:, (SIZE-overlap_w): SIZE] = tmp[:, (SIZE-overlap_w): SIZE]/2
+            
+            tmp2 = np.zeros((SIZE, SIZE*2-overlap_w), dtype=np.uint16)
+            tmp2[:, :SIZE] = imgs[2]
+            tmp2[:, -SIZE:] += imgs[3]
+            tmp2[:, (SIZE-overlap_w): SIZE] = tmp2[:, (SIZE-overlap_w): SIZE]/2
+            
+            merge = np.zeros((SIZE*2-overlap_h, SIZE*2-overlap_w), dtype=np.uint16)
+            merge[:SIZE, :] = tmp
+            merge[-SIZE:, :] += tmp2
+            merge[(SIZE-overlap_h):SIZE, :] = merge[(SIZE-overlap_h):SIZE, :]/2
+            
+            position = [0,0]
+            rows,cols = merge.shape
+
+            for i in range(rows):
+                for j in range(cols):
+                    position[0] += i*merge[i][j]
+                    position[1] += j*merge[i][j]
+            position[0] /= sum(sum(merge))
+            position[1] /= sum(sum(merge))
+            
+            offset_w = int(overlap_w/2)
+            offset_h = int(overlap_h/2)
+            out[SIZE*2+offset_h:SIZE*4-overlap_h+offset_h, offset_w: SIZE*2-overlap_w+offset_w] = merge
+            pos_w = 1.17*position[0] #distanceBetweenSensors_w/(SIZE-overlap_w)*position[0]
+            pos_h = 1.17*position[1] #distanceBetweenSensors_h/(SIZE-overlap_h)*position[1]
+            if not hasPos:
+                startPos = [pos_w, pos_h]
+                sp = position
+                path = []
+                truePath = []
+                times = []
+                startTime = frames[0].time
+                hasPos = True
+            if not innerHasPos and pos_w >= 16 and pos_w <= 109 and pos_h >= 16 and pos_h <= 109: 
+                innerStartPos = [pos_w, pos_h]
+                innerStartTime = frames[0].time
+                innerHasPos = True
+            
+            if pos_w >= 16 and pos_w <= 109 and pos_h >= 16 and pos_h <= 109: 
+                innerEndPos = [pos_w, pos_h]
+                innerEndTime = frames[0].time
+            elif innerHasPos:
+                if innerEndTime - innerStartTime > 0:
+                    print (innerStartPos, innerEndPos)
+                    print ('inner speed:', ((innerEndPos[0]-innerStartPos[0])**2+(innerEndPos[1]-innerStartPos[1])**2)**0.5/(innerEndTime - innerStartTime))
+                    print ('time:', innerEndTime-innerStartTime)
+                innerHasPos = False
+                
+            endPos = [pos_w, pos_h]
+            endTime = frames[0].time
+            path.append(position)
+            truePath.append(endPos)
+            times.append(frames[0].time)
+        elif hasPos:
+            if endTime - startTime > 0:
+                print (startPos, endPos)
+                print ('speed:', ((endPos[0]-startPos[0])**2+(endPos[1]-startPos[1])**2)**0.5/(endTime - startTime))
+                print ('time:', endTime-startTime)
+            if innerHasPos and innerEndTime - innerStartTime > 0:
+                print (innerStartPos, innerEndPos)
+                print ('inner speed:', ((innerEndPos[0]-innerStartPos[0])**2+(innerEndPos[1]-innerStartPos[1])**2)**0.5/(innerEndTime - innerStartTime))
+                print ('time:', innerEndTime-innerStartTime)
+            hasPos = False
+            innerHasPos = False
+        out = out.astype(np.uint8)
+        out = exponential(out, EXPONENTAL_VALUE)
+        
+        out = cv2.cvtColor(out,cv2.COLOR_GRAY2BGR)
+        if endTime - startTime > MIN_EXIST_TIME:
+            speed = ((endPos[0]-startPos[0])**2+(endPos[1]-startPos[1])**2)**0.5/(endTime - startTime)
+            cv2.putText(out, f'{speed:.2f}',
+                    (0, SIZE*2),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2, cv2.LINE_AA)
+            speed = ((truePath[-1][0]-truePath[-2][0])**2+(truePath[-1][1]-truePath[-2][1])**2)**0.5/(times[-1] - times[-2])
+            cv2.putText(out, f'{speed:.2f}',
+                    (0, SIZE*2+30),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2, cv2.LINE_AA)
+        if maxProduct > PRODUCTION_THRESHOLD:
+            cv2.circle(out, (offset_w+int(position[1]), SIZE*2+offset_h+int(position[0])), 10, (255,0,0), 5)
+            cv2.circle(out, (offset_w+int(sp[1]), SIZE*2+offset_h+int(sp[0])), 10, (0,255,0), 5)
+            for i in range(len(path)-1):
+                cv2.line(out, (offset_w+int(path[i][1]), SIZE*2+offset_h+int(path[i][0])), (offset_w+int(path[i+1][1]), SIZE*2+offset_h+int(path[i+1][0])), (0,0,255))
+            cv2.line
+            lastFrame = out[SIZE*2:,:]
+            hasLastFrame = True
+        elif hasLastFrame:
+            out[SIZE*2:,:] = lastFrame
+        cv2.imshow('sample', out)
+        videoWriter.write(out)
+        
+        key = cv2.waitKey(1)
+        if key == ord('q'):
+            break
+            
+    videoWriter.release()
+    cv2.destroyAllWindows()