def __updateObservation(self, bbox_new, idx, X, Cov): """ Update observation model in Euclidean coordinate system :param bbox_new: :param idx: :param X: :param Cov: :return: """ bbox_old = X[idx] cov = Cov[idx] K = cov.dot(inv(cov + self.Q)) z = np.array((bbox_new.xmin, bbox_new.ymin, bbox_new.xmax, bbox_new.ymax)).astype(float) x = np.array((bbox_old.xmin, bbox_old.ymin, bbox_old.xmax, bbox_old.ymax)).astype(float) #print(cov) #print(inv(cov + self.Q)) #print(K) #print(z - x) x = x + K.dot((z - x)) #x = z cov = (np.identity(4) - K).dot(cov) X[idx].xmin = x[0] X[idx].ymin = x[1] X[idx].xmax = x[2] X[idx].ymax = x[3] Cov[idx] = cov bbox_klt = self.__bbox_msg2np([X[idx]]) startXs, startYs = getFeatures(cv2.cvtColor(self.image_old, cv2.COLOR_RGB2GRAY), bbox_klt, use_shi=False) self.startXs[:, idx] = startXs[:, 0] self.startYs[:, idx] = startYs[:, 0] self.bboxes_klt[idx] = bbox_klt[0]
def __updateMotion(self, new_image, prediction): newXs, newYs = estimateAllTranslation(self.startXs, self.startYs, self.image_old, new_image) if prediction: #print(1, self.Cov[0]) Xs, Ys, self.bboxes_klt, self.Cov = applyGeometricTransformation(self.startXs, self.startYs, newXs, newYs, self.bboxes_klt, self.Cov, self.R) #print(1, self.Cov[0]) else: Xs, Ys, self.bboxes_klt = applyGeometricTransformation(self.startXs, self.startYs, newXs, newYs, self.bboxes_klt) # update coordinates self.startXs = Xs self.startYs = Ys # update registration bboxes_klt = copy.deepcopy(self.bboxes_klt) height, width, _ = self.image_old.shape for i, bbox in reversed(list(enumerate(bboxes_klt))): # deregistration: remove bbox out of frame if (bbox.min() < 0) or (bbox[2, 0] > width) or (bbox[2, 1] > height): del self.X[i] del self.Cov[i] self.bboxes_klt = np.delete(self.bboxes_klt, i, 0) #print(self.startXs.shape) self.startXs = np.delete(self.startXs, i, 1) self.startYs = np.delete(self.startYs, i, 1) else: # update bbox self.X[i].xmin = bbox[0][0] self.X[i].ymin = bbox[0][1] self.X[i].xmax = bbox[2][0] self.X[i].ymax = bbox[2][1] # generate new features if needed n_features_left = np.sum(self.startXs[:, i] != -1) if n_features_left < 15: print('Generate New KLT Features') bbox = np.expand_dims(self.bboxes_klt[i], 0) startXs, startYs = getFeatures(cv2.cvtColor(self.image_old, cv2.COLOR_RGB2GRAY), bbox) self.startXs[:, i] = startXs[:, 0] self.startYs[:, i] = startYs[:, 0]
def __init__(self): bbox_nn_sub = rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, self.bbox_nn_callback, queue_size=10) raw_image_sub = rospy.Subscriber('/r200/depth/image_raw', Image, self.raw_image_callback, queue_size=10) self.bridge = CvBridge() self.pub = rospy.Publisher("/bbox_tracker/detection_image", Image, queue_size=1) # debug self.bboxes_new = [] self.X = [] # a list of darknet_ros_msgs/BoundingBox self.Cov = [] # this is in Euclidean coordinate system self.image_new = np.zeros(1) self.image_old = np.zeros(1) self.startXs = np.empty((20, 0), int) self.startYs = np.empty((20, 0), int) self.bboxes_klt = np.empty((0, 4, 2), float) self.observation_done = False self.Q = np.diag((.2, .2, .2, .2)) # observation model noise covariance in Euclidean coordinate system self.R = np.diag((10., 10., 0., 10., 10., 0.)) # motion model noise covariance in Homogeneous coordinate system #self.Q = np.diag((.01, .01, .01, .001)) # observation model noise covariance in Euclidean coordinate system #self.R = np.diag((.01, .01, 0., .01, .01, 0.)) # motion model noise covariance in Homogeneous coordinate system rospy.loginfo("bbox_tracker initialized!") while not rospy.is_shutdown(): # update observation model if not self.observation_done: if len(self.bboxes_new) > 0: if len(self.image_new.shape) == 3: self.observation_done = True seconds = rospy.get_time() # clear cache bboxes_new = copy.deepcopy(self.bboxes_new) X = copy.deepcopy(self.X) Cov = copy.deepcopy(self.Cov) self.bboxes_new = [] # update image self.image_old = copy.deepcopy(self.image_new) self.image_new = np.zeros(1) for i, bbox_new in enumerate(bboxes_new): # check if new bbox is existing idx = self.__checkRegistration(bbox_new, X) if idx == -1: X.append(bbox_new) Cov.append(self.Q) bbox_klt = self.__bbox_msg2np([bbox_new]) startXs, startYs = getFeatures(cv2.cvtColor(self.image_old, cv2.COLOR_RGB2GRAY), bbox_klt, use_shi=False) self.startXs = np.append(self.startXs, startXs, axis=1) self.startYs = np.append(self.startYs, startYs, axis=1) self.bboxes_klt = np.append(self.bboxes_klt, bbox_klt, axis=0) else: self.__updateObservation(bbox_new, idx, X, Cov) img = copy.deepcopy(self.image_old) image_msg = self.__draw_BBox(img, X) self.pub.publish(image_msg) seconds = rospy.get_time() - seconds self.X = copy.deepcopy(X) self.Cov = copy.deepcopy(Cov) print("obs " + str(len(self.X)) + " " + str(seconds)) if len(self.image_new.shape) == 3: if len(self.X) > 0: image_new = copy.deepcopy(self.image_new) self.image_new = np.zeros(1) seconds = rospy.get_time() self.__updateMotion(image_new, prediction=self.observation_done) seconds = rospy.get_time() - seconds print("mot " + str(len(self.X)) + " " + str(seconds) + " " + str(self.observation_done)) self.observation_done = False self.image_old = copy.deepcopy(image_new) image_msg = self.__draw_BBox(copy.deepcopy(self.image_old), self.X) self.pub.publish(image_msg)
def tracking(self): # update observation model while not rospy.is_shutdown(): if not self.observation_done: if len(self.bboxes_new) > 0: if len(self.image_new.shape) == 3: seconds = rospy.get_time() # clear cache bboxes_new = copy.deepcopy(self.bboxes_new) X = copy.deepcopy(self.X) Cov = copy.deepcopy(self.Cov) traces = copy.deepcopy(self.traces) self.bboxes_new = [] # update image self.image_old = copy.deepcopy(self.image_new) self.image_new = np.zeros(1) for i, bbox_new in enumerate(bboxes_new): # check if new bbox is existing idx = self.__checkRegistration(bbox_new, X) if idx == -1: bbox_klt = self.__bbox_msg2np([bbox_new]) startXs, startYs = getFeatures(cv2.cvtColor( self.image_old, cv2.COLOR_RGB2GRAY), bbox_klt, use_shi=False) n_features_left = np.sum(startXs != -1) if n_features_left < 15: continue center_list = [ (int((bbox_new.xmin + bbox_new.xmax) / 2.), int((bbox_new.ymin + bbox_new.ymax) / 2.)) ] X.append(bbox_new) Cov.append(self.Q) traces.append(center_list) self.startXs = np.append(self.startXs, startXs, axis=1) self.startYs = np.append(self.startYs, startYs, axis=1) self.bboxes_klt = np.append(self.bboxes_klt, bbox_klt, axis=0) self.observation_done = True else: self.__updateObservation(bbox_new, idx, X, Cov) center = (int( (bbox_new.xmin + bbox_new.xmax) / 2.), int((bbox_new.ymin + bbox_new.ymax) / 2.)) traces[idx].append(center) self.observation_done = True img = copy.deepcopy(self.image_old) image_msg = self.__draw_BBox(img, X) self.pub.publish(image_msg) seconds = rospy.get_time() - seconds self.X = copy.deepcopy(X) self.Cov = copy.deepcopy(Cov) self.traces = copy.deepcopy(traces) print("obs " + str(len(self.X)) + " " + str(seconds)) self.__publish_bbox() # update motion model if len(self.image_new.shape) == 3: if len(self.X) > 0: image_new = copy.deepcopy(self.image_new) self.image_new = np.zeros(1) seconds = rospy.get_time() # self.__updateMotion(image_new, prediction=self.observation_done) self.__updateMotion(image_new, prediction=True) seconds = rospy.get_time() - seconds print("mot " + str(len(self.X)) + " " + str(seconds) + " " + str(self.observation_done)) self.observation_done = False self.image_old = copy.deepcopy(image_new) image_msg = self.__draw_BBox(copy.deepcopy(self.image_old), self.X) self.pub.publish(image_msg) self.__publish_bbox()
def __updateMotion(self, new_image, prediction): newXs, newYs = estimateAllTranslation(self.startXs, self.startYs, self.image_old, new_image) if prediction: # print(1, self.Cov[0]) Xs, Ys, self.bboxes_klt, self.Cov = applyGeometricTransformation( self.startXs, self.startYs, newXs, newYs, self.bboxes_klt, self.Cov, self.R) # print(1, self.Cov[0]) else: Xs, Ys, self.bboxes_klt = applyGeometricTransformation( self.startXs, self.startYs, newXs, newYs, self.bboxes_klt) # update coordinates self.startXs = Xs self.startYs = Ys # update registration bboxes_klt = copy.deepcopy(self.bboxes_klt) height, width, _ = self.image_old.shape for i, bbox in reversed(list(enumerate(bboxes_klt))): # deregistration 1: measure differential entropy to remove false positive detection cov = self.Cov[i] DE = 5.675754132818691 + np.log( det(cov) ) / 2.0 # differential entropy for Multivariate normal distribution print('DE: ', DE) if DE >= 16.: # it was 16. del self.X[i] del self.Cov[i] del self.traces[i] self.bboxes_klt = np.delete(self.bboxes_klt, i, 0) self.startXs = np.delete(self.startXs, i, 1) self.startYs = np.delete(self.startYs, i, 1) continue # deregistration 2: remove bbox out of frame if (bbox.min() < 0) or (bbox[2, 0] > width) or (bbox[2, 1] > height): del self.X[i] del self.Cov[i] del self.traces[i] self.bboxes_klt = np.delete(self.bboxes_klt, i, 0) self.startXs = np.delete(self.startXs, i, 1) self.startYs = np.delete(self.startYs, i, 1) else: # update bbox self.X[i].xmin = bbox[0][0] self.X[i].ymin = bbox[0][1] self.X[i].xmax = bbox[2][0] self.X[i].ymax = bbox[2][1] center = (int((self.X[i].xmin + self.X[i].xmax) / 2.), int((self.X[i].ymin + self.X[i].ymax) / 2.)) self.traces[i].append(center) # generate new features if needed n_features_left = np.sum(self.startXs[:, i] != -1) if n_features_left < 15: print('Generate New KLT Features') bbox = np.expand_dims(self.bboxes_klt[i], 0) startXs, startYs = getFeatures( cv2.cvtColor(self.image_old, cv2.COLOR_RGB2GRAY), bbox) self.startXs[:, i] = startXs[:, 0] self.startYs[:, i] = startYs[:, 0]