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
