fingertracker_skeleton.py

from fingertracker import FingerTracker
from SimpleCV import Camera, Display, Color, cv
from SimpleCV.Features import Detection
import numpy as np
import scipy
import scipy.signal
from tracking import TrackerIn, util
from math import sqrt
import sys
import time
import random
import math
import geometry
import networkx as nx

class FingerTrackerSkeleton(FingerTracker):
    """Finger tracking using peak-findings
    """

    def __init__(self, camera=None):
        """Initialize the finger tracker

        :param TrackerIn ti: Tracker input
        :param camera: Camera index, filename, or None
        """
        FingerTracker.__init__(self, camera)
        self.display = None

    def crop_img(self, img):
        return img.crop(0, 150, img.width, img.height-150).scale(0.5)
        #return img.crop(490,95,img.width-1000, img.height-290).rotate(90, fixed=False)

    def find_fingers5(self, img):
        if img is None:
            return []

        hands = img.edges().invert().erode(2) & img.getSkintoneMask().dilate(1).erode(1)
        hands2 = hands.dilate(10)
        hands2Blobs = hands2.findBlobs(minsize=1200)
        if hands2Blobs is None:
            return []
        for blob in hands2Blobs:
            x, y = blob.centroid()
            blob.draw()
            hands.dl().rectangle((x - 150, y+30), (300, 500), filled=True, color=Color.BLACK)
            hands.dl().rectangle((x - 150, y-230), (300, 200), filled=True, color=Color.BLACK)
        hands = hands.applyLayers()
        hands3 = hands & hands.skeletonize(5)
        blobs = hands3.dilate(2).findBlobs(minsize=10)
        if not blobs:
            return []
        blobs.sort(key=lambda b:-b.area())
        for blob in blobs[:10]:
            x, y = blob.centroid()
            hands3.dl().circle((x, y), 15, color=Color.YELLOW)
        img.addDrawingLayer(hands3.dl())

        return [blob.centroid() for blob in blobs[:10]]

    def get_line(self, cx, cy, angle, img, drawimg, start):
        if abs(angle) < 30:
            return []

        res = []
        for x, y in zip([cx-5, cx, cx+5], [cy-5, cy, cy+5]):
            if x < 0 or x >= img.width or y < 0 or y >=img.height:
                continue
            if angle > 0:
                ty = 0
                tx = x - y / math.tan(angle / 180.0 * math.pi)
                if tx > img.width:
                    continue
                elif tx < 0:
                    continue
            else:
                ty = 0
                tx = x - y / math.tan(angle / 180.0 * math.pi)
                if tx > img.width:
                    continue
            intersections = img.edgeIntersections((int(x), int(y)), (int(tx), int(ty)))

            if intersections[0] is not None:
                l = Detection.Line(drawimg, [start, intersections[0]])
                res.append(l)
                l.draw()
        return res

    def find_fingers4(self, img):
        self.NUM_FEATURES = 22
        res = [0 for _ in range(self.NUM_FEATURES)]
        if img is None:
            return res


        hands = img.edges().invert().erode(2) & img.getSkintoneMask().dilate(1).erode(1)
        skel = hands.skeletonize()

        lines = []
        for line in skel.findLines(maxlinegap=30, threshold=40):
            lines.extend(self.get_line(line.coordinates()[0],
                                       line.coordinates()[1],
                                       line.angle(),
                                       img,
                                       skel,
                                       line.points[3 if line.angle() > 0 else 0]))

        line_sets = geometry.cluster_lines(lines)
        NUM_FINGERS = 4
        if self.lines is None or len(self.lines) < NUM_FINGERS:
            # Set up the initial values of the hidden state
            self.lines = [geometry.combine_lines(skel, ls) for ls in line_sets]
            self.line_weights = [1.0 for ls in line_sets]
            self.line_speeds = [np.array([0, 0]) for ls in line_sets]
            img.addDrawingLayer(skel.dl())
            return res
        else:
            # Propagate our hidden state in time
            for i, ln in enumerate(self.lines):
                self.lines[i] = geometry.offset_line(ln, self.line_speeds[i])

            # Match each observed line with one in the hidden state
            new_lines = [geometry.combine_lines(skel, ls) for ls in line_sets]
            G = nx.Graph()
            G.add_nodes_from(new_lines)
            G.add_nodes_from(self.lines)
            for l1 in new_lines:
                for i, l2 in enumerate(self.lines):
                    if self.line_weights[i] < 0.000001:
                        # A non-present line has a fixed, positive weight, so it is
                        # preferable to pick it than to shift over all other lines
                        # in the image by one finger
                        w = 5000
                    else:
                        w = 10000-geometry.lines_cost(l1, l2)
                    G.add_edge(l1, l2, weight=w)
            mate = nx.max_weight_matching(G)

            #Update the hidden state based on the observations and matches
            for i, ln in enumerate(self.lines):
                if ln in mate:
                    # If a line has a match, replace the hidden state with the
                    # observation (TODO: something smarter)
                    self.lines[i] = mate[ln]
                    old_weight = self.line_weights[i]
                    self.line_weights[i] = 1.0
                    # Estimate the speed of the line
                    new_speed = np.array(mate[ln].coordinates()) - np.array(ln.coordinates())
                    self.line_speeds[i] = self.line_speeds[i] * 0.1 + new_speed * 0.9
                    if old_weight < 0.00001:
                        self.line_speeds[i] = np.array([0, 0])
                    # Consider small movements to be due to noise
                    if self.line_speeds[i][0] < 5.0:
                        self.line_speeds[i][0] = 0
                    if self.line_speeds[i][1] < 15.0:
                        self.line_speeds[i][1] = 0
                else:
                    # Keep the hidden state the same, but decrease the weight
                    # Also decrease the speed to prevent runaway lines
                    self.lines[i].image = skel
                    self.line_weights[i] *= 0.7
                    self.line_speeds[i] *= 0.7


            # We might have multiple intersecting lines in the hidden state,
            # but we want only one line per finger
            # In this case, discard the one that has a smaller weight
            for i, ln in enumerate(self.lines):
                for j, ln2 in enumerate(self.lines[:i]):
                    if geometry.lines_distance(ln, ln2) < 0.2:
                        if self.line_weights[i] > self.line_weights[j]:
                            self.lines[j] = Detection.Line(skel, ((0, 0),(0,0)))
                            self.line_weights[j] = 0
                        else:
                            self.lines[i] = Detection.Line(skel, ((0, 0),(0,0)))
                            self.line_weights[i] = 0


        # Display
        colors = [Color.RED, Color.ORANGE, Color.YELLOW, Color.GREEN,
                  Color.BLUE, Color.VIOLET, Color.BLACK, Color.WHITE]

        for i, line, color in zip(range(100), self.lines, colors):
            if self.line_weights[i] > 0.5:
                w = 3
            elif self.line_weights[i] > 0.2:
                w = 2
            elif self.line_weights[i] > 0.05:
                w = 1
            else:
                continue

        res = self.extract_line_features(img.width, img.height)
        for i, f in enumerate(res):
            if math.isinf(f) or math.isnan(f):
                res[i] = 0.0
        assert len(res) == self.NUM_FEATURES, "Feature number mismatch"
        img.addDrawingLayer(skel.dl())
        return res

    def extract_line_features(self, width, height):
        lines = [self.lines[i] for i in range(len(self.lines)) if self.line_weights[i] > 0.001]
        sorted_lines = sorted(lines, key=lambda x: x.coordinates()[0])

        def split_weight(l1, l2):
            "Weight assigned to splitting the list into hands between these two lines"
            res = abs(l2.coordinates()[0] - l1.coordinates()[0])
            if l2.angle() > 0 and l1.angle() <= 0:
                res += 200
            return res
        if len(sorted_lines) < 2:
            j = 100
        else:
            j = np.argmax([split_weight(sorted_lines[i], sorted_lines[i+1]) for i in range(len(sorted_lines)-1)])
        if split_weight(sorted_lines[j], sorted_lines[j+1]) < 60:
            h1 = sorted_lines
            h2 = []
        else:
            h1, h2 = sorted_lines[:j+1], sorted_lines[j+1:]
        if not h2 and h1:
            if np.average([x.coordinates()[0] for x in h1]) > width*0.5:
                h2, h1 = h1, h2

        for l in h1:
            l.draw(color=Color.GREEN, width=3)

        for l in h2:
            l.draw(color=Color.RED, width=3)

        return (self.extract_hand_features(h1, width, height)
                + self.extract_hand_features(h2, width, height)
                + self.extract_total_features(sorted_lines, width, height))

    def extract_total_features(self, lines, width, height):
        return []

    def extract_hand_features(self, hand, width, height):
        relangles = [abs(hand[i+1].angle() - hand[i].angle()) for i in range(len(hand)-1)]
        relangle_hist = np.histogram(relangles, bins = 18, range=(0, 90))[0][:10]

        angle_std = np.std([x.angle() for x in hand])

        return list(relangle_hist) + [angle_std]

    def run_frame(self, ti, img):
        """Run the algorithm for one frame

        :param TrackerIn ti: TrackerIn object to send events to
        :return: True if I should be called with the next frame
        """
        img = self.crop_img(img)

        if self.display is None:
            # Consume one frame for the initialization
            self.display = Display(img.size())
            self.last_time = time.time()
            self.lines = None
            self.line_weights = None
            self.line_speeds = None
            return True
        elif self.display.isDone():
            return False

        #positions = self.find_fingers4(img)
        print self.find_fingers4(img)
        di = img

        #self.add_positions(ti, positions)

        fps = 1.0 / (time.time() - self.last_time)
        di.dl().ezViewText("{0:.3f} fps".format(fps), (0, 0))
        di.save(self.display)

        self.last_time = time.time()
        if self.display.mouseLeft or self.display.mouseRight:
            self.display.done = True
            return False
        else:
            return True

    def finish(self):
        if self.display is not None:
            self.display.done = True
            self.display.quit()

if __name__=="__main__":
    color_tracker = FingerTrackerSkeleton()
    def run(ti):
        color_tracker.run(ti)
    util.run_async_consumer(run, util.print_consumer)