def _match_frame(self, frame, prev_gt_matches):
"""
@type frame: int
@type prev_gt_matches: list
@return: gt_matches - list of measurement ids to that the ground truth objects match
None for gt objects not present in the frame
gt_distances - distances from ground truth objects to matched measured objects
None for objects not found in the frame
measurements_matches - list of ground truth ids to that the measured objects match
@rtype: list, list, list
"""
sq_distance = self._get_sq_distance_matrix(frame)
sq_distance[sq_distance > (self.thresh ** 2)] = sys.maxint
# set all ground truth matches to FN or not defined
gt_matches = []
for i in xrange(len(self.groundtruth[frame])):
if self.groundtruth[frame][i] is None:
gt_matches.append(None)
else:
gt_matches.append(-1)
# set all measurements matches to FP or not defined
gt_distances = [-1] * len(self.groundtruth[frame])
measurements_matches = []
for i in xrange(len(self.measurements[frame])):
if self.measurements[frame][i] is None:
measurements_matches.append(None)
else:
measurements_matches.append(-1)
# verify TP from previous frame
for prev_gt, prev_measurement in enumerate(prev_gt_matches):
if prev_measurement != -1 and sq_distance[prev_gt, prev_measurement] <= (self.thresh ** 2):
gt_matches[prev_gt] = prev_measurement
measurements_matches[prev_measurement] = prev_gt
gt_distances[prev_gt] = np.sqrt(sq_distance[prev_gt, prev_measurement])
# prev_gt and prev_measurement are excluded from further matching
sq_distance[prev_gt, :] = sys.maxint
sq_distance[:, prev_measurement] = sys.maxint
hungarian = Hungarian(sq_distance)
hungarian.calculate()
matches = hungarian.get_results()
# fill in new TP
for m in matches:
if sq_distance[m[0], m[1]] == sys.maxint:
continue
gt_matches[m[0]] = m[1]
measurements_matches[m[1]] = m[0]
gt_distances[m[0]] = np.sqrt(sq_distance[m[0], m[1]])
return gt_matches, gt_distances, measurements_matches
def pseudo_cost_function(initial_pos, desired_shape, n):
k = [[0 for j in range(n)] for i in range(n)]
for i in range(n):
for j in range(n):
x = -np.transpose(initial_pos[i]).dot(np.array(desired_shape[j]))
k[i][j] = x
hungarian = Hungarian(k)
hungarian.calculate()
x_star = hungarian.get_results()
k_star = hungarian.get_total_potential()
return [x_star, k_star]
def calculate_Hungarian(self, weight_matrix):
# Find the minimum using Hungarian algorithm
# If number of robots > number of frontiers
# If the number of raws > number of columns in weight_matrix
# if len(weight_matrix) > len(weight_matrix[0]): add!!
# Check if the weight matrix is complete
if len(weight_matrix) == self.n_robots:
h = Hungarian(weight_matrix)
h.calculate()
result = h.get_results()
else:
result = []
rospy.logwarn("Invalid input for Hungarian algorithm.")
print('result je', result)
return result
def bestMap(L1, L2):
"""
两个向量的最佳匹配
:param L1:np.ndarray 真实值
:param L2:np.ndarray 预测标签
:return:L1对L2的最佳匹配 np.ndarray
"""
Label1 = np.unique(L1)
nClass1 = len(Label1)
Label2 = np.unique(L2)
nClass2 = len(Label2)
# predValue中间有跳过的label ： 0，1，5，6，10
if nClass2 < np.max(L2):
newLabel2 = np.argsort(Label2)
for i in range(nClass2):
if Label2[i] != newLabel2[i]:
L2 = np.where(L2 == Label2[i], newLabel2[i], L2)
Label2 = newLabel2
# 如果groundTruth是从1开始计数，而predValue是从0开始计数，那么predValue += 1
if nClass1 == np.max(L1) and nClass2 > np.max(L2):
L2 = L2 + 1
Label2 = Label2 + 1
nClass = max(nClass1, nClass2)
G = np.zeros((nClass, nClass))
for i in range(nClass1):
for j in range(nClass2):
G[i, j] = np.sum(((L1 == Label1[i]) & (L2 == Label2[j])))
hungarian = Hungarian(G, is_profit_matrix=True)
hungarian.calculate()
resultMap = hungarian.get_results()
resultMap = sorted(resultMap,
key=lambda s: s[1]) # resultMap[1] = (trueId, predId)
newL = np.zeros(L1.shape[0], dtype=np.uint)
for i, v in enumerate(Label1):
newL[L2 == v] = Label1[resultMap[i][0]]
return newL
def callback(data):
global trans_od_base
global trans_mat
global rot_mat
try:
(trans,rot) = listener_tf.lookupTransform(fixed_frame, camera_frame, rospy.Time(0))
trans_mat = np.asmatrix( tf.transformations.translation_matrix(trans) )
rot_mat = np.asmatrix( tf.transformations.quaternion_matrix(rot) )
trans_od_base = trans_mat*rot_mat
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
rospy.loginfo("- Detection and tracking - no transformation found")
print(e)
return
bridge = CvBridge()
try:
if classifier_type == 'DepthJet':
cv_image = bridge.imgmsg_to_cv2(data.img_jet, "passthrough")
if classifier_type == 'RGB':
cv_image = bridge.imgmsg_to_cv2(data.img_rgb, "passthrough")
except CvBridgeError as e:
print(e)
return
im_h , im_w , im_layers = cv_image.shape
intrinsic = [data.cx, data.cy, data.fx, data.fy]
plane_coeff =[data.coeff_a, data.coeff_b, data.coeff_c, data.coeff_d]
candidates = np.asarray(get_candidates(data.boxes), dtype=np.float)
detections = np.empty([0, 0])
if candidates.shape[0] != 0:
# Classification using Fast R-CNN
scores, boxes_res = im_detect(net, cv_image, candidates)
# Detections: each row -> [x1 y1 x2 y2 score depth class]
detections = preprocess_detections(scores, boxes_res, data.boxes)
global tracks_list
global next_ID
global old_stamp
time_stamp = int(str(data.img_jet.header.stamp))*10e-10
delta_t = time_stamp - old_stamp
if delta_t > 5: delta_t=0.06
old_stamp = time_stamp
#Update dt
KF_tracks.dt_update(delta_t)
KF_tracks.V_mat = camera2tracking_rotate_only(KF_tracks.V_mat_cam, rot_mat)
KF_tracks.P_init = camera2tracking_rotate_only(KF_tracks.P_init_cam, rot_mat)
# Apply Kalman Prediction Step
for j in tracks_list:
tracks_list[j].kalman_prediction()
# Create cost matrix for data association
# rows=Tracks (predictions), cols=detections
m_inf = 1e10
max_size = np.amax( [len(tracks_list), detections.shape[0]] )
cost_m = m_inf*np.ones([ max_size,max_size ])
for i in range(detections.shape[0]):
d_x, d_y, d_z, d_class, d_score, d_h, d_w = read_detection(detections[i, :], plane_coeff, intrinsic)
trk_x, trk_y, trk_z = cameraframe2trackingframe([d_x, d_y, d_z], trans_od_base)
meas = np.array([[ trk_x ], [ trk_y ], [0] ])
k = -1
for j in tracks_list:
k += 1
Cpred = tracks_list[j].C_mat*np.transpose(tracks_list[j].Xest)
d_v = meas-Cpred
S = tracks_list[j].C_mat * tracks_list[j].Pest * np.transpose(tracks_list[j].C_mat) + tracks_list[j].V_mat
# Mahalanobis variable corresponds to Mahalanobis_dist^2
Mahalanobis = np.transpose(d_v)*inv(S)*d_v
Mahalanobis = Mahalanobis[0,0]
if ( Mahalanobis <= validation_gate ):
cost_m[k, i] = Mahalanobis
#Hungarian Algorithm (find track-detection pairs)
hungarian = Hungarian()
hungarian.calculate(cost_m)
result_hungarian = hungarian.get_results()
#remove if cost>=m_inf, means mahalanobis_dist is too big
result_hungarian = [i for i in result_hungarian if not cost_m[i]>=m_inf]
tks_id = [j for j in tracks_list] #indices tracks_list
#In each tuple replace "row number" by trk_ID
result_hungarian = [ (tks_id[ i[0] ], i[1]) for i in result_hungarian]
#list of associated tracks
trks_paired = [i[0] for i in result_hungarian]
#list of associated detections
dts_paired = [i[1] for i in result_hungarian]
#list of NO associated tracks
trks_no_paired = [i for i in tracks_list if i not in trks_paired]
#list of NO associated detections
dts_no_paired = [i for i in range(detections.shape[0]) if i not in dts_paired]
#Update Paired Tracks
for resul in result_hungarian:
trk = resul[0]
det = resul[1]
d_x, d_y, d_z, d_class, d_score, d_h, d_w = read_detection(detections[det, :], plane_coeff, intrinsic)
trk_x, trk_y, trk_z = cameraframe2trackingframe([d_x, d_y, d_z], trans_od_base)
#APPLY KALMAN UPDATE
tracks_list[trk].kalman_update([trk_x, trk_y, 0])
tracks_list[trk].class_detected = d_class
tracks_list[trk].score_detected = d_score
tracks_list[trk].height = d_h
tracks_list[trk].width = d_w
tracks_list[trk].class_estimation(d_class)
#Tracks with No detection
for i in trks_no_paired:
x,y,z = tracks_list[i].Xest[0,0], tracks_list[i].Xest[0,1], tracks_list[i].Xest[0,2]
tracks_list[i].class_detected = -1 #No detection
tracks_list[i].score_detected = -1 #No detection
#apply class estimation only for objects in the FOV of the camera
if is_in_FOV([x, y, z], trans_od_base, intrinsic, im_h, im_w):
tracks_list[i].class_estimation(0)
#New detections, create tracks
for i in dts_no_paired:
d_x, d_y, d_z, d_class, d_score, d_h, d_w = read_detection(detections[i, :], plane_coeff, intrinsic)
trk_x, trk_y, trk_z = cameraframe2trackingframe([d_x, d_y, d_z], trans_od_base)
#Don't create new track if detection is too close to existing track
if detection_istooclose([trk_x, trk_y, 0], tracks_list):
continue
tracks_list[next_ID] = KF_tracks([trk_x, trk_y, 0])
tracks_list[next_ID].class_detected = d_class
tracks_list[next_ID].score_detected = d_score
tracks_list[next_ID].height = d_h
tracks_list[next_ID].width = d_w
tracks_list[next_ID].class_estimation(d_class)
next_ID += 1
#Remove tracks
remove_from_list = remove_tracks(tracks_list)
for i in remove_from_list:
del tracks_list[i]
#**************
# end Tracking
#**************
#Draw bboxes (proposals)
if draw_bbox_proposals:
cv_image = draw_proposals(cv_image, data.boxes)
#Draw bboxes (dtections)
if draw_bbox_detections:
cv_image = draw_detection(cv_image, detections)
#Draw bboxes (Tracks)
if draw_bbox_tracks:
cv_image = draw_tracks(cv_image, tracks_list, trans_od_base, plane_coeff, intrinsic)
#Save Image
if save_images == True:
f_temp = str(data.img_jet.header.stamp)
file_name = 'seq_' + f_temp[:-9] + '.' + f_temp[10:]
image_name = directory_for_images + file_name + ".png"
if not os.path.exists(directory_for_images):
os.makedirs(directory_for_images)
cv2.imwrite(image_name,cv_image)
#Publish Image
try:
mobaids_image_pub.publish( bridge.cv2_to_imgmsg(cv_image, encoding="passthrough") )
except CvBridgeError as e:
print(e)
#Publish detections msg
msg_single_det = Single_detection()
det_list_msg= []
for i in range(detections.shape[0]):
d_x, d_y, d_z, d_class, d_score, d_h, d_w = read_detection(detections[i, :], plane_coeff, intrinsic)
msg_single_det.bounding_box = Bbox(detections[i, 0], detections[i, 1], detections[i, 2], detections[i, 3])
msg_single_det.coordinates = Point(d_x, d_y, d_z)
msg_single_det.class_id = d_class
msg_single_det.class_label = class_labels[int(d_class)]
msg_single_det.score = d_score
det_list_msg.append(msg_single_det)
msg_detections = Detections()
msg_detections.header = data.img_jet.header
msg_detections.header.frame_id = camera_frame
msg_detections.detections = det_list_msg
pub_detections.publish(msg_detections)
#Publish tracks msg
msg_single_trk = Single_track()
trk_list_msg= []
for j in tracks_list:
if tracks_list[j].age < min_track_age:
continue
x,y,z = tracks_list[j].Xest[0,0], tracks_list[j].Xest[0,1], tracks_list[j].Xest[0,2]
vx, vy, vz = tracks_list[j].Xest[0,3], tracks_list[j].Xest[0,4], tracks_list[j].Xest[0,5]
trk_class = np.argmax(tracks_list[j].class_bel)
msg_single_trk.track_ID = j
msg_single_trk.position = Point(x, y, z)
msg_single_trk.velocity = Point(vx, vy, vz)
msg_single_trk.class_id = trk_class
msg_single_trk.class_label = class_labels[int(trk_class)]
msg_single_trk.hmm_probability = np.amax(tracks_list[j].class_bel)
trk_list_msg.append(msg_single_trk)
msg_tracks = Tracks()
msg_tracks.header = data.img_jet.header
msg_tracks.header.frame_id = fixed_frame
msg_tracks.tracks = trk_list_msg
pub_tracks.publish(msg_tracks)
#Publish people msg
msg_person = Person()
person_list_msg= []
for j in tracks_list:
if tracks_list[j].age < min_track_age:
continue
x,y,z = tracks_list[j].Xest[0,0], tracks_list[j].Xest[0,1], tracks_list[j].Xest[0,2]
vx, vy, vz = tracks_list[j].Xest[0,3], tracks_list[j].Xest[0,4], tracks_list[j].Xest[0,5]
trk_class = np.argmax(tracks_list[j].class_bel)
msg_person.name = "Track Nr " + str(j)
msg_person.position = Point(x, y, z)
msg_person.velocity = Point(vx, vy, vz)
msg_person.reliability = np.amax(tracks_list[j].class_bel)
msg_person.tagnames = ["class_id", "class_label"]
msg_person.tags = [ str(trk_class), class_labels[int(trk_class)] ]
person_list_msg.append(msg_person)
msg_people = People()
msg_people.header = data.img_jet.header
msg_people.header.frame_id = fixed_frame
msg_people.people = person_list_msg
pub_people.publish(msg_people)
y = 250
desired_shape.append([x, y])
x = 500
y = 200
desired_shape.append([x, y])
k = [[0 for j in range(n)] for i in range(n)]
for i in range(n):
for j in range(n):
x = -np.transpose(initial_pos[i]).dot(np.array(desired_shape[j]))
k[i][j] = x
hungarian = Hungarian(k)
hungarian.calculate()
x_star = hungarian.get_results()
k_star = hungarian.get_total_potential()
# initialize root Window and canvas
root = Tk()
root.title("Balls")
root.resizable(True, True)
canvas = Canvas(root, width=800, height=800)
canvas.pack()
# create two ball objects and animate them
balls = []
color = [
"red", "green", "black", "orange", "blue", "yellow", "purple", "grey",
"brown", "magenta"
]
# row to the destinations on the column, these distances are computed
# basing on Dijkstra
print("\nSame number of ambulances and patients")
# Distances from ambulance to patient + from patient to nearest hospital
distances = np.array(
[ # P0 P1 P2 P3
[4, 5, 11, 2], # A0
[12, 3, 7, 8], # A1
[9, 15, 5, 4], # A2
[2, 4, 17, 6] # A3
])
hungarian = Hungarian()
hungarian.calculate(distances)
print(distances)
solution = hungarian.get_results()
print("Cost",hungarian.get_total_potential())
for amb in range(distances.shape[0]):
line = "[ "
for node in range(distances.shape[1]):
if (amb, node) in solution:
line += str(distances[amb, node]) + " "
else:
line += "- "
print(line+"]")
print("\nMore ambulances than patients")
# Distances from ambulance to patient + from patient to nearest hospital
# or from ambulance to centroid
distances = np.array(
def forward(self, x, y):
tower_size = x.shape[0]
# get feature_list : tower X puzzle_num X features
feature_list = []
for i in range(self.puzzle_num):
feature_list.append(self.vnet(x[:, i, :, :, :, :]))
# unary loss
unary_list = []
perm_list = []
cur_perm = y
## first iter
feature_stack = torch.stack(feature_list, dim=1)
features = torch.reshape(feature_stack, \
(tower_size, \
self.puzzle_num * self.feature_len))
u_out1 = self.unary_fc1(features)
u_out2 = self.unary_fc2(u_out1)
u_out2 = u_out2.view(tower_size, self.puzzle_num, self.puzzle_num)
u_out = self.u_log_softmax(u_out2)
unary_list.append(u_out)
perm_list.append(cur_perm)
## other iters
for iter_id in range(self.iter_num - 1):
### hungarian algorithm for new permutation
out_detach = u_out.detach().cpu().numpy()
feature_stack_detach = feature_stack.detach().cpu().numpy()
hungarian = Hungarian()
new_feature_stack = np.zeros_like(feature_stack_detach)
results_stack = np.zeros((tower_size, self.puzzle_num))
for i in range(tower_size):
hungarian.calculate(-1 * out_detach[i, :, :])
results = hungarian.get_results()
for j in range(self.puzzle_num):
new_feature_stack[i, results[j][1], :] = \
feature_stack_detach[i, results[j][0], :]
results_stack[i, results[j][1]] = results[j][0]
results_stack = torch.from_numpy(results_stack).long().cuda()
cur_perm = torch.gather(cur_perm, 1, results_stack)
perm_list.append(cur_perm)
### new iteration
feature_stack = torch.from_numpy(new_feature_stack).float().cuda()
features = torch.reshape(feature_stack, \
(tower_size, \
self.puzzle_num * self.feature_len))
u_out1 = self.unary_fc1(features)
u_out2 = self.unary_fc2(u_out1)
u_out2 = u_out2.view(tower_size, self.puzzle_num, self.puzzle_num)
u_out = self.u_log_softmax(u_out2)
unary_list.append(u_out)
if not self.flag_pair:
return unary_list, perm_list
else:
# binary loss
binary_list = []
for i in range(self.puzzle_num):
for j in range(i + 1, self.puzzle_num):
feature_pair = torch.cat([feature_list[i], \
feature_list[j]], dim=1)
b_out1 = self.binary_fc1(feature_pair)
b_out2 = self.binary_fc2(b_out1)
b_out = self.b_log_softmax(b_out2)
binary_list.append(b_out)
binary_stack = torch.stack(binary_list, dim=1)
binary_stack = binary_stack.view(-1, 7)
return unary_list, perm_list, binary_stack