def initialize(self, img, detections):
if len(detections) > 0:
#self.detector = Resnet()
(self.img_height, self.img_width, self.n_channels) = img.shape
dPosX = stats.norm( loc = 0.0, scale = self.POS_STD_X )
dPosY = stats.norm( loc = 0.0, scale = self.POS_STD_Y )
dWidth = stats.norm( loc = 0.0, scale = self.SCALE_STD_WIDTH )
dHeight = stats.norm( loc = 0.0, scale = self.SCALE_STD_HEIGHT )
self.persistent_states = np.empty((self.particles_batch * len(detections), self.DIM), dtype = int)
idx = 0
for det in detections:
self.persistent_states[idx*self.particles_batch:(idx+1)*self.particles_batch,0] = det.bbox.x + dPosX.rvs(self.particles_batch)
self.persistent_states[idx*self.particles_batch:(idx+1)*self.particles_batch,1] = det.bbox.y + dPosY.rvs(self.particles_batch)
self.persistent_states[idx*self.particles_batch:(idx+1)*self.particles_batch,2] = det.bbox.width + dWidth.rvs(self.particles_batch)
self.persistent_states[idx*self.particles_batch:(idx+1)*self.particles_batch,3] = det.bbox.height + dHeight.rvs(self.particles_batch)
target = Target(det.bbox, idx, (random.randint(0,255), random.randint(0,255), random.randint(0,255)))
self.tracks.append(target)
self.current_labels.append(idx)
self.labels.add(idx)
idx+=1
self.persistent_weights = np.ones((self.particles_batch * len(detections)), dtype = float) * ( 1.0/float(self.particles_batch))
#self.tracks_features = self.detector.get_features(img, self.tracks)
#self.birth_model = detections
self.initialized = True
def create_source_abi_reference_dumps_for_all_products(args):
"""Create reference ABI dumps for all specified products."""
num_processed = 0
for product in args.products:
build_vars = get_build_vars_for_product(
['PLATFORM_VNDK_VERSION', 'BOARD_VNDK_VERSION', 'BINDER32BIT'],
product, args.build_variant)
platform_vndk_version = build_vars[0]
board_vndk_version = build_vars[1]
if build_vars[2] == 'true':
binder_bitness = '32'
else:
binder_bitness = '64'
chosen_vndk_version = choose_vndk_version(args.version,
platform_vndk_version,
board_vndk_version)
targets = [Target(True, product), Target(False, product)]
# Remove reference ABI dumps specified in `args.libs` (or remove all of
# them if none of them are specified) so that we may build these
# libraries successfully.
remove_references_for_all_arches_and_variants(args.ref_dump_dir,
chosen_vndk_version,
binder_bitness, targets,
args.libs)
if not args.no_make_lib:
# Build all the specified libs, or build `findlsdumps` if no libs
# are specified.
make_libs_for_product(args.libs, product, args.build_variant,
platform_vndk_version, targets)
lsdump_paths = read_lsdump_paths(product,
args.build_variant,
platform_vndk_version,
targets,
build=False)
num_processed += create_source_abi_reference_dumps(
args, chosen_vndk_version, binder_bitness, lsdump_paths, targets)
return num_processed
def __init__(self):
# ROS Topics:
rospy.Subscriber('IGTL_STRING_IN', igtlstring, self.callbackString)
rospy.Subscriber('IGTL_TRANSFORM_IN', igtltransform,
self.callbackTransformation)
self.pub1 = rospy.Publisher('IGTL_STRING_OUT',
igtlstring,
queue_size=10)
self.pub2 = rospy.Publisher('IGTL_STRING_OUT',
igtlstring,
queue_size=10)
self.motors = rospy.Publisher('IGTL_STRING_OUT',
igtlstring,
queue_size=10)
self.galilStatus = rospy.Publisher('IGTL_STRING_OUT',
igtlstring,
queue_size=10)
rospy.init_node('talker', anonymous=True)
# Define the variables for openigtlink
self.TransferData1 = igtlstring()
self.TransferData1.name = "statusTarget"
self.TransferData2 = igtlstring()
self.TransferData2.name = "statusZ-Frame"
self.motorsData = igtlstring()
self.motorsData.name = "motorPosition"
self.galilStatusData = igtlstring()
self.galilStatusData.name = "status"
#Variables:
self.status = 0
self.CartesianPositionA = 0
self.CartesianPositionB = 0
self.OrientationA = 0
self.OrientationB = 0
self.state = IDLE
self.MotorsReady = 0
self.targetRAS = numpy.matrix(
'1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0'
)
self.targetRAS_angle = numpy.matrix(
'1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0'
)
self.targetRobot = numpy.matrix(
'1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0'
)
self.zTransReady = False
self.zTrans = numpy.matrix(
'1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0'
)
self.target = Target()
self.connectionStatus = False
self.save_position_A = 0
self.save_position_B = 0
self.save_position_C = 0
self.save_position_D = 0
def initialize(self, img, detections=None, calcHist=False):
(self.img_height, self.img_width, self.n_channels) = img.shape
self.tracks = []
if len(detections) > 0 and detections:
for idx, det in enumerate(detections):
target = Target(det.bbox, idx, (random.randint(
0, 255), random.randint(0, 255), random.randint(0, 255)),
det.conf, self.SURVIVAL_RATE, det.feature)
self.tracks.append(target)
self.labels.append(idx)
self.initialized = True
else:
self.initialized = False
def estimate(self, img = None, draw = False, color = (0, 255, 0), verbose = False):
if self.initialized:
num_estimated = int(round(self.persistent_weights.sum()))
if num_estimated > 0:
kmeans = KMeans(n_clusters = num_estimated).fit(self.persistent_states)
estimated_targets = np.rint(kmeans.cluster_centers_).astype(int)
new_tracks = []
label = 0
for et in estimated_targets:
while (label in self.labels or label in self.current_labels):
label+=1
target = Target(Rectangle(et[0], et[1], et[2], et[3]), label,\
(random.randint(0, 255), random.randint(0, 255), random.randint(0, 255)) )
new_tracks.append(target)
self.current_labels.append(label)
label+=1
# affinity
'''new_tracks_features = self.detector.get_features(img, new_tracks)
affinity_matrix = appearance_affinity(self.tracks_features, new_tracks_features) *\
motion_affinity(self.tracks, new_tracks) * \
shape_affinity(self.tracks, new_tracks)
affinity_matrix = 1./affinity_matrix
row_ind, col_ind = linear_sum_assignment(affinity_matrix)'''
#######
cost = cost_matrix(self.tracks, new_tracks)
row_ind, col_ind = linear_sum_assignment(cost)
for r,c in zip(row_ind, col_ind):
new_tracks[c].color = self.tracks[r].color
new_tracks[c].label = self.tracks[r].label
self.tracks = new_tracks[:]
#self.tracks_features = new_tracks_features[:]
del self.current_labels[:]
for track in self.tracks:
self.current_labels.append(track.label)
self.labels = self.labels.union(self.current_labels)
if draw:
for track in self.tracks:
cv2.rectangle(img, (track.bbox.x, track.bbox.y), (track.bbox.x + track.bbox.width, track.bbox.y + track.bbox.height), track.color, 3)
if verbose:
print 'estimated targets: ' + str(num_estimated)
return self.tracks
def __init__(self):
rospy.Subscriber('IGTL_STRING_IN', igtlstring, self.callbackString)
rospy.Subscriber('IGTL_TRANSFORM_IN', igtltransform, self.callbackTransformation)
self.pub = rospy.Publisher('IGTL_STRING_OUT', igtlstring, queue_size=10)
rospy.init_node('talker', anonymous=True)
# Define the variables
self.TransferData = igtlstring
self.CartesianPositionA = 0
self.CartesianPositionB = 0
self.OrientationA = 0
self.OrientationB = 0
self.state = IDLE
self.MotorsReady = 0
self.targetRAS = numpy.matrix('1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0')
self.targetRAS_angle = numpy.matrix('1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0')
self.targetRobot = numpy.matrix('1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0')
self.zTransReady = False
self.zTrans = numpy.matrix('1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 ; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0')
self.target = Target()
if __name__ == '__main__':
try:
try:
modem_id = int(sys.argv[1])
radius = float(sys.argv[2])
distance_between_meas = int(sys.argv[3])
num_points = float(sys.argv[4])
except IndexError:
print('Three arguments are requestd <modem ID>, <circumference radius>, <distance between measurements> and <number of range measurements>')
sys.exit()
if radius<1 or radius>500:
print('Radius must be between 1 and 500')
sys.exit()
elif num_points<1 or num_points>100:
if num_points == -1:
print('Infinit number of points will be measured')
else:
print('Number of measurement must be between 1 and 100')
sys.exit()
elif distance_between_meas<1 or distance_between_meas>100:
print('Distance between measurements must be between 1 and 100')
sys.exit()
target = Target()
tracker = TargetTracking()
tracker.start(target, modem_id, radius, distance_between_meas, num_points)
print('Done')
except rospy.ROSInterruptException or KeyboardInterrupt:
pass
def update(self, img, detections=None, verbose=False, calcHist=False):
self.birth_model = []
if self.is_initialized() and len(detections) > 0 and detections:
new_detections = []
for idx, det in enumerate(detections):
target = Target(bbox = det.bbox, color = (random.randint(0,255), random.randint(0,255), random.randint(0,255)),\
conf = det.conf, survival_rate = self.SURVIVAL_RATE, feature = det.feature)
new_detections.append(target)
new_tracks = []
if len(self.tracks) > 0:
diagonal = np.sqrt(
np.power(self.img_height, 2) + np.power(self.img_width, 2))
area = self.img_height * self.img_width
cost = cost_matrix(self.tracks, new_detections, diagonal, area,
False)
#tracks_ind, new_dets_ind = linear_sum_assignment(cost)
tracks_ind, new_dets_ind = solve_dense(cost)
dets_high_cost = set()
for idxTrack, idxNewDet in zip(tracks_ind, new_dets_ind):
if cost[idxTrack, idxNewDet] < self.THRESHOLD:
new_detections[idxNewDet].label = self.tracks[
idxTrack].label
new_detections[idxNewDet].color = self.tracks[
idxTrack].color
self.tracks[idxTrack].update(
new_detections[idxNewDet].bbox)
new_tracks.append(self.tracks[idxTrack])
else:
self.tracks[idxTrack].survival_rate = np.exp(
self.SURVIVAL_DECAY *
(-1.0 + self.tracks[idxTrack].survival_rate * 0.9))
new_tracks.append(self.tracks[idxTrack])
dets_high_cost.add(idxNewDet)
tracks_no_selected = set(np.arange(len(
self.tracks))) - set(tracks_ind)
for idxTrack in tracks_no_selected:
#print str(new_tracks[idxTrack].bbox.x) + ',' + str(new_tracks[idxTrack].bbox.y) + ',' + str(new_tracks[idxTrack].bbox.width) + ',' + str(new_tracks[idxTrack].bbox.height)
self.tracks[idxTrack].survival_rate = np.exp(
self.SURVIVAL_DECAY *
(-1.0 + self.tracks[idxTrack].survival_rate * 0.9))
new_tracks.append(self.tracks[idxTrack])
#print '###################'
new_detections_no_selected = set(np.arange(
len(new_detections))) - set(new_dets_ind)
new_detections_no_selected = new_detections_no_selected | dets_high_cost
for idxNewDet in new_detections_no_selected:
#print str(new_detections[idxNewDet].bbox.x) + ',' + str(new_detections[idxNewDet].bbox.y) + ',' + str(new_detections[idxNewDet].bbox.width) + ',' + str(new_detections[idxNewDet].bbox.height)
if np.random.uniform() > self.BIRTH_RATE:
new_label = max(self.labels) + 1
new_detections[idxNewDet].label = new_label
self.birth_model.append(new_detections[idxNewDet])
self.labels.append(new_label)
#print '###################'
else:
for idxNewDet, det in enumerate(new_detections):
if np.random.uniform() > self.BIRTH_RATE:
new_label = max(self.labels) + 1
new_detections[idxNewDet].label = new_label
self.birth_model.append(new_detections[idxNewDet])
self.labels.append(new_label)
#dpp = DPP()
#self.tracks = dpp.run(boxes = new_tracks, img_size = (self.img_width, self.img_height),features=None)
#self.tracks = nms(new_tracks, 0.7, 0, 0.5)
self.tracks = new_tracks