class Scanner(object): def __init__( self, device, laser_threshold=50, rotation_step=2*math.pi/200, camera_index=0, scale=10.0, fov=1.047, format_ratio=1.7777 ): self.device = device self.area = None self.mode = None self.keep_running = True self.platform_middle = None self.rotation_angle = 0 self.rotation_step = rotation_step self.width = None self.height = None self.scale = scale self.fov = fov self.format_ratio = format_ratio self.fov_x = format_ratio*fov/math.sqrt(1+format_ratio**2) self.fov_y = fov/math.sqrt(1+format_ratio**2) # FIXME: This should be calibrated, not hardcoded # To be able to calculate the camera position, we need the camera angle and (for example) the size of the platform # m = rotate(array([1, 0, 0]), numpy.array([0, 1, 0]), camera_angle) # Straight ahead rotated by camera angle self.camera_position = array([0, -0.3, 0.15]) self.last_raw_points = None self.laser_threshold = laser_threshold self.is_laser_on = False self.display_image = None self.processed_frames = Scan() self.ui_messages = [] self.capture = cv2.VideoCapture(camera_index) self.window_name = "ui" cv2.namedWindow(self.window_name) cv2.setMouseCallback(self.window_name, self.handle_mouse) # Arduino connection try: self.ser = serial.Serial(device, 9600, timeout=1) except OSError: print "Couldn't establish serial connection" self.ser = None def send_command(self, command, wait_for_reply=False): """ Send serial command to arduino """ if not self.ser: return self.ser.write(command) if wait_for_reply: self.ser.readline().strip() def calibrate(self, x=None): frame = self.capture_diff() self.height, self.width, channels = frame.shape def handle_mouse(self, e, x, y, flags, param): if self.mode == 'set_middle': if e == cv2.EVENT_LBUTTONDOWN: self.platform_middle = (x, y) self.mode = None else: if e == cv2.EVENT_LBUTTONDOWN: self.area = Box(x, y, None, None) elif e == cv2.EVENT_LBUTTONUP: self.area.x2 = x self.area.y2 = y def add_message(self, message): if len(self.ui_messages) >= 5: self.ui_messages = self.ui_messages[-5:] self.ui_messages.append((datetime.now(), message)) def clear_old_messages(self): new_messages = [] now = datetime.now() max_delta = timedelta(seconds=5) for (timestamp, message) in self.ui_messages: if now - timestamp < max_delta: new_messages.append((timestamp, message)) self.ui_messages = new_messages def handle_keyboard(self): k = cv2.waitKey(1) & 0xFF if k == 115: # 's' for scan if not self.platform_middle: self.add_message('Please set platform middle before scanning (press m and click)') return # Enable motors to avoid a delay on the first step self.send_command('M') time.sleep(0.5) self.mode = 'scan' elif k == 27: if self.mode == 'scan': self.mode = 'stop_scan' else: self.keep_running = False elif k == 105: # 'i' for info print self.send_command('d') elif k == 108: self.set_laser(not self.is_laser_on) elif k == 109: # 'm' to set platform middle self.mode = 'set_middle' def _capture_frame(self): # if self.is_laser_on: # return cv2.imread("sketch1_laser.png") # return cv2.imread("sketch1.png") return self.capture.read()[1] def set_laser(self, is_on): command = 'L' if is_on else 'l' self.send_command(command) self.is_laser_on = is_on def capture_diff(self, thresholded=False): # Captures two frames, one with laser on and one with laser off # Set thresholded to True to return the frame with self.laser_threshold applied lower_red = numpy.array([15,50,50]) upper_red = numpy.array([165,255,255]) # Threshold the HSV image to get only red colors # Capture frame with laser on self.set_laser(True) time.sleep(0.2) frame = self._capture_frame() # Set frame with laser to be displayed # Capture frame with laser off self.set_laser(False) time.sleep(0.2) frame_no_laser = self._capture_frame() frame_diff = self.red_filter(cv2.absdiff(frame, frame_no_laser)) self.display_image = frame_diff if not thresholded: return frame_diff blue, green, red = cv2.split(frame_diff) retval, thresholded = cv2.threshold(red, self.laser_threshold, 255, cv2.THRESH_BINARY) return thresholded def get_laser_plane_intersection(self, v): """ Calculates the intersection of a vector with the laser plane. The camera position is represented by the vector [0, c_y, c_z], where c_y and c_z are the camera coordinates. c_y should be negative, as the y axis is positive away from the camera, starting at the platform middle. The plane is represented by the plane created by the vectors [1,1,0] and [0,0,1] """ c_y, c_z = self.camera_position[1:3] x, y, z = v[:3] lam = c_y/(x-y) return array([ lam*x, lam*y + c_y, lam*z + c_z ]) return v + array([0, ]) def process_frame(self, thresholded_frame): if self.area and self.area.is_complete: thresholded_frame = thresholded_frame[self.area.y1:self.area.y2, self.area.x1:self.area.x2] processed_frame = Frame() points = numpy.transpose(thresholded_frame.nonzero()) if not len(points): return # Precalculations tan_half_fov_x = math.tan(self.fov_x/2) tan_half_fov_y = math.tan(self.fov_y/2) # m is the vector from the camera position to the origin m = self.camera_position * -1 w = self.width/2 h = self.height/2 for point in points: img_y, img_x = point if self.area and self.area.is_complete: img_y += self.area.y1 img_x += self.area.x1 # Horizontal angle between platform middle (in image) and point delta_x = float(img_x - self.platform_middle[0])/2 tau = math.atan(delta_x/w*tan_half_fov_x) # Vertical angle delta_y = float(img_y - self.platform_middle[1])/2 rho = math.atan(delta_y/h*tan_half_fov_y) # Rotate vector m around tau and rho to point towards 'point' v = m v = rotate('z', v, tau) # Rotate around z axis for horizontal angle v = rotate('x', v, rho) # Rotate around x axis for vertical angle v = self.get_laser_plane_intersection(v) # Ignore any vertices that have negative z coordinates (pre scaling) if v[2] < 0: continue x,y,z = v*self.scale x,y,z = rotate('z', v, self.rotation_angle) vertex = Vertex(x, y, z) processed_frame.append(vertex) self.processed_frames.append(processed_frame) def show_frame(self): cv2.line(self.display_image, (self.width/2, 0), (self.width/2, self.height), (255, 255, 255)) cv2.line(self.display_image, (0, self.height/2), (self.width, self.height/2), (255, 255, 255)) if self.platform_middle: cv2.line( self.display_image, (self.platform_middle[0]-10, self.platform_middle[1]), (self.platform_middle[0]+10, self.platform_middle[1]), (255, 0, 0) ) cv2.line( self.display_image, (self.platform_middle[0], self.platform_middle[1]-10), (self.platform_middle[0], self.platform_middle[1]+10), (255, 0, 0) ) if self.area and self.area.is_complete(): x1, y1, x2, y2 = [int(c) for c in self.area.to_tuple()] cv2.rectangle(self.display_image, (x1, y1), (x2, y2), (255, 100, 0)) for index, (timestamp, message) in enumerate(self.ui_messages): cv2.putText(self.display_image, message, (10, self.height-10-15*index), cv2.FONT_HERSHEY_COMPLEX_SMALL, 0.8, (255, 255, 255)) cv2.imshow(self.window_name, self.display_image) def rotate(self): self.rotation_angle += self.rotation_step self.send_command('s') def closest_vertex_y(self, y, frame): """ Finds vertex in frame the y coordinate of which is closest to y """ 'Find rightmost value less than or equal to x' return max(0, bisect_right([v.y for v in frame], y)-1) def save_image(self): f = open('output.obj', 'w') print "writing to file..." for frame in self.processed_frames: for vertex in frame: f.write('v %s %s %s\n' % vertex.to_tuple()) f.close() self.processed_frames = [] print "Done writing output file." def red_filter(self, image): begin_lower_border = numpy.array([0, 50, 20]) begin_upper_border = numpy.array([35, 255, 255]) end_lower_border = numpy.array([145, 50, 20]) end_upper_border = numpy.array([180, 255, 255]) white_lower_border = numpy.array([0, 0, 180]) white_upper_border = numpy.array([180, 255, 255]) image = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) mask1 = cv2.inRange(image, begin_lower_border, begin_upper_border) mask2 = cv2.inRange(image, end_lower_border, end_upper_border) mask3 = cv2.inRange(image, white_lower_border, white_upper_border) mask = cv2.bitwise_or(mask1, mask2) mask = cv2.bitwise_or(mask, mask3) image = cv2.bitwise_and(image, image, mask=mask) image = cv2.cvtColor(image, cv2.COLOR_HSV2BGR) return image def loop(self): frame = 0 self.calibrate() while self.keep_running: self.handle_keyboard() if self.mode == 'scan' or self.mode == 'stop_scan': thresholded_frame = self.capture_diff(thresholded=True) if self.rotation_angle > math.pi*2 or self.mode == 'stop_scan': self.save_image() self.mode = None self.rotation_angle = 0 self.set_laser(False) continue self.process_frame(thresholded_frame) if frame % 100: debug(self.rotation_angle) self.rotate() time.sleep(0.1) else: self.display_image = self._capture_frame() self.clear_old_messages() self.show_frame() frame += 1