def detectLines(self, color): with dtu.timeit_clock('_getMask'): mask = self._getMask(color) # Detect yellow points using ST keypoints. # if color == 'yellow': # # Default keypoints # lines = [] # normals = [] # centers = [] # with dtu.timeit_clock('_getKeypoints'): # kps = self._getKeypoints(mask, color) # # If kps available, package up lines # if len(kps) > 0: # # KP info is stored in [0:1], add +2 for visualization # lines = np.hstack((kps, kps / 2)).astype('int32') # # Fake the normals and centres, we don't use these # normals = np.zeros((len(lines), 2)) # centers = np.zeros((len(lines), 2)) # Detect white points using prob. Hough lines # Also for red points, for now if color == 'white' or color == 'red' or color == 'yellow': with dtu.timeit_clock('_getLines'): lines, normals, centers = self._getLines(mask) return Detections(lines=lines, normals=normals, area=mask, centers=centers)
def setImage(self, bgr): with dtu.timeit_clock('np.copy'): self.bgr = np.copy(bgr) with dtu.timeit_clock('cvtColor COLOR_BGR2HSV'): self.hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV) with dtu.timeit_clock('_findEdge'): self.edges = self._findEdge(self.bgr)
def detectLines(self, color): with dtu.timeit_clock('_colorFilter'): bw, edge_color = self._colorFilter(color) with dtu.timeit_clock('_HoughLine'): lines = self._HoughLine(edge_color) with dtu.timeit_clock('_findNormal'): centers, normals = self._findNormal(bw, lines) return Detections(lines=lines, normals=normals, area=bw, centers=centers)
def setImage(self, bgr): # Smooth image, convert to hsv, and convert to grayscale. with dtu.timeit_clock('np.copy'): self.bgr = np.copy(bgr) with dtu.timeit_clock('bilateral smoothing'): self.smoothed = self._smooth(bgr) with dtu.timeit_clock('cvtColor COLOR_BGR2HSV'): self.hsv = cv2.cvtColor(self.smoothed, cv2.COLOR_BGR2HSV) with dtu.timeit_clock('_greyscale'): self.gray = cv2.cvtColor(self.smoothed, cv2.COLOR_BGR2GRAY)
def phase(self, phase_name): with dtu.timeit_clock(phase_name): if not phase_name in self.phase_names: self.phase_names.append(phase_name) if self.last_msg_being_processed is None: msg = 'Did not call decided_to_process() before?' raise ValueError(msg) # t1 = rospy.get_time() # @UndefinedVariable t1 = time.time() c1 = time.clock() try: yield finally: c2 = time.clock() # t2 = rospy.get_time() # @UndefinedVariable t2 = time.time() delta_clock = c2 - c1 delta_wall = t2 - t1 latency_from_acquisition = t2 - self.last_msg_being_processed self.stats[(phase_name, 'clock')].sample(delta_clock) self.stats[(phase_name, 'wall')].sample(delta_wall) self.stats[(phase_name, 'latency')].sample(latency_from_acquisition)
def update(self, segment_list): """ Returns the likelihood """ self.last_segments_used = segment_list.segments with dtu.timeit_clock('generating likelihood'): if not self.optimize: measurement_likelihood = self.generate_measurement_likelihood( segment_list.segments) else: measurement_likelihood = self.generate_measurement_likelihood_faster( segment_list.segments) with dtu.timeit_clock('multiply belief'): if measurement_likelihood is not None: self.belief = np.multiply(self.belief, measurement_likelihood) if np.sum(self.belief) == 0: self.belief = measurement_likelihood alpha = 1.0 / np.sum(self.belief) self.belief = self.belief * alpha return measurement_likelihood
#!/usr/bin/env python import duckietown_utils as dtu import sys dtu.DuckietownConstants.show_timeit_benchmarks = True with dtu.timeit_clock('empty 1'): with dtu.timeit_clock('empty 2'): with dtu.timeit_clock('empty 3'): with dtu.timeit_clock('empty 4'): with dtu.timeit_clock('empty 5'): with dtu.timeit_clock('empty 6'): with dtu.timeit_clock('empty 7'): with dtu.timeit_clock('empty 8'): with dtu.timeit_clock('empty 9'): pass
def generate_measurement_likelihood_faster(self, segments): with dtu.timeit_clock("get_compat_representation_obs (%d segments)" % len(segments)): rep_obs = get_compat_representation_obs(segments) rep_map = self.rep_map with dtu.timeit_clock("generate_votes_faster (map: %d, obs: %d)" % (len(rep_map.weight), len(rep_obs.weight))): votes = generate_votes_faster(rep_map, rep_obs) if self.bounds_theta_deg is not None: weight = votes.weight.copy() theta_min = np.deg2rad(self.bounds_theta_deg[0]) theta_max = np.deg2rad(self.bounds_theta_deg[1]) num_outside = 0 for i in range(weight.shape[0]): theta = votes.theta[i] if not (theta_min <= theta <= theta_max): weight[i] = 0 num_outside += 1 # print('Removed %d of %d because outside box' % (num_outside, len(weight))) votes = remove_zero_weight(votes._replace(weight=weight)) with dtu.timeit_clock("compute pos iterative (%d)" % len(votes.weight)): locations = self._localization_template.coords_from_position_orientation( votes.p, votes.theta) num = len(locations) est = np.zeros((2, num)) for i in range(2): v = list(self.variables)[i] est[i, :] = locations[v] F = self.F compare = False with dtu.timeit_clock("add voting faster (%d)" % len(votes.weight)): measurement_likelihood = self.grid_helper.create_new('float32') measurement_likelihood.fill(0) if compare: counts1 = np.zeros(measurement_likelihood.shape, dtype='int') else: counts1 = None added = self.grid_helper.add_vote_faster(measurement_likelihood, est, votes.weight, F=F, counts=counts1) # print('Counts (new):\n' + array_as_string(counts1, lambda x: ' %3d' % x)) if compare: with dtu.timeit_clock("add voting (traditional)"): measurement_likelihood_classic = self.grid_helper.create_new( 'float32') measurement_likelihood_classic.fill(0) hit = miss = 0 counts2 = np.zeros(measurement_likelihood.shape, dtype='int') for i in range(len(locations)): loc = locations[i] weight = votes.weight[i] value = dict((k, loc[k]) for k in self.variables) added = self.grid_helper.add_vote( measurement_likelihood_classic, value, weight, F=F, counts=counts2) hit += added > 0 miss += added == 0 # dtu.logger.debug('tradoitional hit: %s miss : %s' % (hit, miss)) # print('Counts (old):\n' + array_as_string(counts2, lambda x: ' %3d' % x)) # diff = measurement_likelihood - measurement_likelihood_classic deviation = np.max(np.abs(diff)) if deviation > 1e-6: s = array_as_string_sign(diff) print('max deviation: %s' % deviation) print s return measurement_likelihood
def generate_measurement_likelihood(self, segments): # initialize measurement likelihood to all zeros measurement_likelihood = self.grid_helper.create_new('float32') measurement_likelihood.fill(0) hit = miss = 0 pose_weight = [] num_by_color = defaultdict(lambda: 0) adjust_by_number = False # add to configuration with dtu.timeit_clock("pose gen for %s segs" % len(segments)): for segment in segments: num_by_color[segment.color] += 1 for segment in segments: for pose, weight in self.generate_votes( segment, self.delta_segment): if self.bounds_theta_deg is not None: theta_deg = np.rad2deg( dtu.norm_angle(dtu.geo.angle_from_SE2(pose))) m, M = self.bounds_theta_deg if not (m <= theta_deg < M): continue if adjust_by_number: n = num_by_color[segment.color] weight_adjusted = weight * 1.0 / n else: weight_adjusted = weight pose_weight.append((pose, weight_adjusted)) values = [] with dtu.timeit_clock("generating coords for %s votes" % len(pose_weight)): for pose, weight in pose_weight: est = self._localization_template.coords_from_pose(pose) value = dict((k, float(est[k])) for k in self.variables) values.append((value, weight)) with dtu.timeit_clock("add voting for %s votes" % len(pose_weight)): for value, weight in values: # if value['dstop'] > 0.3: # print('value:%s' % value) # else: # continue added = self.grid_helper.add_vote(measurement_likelihood, value, weight, F=self.F) hit += added > 0 miss += added == 0 dtu.logger.debug('hit: %s miss : %s' % (hit, miss)) if np.linalg.norm(measurement_likelihood) == 0: return None return measurement_likelihood
def add_vote_faster(self, target, values, weights, F=1, counts=None): with dtu.timeit_clock("adding additional votes (orig: %s)" % values.shape[1]): _factor, values_ref, values, weights, group = self.multiply( values, weights, F) # cells_in_group = np.zeros(len(group)) # for i in range(len(group)): # cells_in_group[group[i]] += 1 # print('cells_in_group: %s' % cells_in_group) # diff = values - values_ref # for a in [0,1]: # print('diff %s min %s max %s res %s' % (a, np.min(diff[a,:]), np.max(diff[a,:]), # self._specs[a].resolution)) with dtu.timeit_clock("computing coordinates (nvalid = %s)" % values.shape[1]): nvalid = values.shape[1] coords = np.zeros((2, nvalid), dtype='int32') K = [self.K0, self.K1] for a in range(2): spec = self._specs[a] inv_resolution = 1.0 / spec.resolution x = (values[a, :] - spec.min) * inv_resolution # print('values[%d] = %s' % (a, values[a, :])) # print('x = %s' % x) coords[a, :] = np.floor(x) # print('coords[%d] = %s' % (a, coords[a, :])) xr = (values_ref[a, :] - spec.min) * inv_resolution dc = (xr - (coords[a, :] + 0.5)) # print('dc[%d] = %s' % (a, dc)) dr = dc * spec.resolution k = 1 k = K[a](dr) weights *= k if nvalid == 0: return 0 with dtu.timeit_clock("normalizing weights"): ngroups = np.max(group) + 1 weight_group = np.zeros(ngroups) if False: for i in range(len(weights)): weight_group[group[i]] += weights[i] else: np.add.at(weight_group, group, weights) assert len(weights) == nvalid weights_normalized = weights / weight_group[group] with dtu.timeit_clock("selecting valid (using %d)" % values.shape[1]): AND = np.logical_and inside0 = AND(self._specs[0].min <= values[0, :], values[0, :] <= self._specs[0].max) inside1 = AND(self._specs[1].min <= values[1, :], values[1, :] <= self._specs[1].max) inside = AND(inside0, inside1) # hits = np.sum(inside) # misses = len(inside) - hits # print('num hit: %s (eq %d) misses %s (eq %d) ' % (hits, hits/_factor, misses, misses/_factor)) # only consider inside values = values[:, inside] values_ref = values_ref[:, inside] weights = weights[inside] weights_normalized = weights_normalized[inside] group = group[inside] coords = coords[:, inside] with dtu.timeit_clock("using numpy.at"): np.add.at(target, tuple(coords), weights_normalized) if counts is not None: with dtu.timeit_clock("update counts"): for i in range(nvalid): counts[coords[0, i], coords[1, i]] += 1 return nvalid
#!/usr/bin/env python import duckietown_utils as dtu import sys print('starting') fn = sys.argv[1] data = open(fn).read() dtu.DuckietownConstants.show_timeit_benchmarks = True for f in [ dtu.rgb_from_jpg_by_PIL, dtu.rgb_from_jpg_by_JPEG_library, dtu.bgr_from_jpg ]: for i in range(3): res = f(data) with dtu.timeit_clock(f.__name__): for i in range(10): res = f(data) print res.shape