class TrackBase: def __init__(self, trackId, rect, img, mask): self.trackId = trackId self.lastClass = 'unknown' self.lastKeypoints = None self.rect = [] self.searchWindow = rect self.template = [] self.tracklet = [] self.trackletRects = [] self.direction = Enum(['LEFT', 'RIGHT', 'UNKNOWN']) self.actions = [] self.actions.append('unknown') self.classHistory = [] self.active = True self.occluded = False self.foundCount = 0 self.numTemplates = 6 self.outDimens = None #self.trackOutputStream = None self.doTrackWrite = False self.savedFrames = [] self.outFps = 30 height, width = img.shape[:2] self.imgBounds = (width, height) x, y, w, h = rect x, y, w, h = rect = self.resizeRect(rect, (1.0, 1.0), (0.0, 0.0)) #x,y,w,h = rect = self.resizeRect(rect,(1.0,0.75),(-0.15,-0.2)) #crop syntax is [starty:endy, startx:endx] template = img[y:y + h, x:x + w] #maskFiltered = mask[y:y+h, x:x+w] # 3 Kalman filters per research document self.kalmanFast = Kalman((x + (w / 2), y + (h / 2))) self.kalmanArea = Kalman((w, h)) # ,0.003,0.3) self.kalmanSlow = Kalman((x + (w / 2), y + (h / 2)), 0.003, 0.03) self.updateTemplate(rect, template) #x,y,w,h = rect = self.resizeRect(rect,(1.5,1.3),(0,0.5)) self.trackletSize = (0, 0) self.updateTracklet(rect, img) # set up the ROI for tracking #roi = template #hsv_roi = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV) #maskFiltered = cv2.inRange(hsv_roi, np.array((0., 150.,60.)), np.array((180.,255.,255.))) #cv2.imshow('maskFiltered',maskFiltered) #self.roi_hist = cv2.calcHist([hsv_roi],[0,1],maskFiltered,[36,50],[0,180,0,255]) #cv2.normalize(self.roi_hist,self.roi_hist,0,255,cv2.NORM_MINMAX) # returns true if the provided point plus current track bounds go over the edge of the frame, false otherwise def isInBounds(self, point, bounds): bh, bw = bounds px, py = point x, y, w, h = self.getNewestRect() if px + w / 2 > bw or px - w / 2 < 0: return False if py + h / 2 > bh or py - h / 2 < 0: return False return True # search for this track in the provided frame by whatever concrete algorithm the track implements def update(self, img): self.savedFrames.append(img) # always do predictions before measurements pLoc = self.kalmanFast.predict() self.kalmanSlow.predict() self.kalmanArea.predict() # make sure we haven't already gone off the frame if not self.isInBounds(pLoc, img.shape[:2]): self.active = False print('Track #' + repr(self.trackId) + ' ' + repr(self.occluded) + ' ENDING TRACK: gone off frame ' + repr(pLoc)) return newroi = (0, 0, 0, 0) # search phase if not self.occluded: self.occluded, newroi = self.doNonOccludedSearch(img) else: self.occluded, newroi = self.doOccludedSearch(img) # update phase if not self.occluded: self.doNonOccludedUpdate(newroi, img) else: self.doOccludedUpdate(newroi, img) #if not self.trackOutputStream is None and self.doTrackWrite: #w,h = self.outDimens #resized = cv2.resize(self.getNewestTemplate(), (w,h)) # self.trackOutputStream.write(self.getNewestTracklet()) #print('Track #' + repr(self.trackId) + ' ' + repr(self.occluded) + ' ROI: ' + repr(self.rect[0])) # perform search for tracked object with the assumption it was occluded in the previous frame # default implementation provides a global template match for previously known good template def doOccludedSearch(self, img): # set search window so we're not performing a global search roi = self.resizeRect(self.getNewestRect(), (4, 4)) x, y, w, h = roi newroi = (0, 0, 0, 0) self.searchWindow = roi window = img[y:y + h, x:x + w] temp = self.getOldestTemplate() # make sure we haven't already gone off the frame if window.shape[:2][0] <= 0 or window.shape[:2][0] < temp.shape[:2][ 0] or window.shape[:2][1] <= 0 or window.shape[:2][ 1] < temp.shape[:2][1]: self.active = False print('Track #' + repr(self.trackId) + ' ' + repr(self.occluded) + ' ENDING TRACK: match window off frame ' + repr(window.shape[:2])) return True, newroi #print('Track #' + repr(self.trackId) + ' window:' + repr(window.shape[:2]) + ' temp:' + repr(temp.shape[:2])) #cv2.imshow('template',temp) # perform the template matching search res = cv2.matchTemplate(window, temp, cv2.TM_CCOEFF_NORMED) min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(res) if max_val >= 0.7: #found match px, py = max_loc x, y, w, h = roi x2, y2, w2, h2 = self.getOldestRect() newroi = (px + x, py + y, w2, h2) return False, newroi else: return True, newroi #print('Track #' + repr(self.trackId) + ' ' + repr(self.occluded) + ' CORR: ' + repr(max_val)) # perform search for tracked object with the assumption it was not occluded in the previous frame # all subclasses should implement this method with their specific tracking algorithm def doNonOccludedSearch(self, img): return # updates the track when it was not found in the search frame using Kalman filter predictions def doOccludedUpdate(self, roi, img): if self.foundCount < 10: pLocS = self.kalmanFast.getLastPrediction() else: pLocS = self.kalmanSlow.getLastPrediction() x, y, w, h = self.getOldestRect() newRoi = self.resizeRect( (int(pLocS[0] - (w / 2)), int(pLocS[1] - (h / 2)), w, h), (1.0, 1.0)) self.updateTemplate(newRoi, None) self.updateTracklet(newRoi, img) # don't want missing frames in tracklet # updates the track with a real measurement when it was found in the search frame def doNonOccludedUpdate(self, roi, img): self.foundCount = self.foundCount + 1 x, y, w, h = roi # correct Kalman filters self.kalmanFast.correct((x + (w / 2), y + (h / 2))) self.kalmanSlow.correct((x + (w / 2), y + (h / 2))) self.kalmanArea.correct((w, h)) self.updateTemplate(roi, img[y:y + h, x:x + w]) self.updateTracklet(roi, img) # TODO: remove when deemed no longer necessary # callback when a potential seed roi is detected near this def onFindIntersect(self, roi, img): return # returns true if provided roi is near this track position def matches(self, roi, img): if self.intersects(roi, 100): self.onFindIntersect(roi, img) return True else: return False def updateTemplate(self, rect, template): x, y, w, h = rect #self.rect.insert( 0,rect ) if not rect is None: self.rect.append(rect) if len(self.rect) > self.numTemplates: del self.rect[0] #del self.rect[self.numTemplates-1] #self.template.insert( 0,template ) if not template is None: self.template.append(template) if len(self.template) > self.numTemplates: del self.template[0] #del self.template[self.numTemplates-1] def updateTracklet(self, rect, img): #x,y,w,h = rect = self.resizeRect(rect,(1.3,1.5),(-0.15,0.25)) #wave x, y, w, h = rect = self.resizeRect(rect, (1.2, 1.5), (0.0, 0.25)) if self.trackletSize[0] == 0: self.trackletSize = (w, h) else: w, h = self.trackletSize #x = rect[0] #y = rect[1] roiGray = cv2.cvtColor(img[y:y + h, x:x + w], cv2.COLOR_BGR2GRAY) self.tracklet.append(roiGray) self.trackletRects.append((x, y, w, h)) #if not self.trackOutputStream is None and self.doTrackWrite: # self.trackOutputStream.write(img[y:y+h, x:x+w]) # returns true is the provided rectangle touches this track's known rectangle # based on a circle drawn from the center def intersects(self, rect, dmin=100): x1, y1, w1, h1 = rect x2, y2, w2, h2 = self.getNewestRect() center1 = (int(x1 + w1 / 2), int(y1 + h1 / 2)) center2 = (int(x2 + w2 / 2), int(y2 + h2 / 2)) # radius of search area radius1 = int(math.hypot(w1, h1) / 2) radius2 = int(math.hypot(w2, h2) / 2) # distance between centers dist = int(math.hypot(center1[0] - center2[0], center1[1] - center2[1])) #cv2.line(img, center1, center2, (0,255,255),1) #cv2.circle(img, center1, radius1, (0,0,255), 1) #cv2.circle(img, center2, radius2, (0,255,255), 1) return dist < (radius1 + radius2) or dist < dmin # return the input rectangle resized by given area coefficient and bounds checked against provided bounds def resizeRect(self, rect, scale=(1.0, 1.0), offsets=(0, 0)): x, y, w, h = rect width, height = self.imgBounds if x < 0: x = 0 sx = x - (w * (scale[0] - 1.0)) / 2 sx = sx + offsets[0] * w if sx < 0: sx = 0 if sx >= width: sx = width - 1 ex = sx + (w * scale[0]) if ex >= width: ex = width - 1 if y < 0: y = 0 sy = y - (h * (scale[1] - 1.0)) / 2 sy = sy + offsets[1] * h if sy < 0: sy = 0 if sy >= height: sy = height - 1 ey = sy + (h * scale[1]) if ey >= height: ey = height - 1 return (int(sx), int(sy), int(ex - sx), int(ey - sy)) # Returns a measure of how close the provided image's histogram matches this one. Lower value = more similar def histogramsMatch(self, img1, img2, thresh=0.6): hsv_roi1 = cv2.cvtColor(img1, cv2.COLOR_BGR2HSV) hist1 = cv2.calcHist([hsv_roi1], [0, 1], None, [36, 50], [0, 180, 0, 255]) cv2.normalize(hist1, hist1, 0, 255, cv2.NORM_MINMAX) hsv_roi2 = cv2.cvtColor(img2, cv2.COLOR_BGR2HSV) hist2 = cv2.calcHist([hsv_roi2], [0, 1], None, [36, 50], [0, 180, 0, 255]) cv2.normalize(hist2, hist2, 0, 255, cv2.NORM_MINMAX) score = cv2.compareHist(hist1, hist2, cv2.HISTCMP_BHATTACHARYYA) return score <= thresh, score # TODO: resize templates more intelligently # returns true if the provided images match, false otherwise def templatesMatch(self, img1, img2, thresh=0.3): #cv2.imshow('template',img) h1, w1 = img1.shape[:2] h2, w2 = img2.shape[:2] w = min((w1, w2)) h = min((h1, h2)) if w <= 0 or h <= 0: return False, 0.0 x1 = (w1 - w) / 2 y1 = (h1 - h) / 2 x2 = (w2 - w) / 2 y2 = (h2 - h) / 2 crop1 = img1[y1:y1 + h, x1:x1 + w] crop2 = img2[y2:y2 + h, x2:x2 + w] #resized = cv2.resize(img2, (w,h)) #print('Track #' + repr(self.trackId) + ' img1:' + repr(img1.shape[:2]) + ' img2:' + repr(img2.shape[:2]) + ' resize:' + repr(resized.shape[:2])) #cv2.imshow('resized',resized) res = cv2.matchTemplate(crop1, crop2, cv2.TM_CCOEFF_NORMED) min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(res) return max_val >= thresh, max_val def getNewestRect(self): #return self.rect[0] return self.rect[-1] def getTrackletRect(self): return self.trackletRects[-1] def getTrackletRects(self): return self.trackletRects def saveLastTracklet(self): lasttracklet = self.getLastTracklet(45) if not lasttracklet is None: h, w = lasttracklet[0].shape[:2] outputStream = cv2.VideoWriter( 'track' + repr(self.trackId) + '.avi', cv2.VideoWriter_fourcc(*'DIVX'), self.outFps, (w, h)) for frame in lasttracklet: outputStream.write(frame) outputStream.release() def getLastTracklet(self, numFrames): if len(self.trackletRects) > numFrames: h, w = self.savedFrames[0].shape[:2] temptracklet = [] #temprects = [] xmax, xmin, ymax, ymin = (0, w, 0, h) for rect in self.trackletRects[-numFrames:]: x, y, w, h = rect xmin = x if x < xmin else xmin xmax = (x + w) if (x + w) > xmax else xmax ymin = y if y < ymin else ymin ymax = (y + h) if (y + h) > ymax else ymax for frame in self.savedFrames[-numFrames:]: temptracklet.append(frame[ymin:ymin + (ymax - ymin), xmin:xmin + (xmax - xmin)]) return temptracklet else: return None def getNewestTracklet(self): return self.tracklet[-1] def getOldestRect(self): return self.rect[0] #if len(self.rect) < self.numTemplates : # return self.rect[len(self.rect)-1] #else: # return self.rect[self.numTemplates-1] def getNewestTemplate(self): return self.template[-1] def getOldestTemplate(self): return self.template[0] #if len(self.template) < self.numTemplates : # return self.template[len(self.template)-1] #else: # return self.template[self.numTemplates-1] def getPath(self): return self.kalmanFast.getPath() def getPredicted(self): return self.kalmanFast.getPredicted() def getTrackId(self): return self.trackId def isOccluded(self): return self.occluded def getSearchWindow(self): return self.searchWindow def isActive(self): return self.active def updateClass(self, classType, kp): self.lastKeypoints = kp self.lastClass = classType self.classHistory.append(classType) try: lc = mode(self.classHistory) self.lastClass = lc return self.lastClass except StatisticsError: return self.lastClass def getLastClass(self): return self.lastClass def getLastKeypoints(self): return self.lastKeypoints def getTracklet(self): return self.tracklet def updateAction(self, action): self.actions.append(action) return self.actions[-1] def getLastAction(self): return self.actions[-1] def setDoTrackWrite(self, doWrite): self.doTrackWrite = doWrite def setOutFps(self, fps): self.outFps = fps
measurements = np.genfromtxt(measurements_path, delimiter=",") kalman = Kalman() kalman.init(dt, u, std_acc, std_pos) for i in range(len(image_names)): image = cv2.imread(image_folder_path + image_names[i]) measurement = measurements[i].astype(np.int) # Given measurement color BLUE cv2.rectangle(image, (measurement[0], measurement[1]), (measurement[0]+measurement[2], measurement[1]+measurement[3]), (255, 0, 0), 2) (x, y, vx, vy) = kalman.predict() # Predicted state color GREEN cv2.rectangle(image, (x, y), (x + measurement[2], y + measurement[3]), (0, 255, 0), 2) update = np.array([[measurement[0]+(measurement[2]/2)], [measurement[1]+(measurement[3]/2)]]) (x1, y1, vx1, vy1) = kalman.correct(update) # Estimated state color WHITE cv2.rectangle(image, (x1, y1), (x1 + measurement[2], y1 + measurement[3]), (255, 255, 255), 2) cv2.putText(image, "Estimated velocity: " + str((vx1[0], vy1[0])), (20, 680), 0, 0.5, (255, 255, 255), 2) cv2.putText(image, "Predicted velocity: " + str((vx[0], vy[0])), (300, 680), 0, 0.5, (0, 255, 0), 2) cv2.imshow("Frame", image) cv2.waitKey(0)