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

trunk/code/readGridEye.py

@@ -65,52 +65,163 @@ class GridEye():
                     data = []
                 if len(data) > 8:
                     data.pop(0)
-                    
+
+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
+    def exponential(img, value):
+        tmp = cv2.pow(img.astype(np.double), value)*(255.0/(255.0**value))
+        return tmp.astype(np.uint8)
+        
     SIZE = 128
     overlap = 120
-    grideye = GridEye('COM17', 115200)
+    AVERAGE_FRAME = 10
+    distanceBetweenSensors = 7.7 #cm
+    distance2Object = 60.0 #cm
+    ADJUST_BACK = 5
+    EXPONENTAL_VALUE = 0.4
+    
+    offset = (distanceBetweenSensors / (2*distance2Object*math.tan(30.0/180.0*math.pi))) * SIZE
+    overlap = int(SIZE - offset)
+    print (overlap)
+    grideye = GridEye('COM15', 115200)
     grideye.start()
-    grideye2 = GridEye('COM15', 115200)
+    grideye2 = GridEye('COM17', 115200)
     grideye2.start()
     
+    distanceSensor = Distance('COM18', 9600)
+    distanceSensor.start()
+    
     fourcc = cv2.VideoWriter_fourcc(*'XVID')
     videoWriter = cv2.VideoWriter('output.avi', fourcc, 10.0, (SIZE*2,SIZE*2))
     cv2.imshow('sample', np.zeros((SIZE*2,SIZE*2), np.uint8))
+    aver1 = np.zeros((SIZE,SIZE), np.uint16)
+    aver2 = np.zeros((SIZE,SIZE), np.uint16)
+    cnt = 0
     while True:
         if grideye.frame and grideye2.frame:
             grideye.lock.acquire()
             grideye2.lock.acquire()
+            distanceSensor.lock.acquire()
             frame = grideye.frame
             grideye.frame = None
             frame2 = grideye2.frame
             grideye2.frame = None
             # frame2 = frame
+            distance2Object = distanceSensor.distance
+            distanceSensor.lock.release()
             grideye2.lock.release()
             grideye.lock.release()
             
-            
             img = (np.array(frame.data)-15)*10
-            img = cv2.equalizeHist(cv2.resize(img.astype(np.uint8), (SIZE,SIZE), interpolation = cv2.INTER_CUBIC)) # INTER_LINEAR, INTER_CUBIC
+            img = cv2.resize(img.astype(np.uint8), (SIZE,SIZE), interpolation = cv2.INTER_CUBIC) # INTER_LINEAR, INTER_CUBIC
             img2 = (np.array(frame2.data)-15)*10
-            img2 = cv2.equalizeHist(cv2.resize(img2.astype(np.uint8), (SIZE,SIZE), interpolation = cv2.INTER_CUBIC))
+            img2 = cv2.resize(img2.astype(np.uint8), (SIZE,SIZE), interpolation = cv2.INTER_CUBIC)
             
-            out = np.zeros((SIZE*2, SIZE*2), dtype=np.uint16)
+            if cnt < AVERAGE_FRAME:
+                cnt += 1
+                aver1 += img
+                aver2 += img2
+                if cnt == AVERAGE_FRAME:
+                    aver1 = aver1/AVERAGE_FRAME
+                    aver1 = aver1.astype(np.uint8)
+                    aver1 += ADJUST_BACK
+                    aver2 = aver2/AVERAGE_FRAME
+                    aver2 = aver2.astype(np.uint8)
+                    aver2 += ADJUST_BACK
+                continue
+            img = cv2.subtract(img, aver1)
+            img2 = cv2.subtract(img2, aver2)
+            
+            out = np.full((SIZE*3, SIZE*2), 255, dtype=np.uint16)
             out[:SIZE, :SIZE] = img
             out[:SIZE, SIZE:] = img2
             
             
+            overlap = int(SIZE - (distanceBetweenSensors / (2*distance2Object*math.tan(30.0/180.0*math.pi))) * SIZE)
             offset = int(overlap/2)
-            out[SIZE:, offset:SIZE+offset] = img
-            out[SIZE:, (SIZE-overlap)+offset:SIZE+offset] += img2[:,:overlap]
-            out[SIZE:, (SIZE-overlap)+offset:SIZE+offset] = out[SIZE:, (SIZE-overlap)+offset:SIZE+offset]/2
+            # tmp = cv2.resize(img.astype(np.uint8), (SIZE*2-overlap, SIZE))
+            # tmp.astype(np.uint16)
+            tmp = np.zeros((SIZE, SIZE*2-overlap), dtype=np.uint16)
+            tmp[:, :SIZE] = img
+            tmp[:, -SIZE:] += img2
+            tmp[:, (SIZE-overlap): SIZE] = tmp[:, (SIZE-overlap): SIZE]/2
+            tmp = exponential(tmp, EXPONENTAL_VALUE)
+            out[SIZE:SIZE*2, offset: SIZE*2-overlap+offset] = tmp
+            # out[SIZE:SIZE*2, offset:SIZE+offset] = img
+            # out[SIZE:SIZE*2, (SIZE-overlap)+offset:SIZE+offset] += img2[:,:overlap]
+            # out[SIZE:SIZE*2, (SIZE-overlap)+offset:SIZE+offset] = out[SIZE:SIZE*2, (SIZE-overlap)+offset:SIZE+offset]/2
+            # out[SIZE:SIZE*2, SIZE+offset:SIZE+(SIZE-overlap)+offset] = img2[:,overlap:SIZE]
+            
             
-            out[SIZE:, SIZE+offset:SIZE+(SIZE-overlap)+offset] = img2[:,overlap:SIZE]
+            maxProduct = 0
+            overlap2 = 0
+            for i in range(80, 128):
+                product = sum(img[:,SIZE-i:].astype(np.uint32)*img2[:,:i].astype(np.uint32))
+                product = sum(product) / len(product)
+                if product > maxProduct:
+                    maxProduct = product
+                    overlap2 = i
+                    
+            
+            
+            offset = int(overlap2/2)
+            tmp = np.zeros((SIZE, SIZE*2-overlap2), dtype=np.uint16)
+            tmp[:, :SIZE] = img
+            tmp[:, -SIZE:] += img2
+            tmp[:, (SIZE-overlap2): SIZE] = tmp[:, (SIZE-overlap2): SIZE]/2
+            tmp = exponential(tmp, EXPONENTAL_VALUE)
+            out[SIZE*2:, offset: SIZE*2-overlap2+offset] = tmp
+            
+            
+            # out[SIZE*2:, offset:SIZE+offset] = img
+            # out[SIZE*2:, (SIZE-overlap2)+offset:SIZE+offset] += img2[:,:overlap2]
+            # out[SIZE*2:, (SIZE-overlap2)+offset:SIZE+offset] = out[SIZE*2:, (SIZE-overlap2)+offset:SIZE+offset]/2
+            # out[SIZE*2:, SIZE+offset:SIZE+(SIZE-overlap2)+offset] = img2[:,overlap2:SIZE]
             out = out.astype(np.uint8)
+            
             cv2.imshow('sample', out)
             videoWriter.write(cv2.cvtColor(out,cv2.COLOR_GRAY2BGR))
             key = cv2.waitKey(1)