add gridbox code

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

trunk/gridEyeBox/dataFuser.py

@@ -0,0 +1,69 @@
+import cv2
+import math
+import numpy as np
+
+kBACKGROUND_NUM = 10
+kSIZE = 128
+kEXPONENTAL_VALUE = 0.7
+
+gSENSOR_FOV = 60.0 / 180.0 * math.pi
+
+
+def exponential(img, value):
+  tmp = cv2.pow(img.astype(np.double), value)*(255.0/(255.0**value))
+  return tmp.astype(np.uint8)
+
+
+def mergeFrames(imgs, SIZE, overlap):
+  tmp = np.zeros((SIZE, SIZE*2-overlap), dtype=np.uint16)
+  tmp[:, :SIZE] = imgs[0]
+  tmp[:, -SIZE:] += imgs[1]
+  tmp[:, (SIZE-overlap): SIZE] = tmp[:, (SIZE-overlap): SIZE]/2
+  
+  tmp2 = np.zeros((SIZE, SIZE*2-overlap), dtype=np.uint16)
+  tmp2[:, :SIZE] = imgs[2]
+  tmp2[:, -SIZE:] += imgs[3]
+  tmp2[:, (SIZE-overlap): SIZE] = tmp2[:, (SIZE-overlap): SIZE]/2
+  
+  merge = np.zeros((SIZE*2-overlap, SIZE*2-overlap), dtype=np.uint16)
+  merge[:SIZE, :] = tmp
+  merge[-SIZE:, :] += tmp2
+  merge[(SIZE-overlap):SIZE, :] = merge[(SIZE-overlap):SIZE, :]/2
+  #merge = exponential(merge, kEXPONENTAL_VALUE)
+  
+  return merge.astype(np.uint8)
+
+
+class DataFuser(object):
+  def __init__(self, sensor_dist):
+    self.background_cnt = 0
+    self.background_frame = np.zeros((4, 64))
+    self.sensor_dist = sensor_dist
+
+  def mergeFrame(self, frame, dist = None):
+    if self.background_cnt < kBACKGROUND_NUM:
+      self.background_frame += frame / kBACKGROUND_NUM
+      self.background_cnt += 1
+      return False
+
+    frame = exponential(cv2.subtract(exponential(frame, kEXPONENTAL_VALUE),
+                         exponential(self.background_frame, kEXPONENTAL_VALUE)),
+                         0.3)
+    print (([max(x) for x in frame]))
+    imgs = [np.reshape(img, (8, 8)) for img in frame]
+    imgs = [cv2.resize(img.astype(np.uint8), (kSIZE, kSIZE),
+        interpolation = cv2.INTER_LINEAR) for img in imgs]
+    try:
+      overlap = int(kSIZE - 
+          kSIZE * (self.sensor_dist / (2 * dist * math.tan(gSENSOR_FOV / 2))))
+    except:
+      overlap = 0
+    if overlap < 0:
+      overlap = 0
+    overlap = 0
+    
+    return mergeFrames(imgs, kSIZE, overlap)
+
+    if not dist:
+      pass
+

trunk/gridEyeBox/gridEyeReader.py

@@ -0,0 +1,24 @@
+import serial
+
+class GridEye(object):
+  def __init__(self, port, baudrate=115200):
+    self.serial = serial.Serial(port, baudrate, timeout=0.1)
+  
+  def readData(self):
+    datas = []
+    for i in range(4):
+      retry = True
+      while retry:
+        self.serial.reset_input_buffer()
+        self.serial.reset_output_buffer()
+        self.serial.write(bytes([ord('0')+i]))
+        raw_data = self.serial.read(64*5+3)
+        if raw_data.split(b':')[0] != bytes([ord('0')+i]):
+          continue
+        data = [int(x, 16)*0.25 for x in raw_data.split()[1:]]
+        #data = self.serial.read(64*5+3)
+        if b'FFFF' not in data:
+          retry = False
+        datas.append(data)
+
+    return datas

trunk/gridEyeBox/main.py

@@ -0,0 +1,27 @@
+from gridEyeReader import GridEye
+from dataFuser import DataFuser
+
+import cv2
+import logging
+import time
+import numpy as np
+
+logging.basicConfig(level=logging.DEBUG)
+
+g = GridEye('/dev/ttyACM0')
+d = DataFuser(10) # distance between sensors.
+cv2.namedWindow("Image", cv2.WINDOW_NORMAL)
+for _ in range(1000000):
+  frame = g.readData()
+  out = d.mergeFrame(np.array(frame), 100)
+  if type(out) is np.ndarray:
+    cv2.imshow('Image', cv2.cvtColor(out, cv2.COLOR_GRAY2BGR))
+  k = cv2.waitKey(1)
+  if k == ord('q'):
+    break
+  elif k == ord('c'):
+    cv2.imwrite('shot.png', out)
+
+
+cv2.destroyAllWindows()
+