def visualize_section_lines(points, img1): img1 = np.dstack((img1, img1, img1)) img1 = img1.astype(np.uint8) for i in range(len(points) - 1): line = fl.fit_line(points[i], points[i + 1]) pt1, pt2 = fl.get_intercepts(line) img1 = fl.draw_line(img1, points[i], points[i + 1], i) # img1 = fl.draw_line(img1.copy(), pt1, pt2, i) img1 = cv2.resize(img1, (600, 210), interpolation=cv2.INTER_AREA) cv2.imshow('Window 1', img1) cv2.waitKey(1) return img1
def split(scan): """ Function split() always checks the first item of line_list and see if it is splitable. If so then it will split the list and create two sub lists. If not it will append the line to lines and then delete it from the line_list. """ n = len(scan.ranges) line_list = [[0, n-1]] # a list instance whose each item represents the [start_data_index, end_data_index] of a line lines = ExtractedLines() lines.header.frame_id = '/base_laser_link' # keep splitting while line_list is not empty while len(line_list) > 0: # find the fitted line of the first item in line_list # line is an ExtractedLine(r, alpha, firstScanPoint, lastScanPoint) instance line = fit_line(scan, line_list[0][0], line_list[0][1], max_range) # return the max_dist and its index max_dist, max_dist_index = find_max_dist(scan, line, line_list[0][0], line_list[0][1]) # if max_dist is larger than or equal to the threshold, split the points if max_dist >= dist_threshold: # split only if the number of points in the splitted subset is larger than the threshold if (max_dist_index - line_list[0][0] + 1) >= min_points and \ (line_list[0][1] - max_dist_index + 1) >= min_points: # insert [max_dist_index, end_index] line_list.insert(0, [max_dist_index, line_list[0][1]]) # note that the original first item now becomes the second item line_list.insert(0, [line_list[1][0], max_dist_index]) # delete the original first item in line_list, which now # becomes the third item line_list.pop(2) # if the number of points after splitting are two small, then # do not split and just append the line to lines else: lines.lines.append(line) # delete the first item in line_list line_list.pop(0) # if max_dist is less than the threshold, append the line to lines else: lines.lines.append(line) line_list.pop(0) return lines
def scan_callback(scan): """ Fit a single line to the given laser scan. The line is fitted using scan points that are within 'maximum_range' distance (this parameter should be found by the ROS parameter server). An ExtractedLines message is constructed to hold this single line and this message is published on /extracted_lines. """ # The fit_line method expects the scan itself, a pair of integers # describing the indices upon which it will operate, and the max. range. n = len(scan.ranges) line = fit_line(scan, 0, n - 1, maximum_range) lines = ExtractedLines() lines.header.frame_id = '/base_laser_link' if line is not None: lines.lines.append(line) extracted_publisher.publish(lines)
def do_the_thing(basefilename,time_stamp,delta1,delta2): time_stamp = str(time_stamp) conf_file = basefilename + time_stamp + '_conf' conf_data = read_config(conf_file) #print(conf_data['step_size']['val']) f_freqs = basefilename + time_stamp + '_set_points' f_ch0 = basefilename + time_stamp + '_ch0_arr' f_ch1 = basefilename + time_stamp + '_ch1_arr' f_ch2 = basefilename + time_stamp + '_ch2_arr' f_ch3 = basefilename + time_stamp + '_ch3_arr' f_ch4 = basefilename + time_stamp + '_ch4_arr' f_ch0s = basefilename + time_stamp + '_ch0_arr_slow' f_ch1s = basefilename + time_stamp + '_ch1_arr_slow' f_ch2s = basefilename + time_stamp + '_ch2_arr_slow' f_ch3s = basefilename + time_stamp + '_ch3_arr_slow' f_ch4s = basefilename + time_stamp + '_ch4_arr_slow' freqs = np.genfromtxt(f_freqs, delimiter=",") ch0 = np.genfromtxt(f_ch0, delimiter=",") ch1 = np.genfromtxt(f_ch1, delimiter=",") ch2 = np.genfromtxt(f_ch2, delimiter=",") ch3 = np.genfromtxt(f_ch3, delimiter=",") ch4 = np.genfromtxt(f_ch4, delimiter=",") ch0s = np.genfromtxt(f_ch0s, delimiter=",") ch1s = np.genfromtxt(f_ch1s, delimiter=",") ch2s = np.genfromtxt(f_ch2s, delimiter=",") ch3s = np.genfromtxt(f_ch3s, delimiter=",") ch4s = np.genfromtxt(f_ch4s, delimiter=",") # get number of averages no_of_avg = int(len(freqs)/len(np.unique(freqs))) print('Found ' + str(no_of_avg) + ' averages.') freqs = av(freqs, no_of_avg) ch0 = av(ch0, no_of_avg) ch1 = av(ch1, no_of_avg) ch2 = av(ch2, no_of_avg) ch3 = av(ch3, no_of_avg) ch4 = av(ch4, no_of_avg) ch0s = av(ch0s, no_of_avg) ch1s = av(ch1s, no_of_avg) ch2s = av(ch2s, no_of_avg) ch3s = av(ch3s, no_of_avg) ch4s = av(ch4s, no_of_avg) offset_freq = float(conf_data['cooling_offet']['val']) #offset_freq = 382.08140 #nus = 2*freqs delay_in_for_loop = float(conf_data['step_size']['val'])*1e-6 #delay_in_for_loop = 100e-6 #delay_in_for_loop = 50e-6 no_of_time_points = ch1.shape[1] times = np.arange(0, no_of_time_points) * (delay_in_for_loop) / 1e-3 #time_cut1 = 182 #time_cut2 = time_cut1+10 #time_cut1 = 98 #time_cut2 = time_cut1+10 time_cut1 = int(8.4e-3/delay_in_for_loop) + 14 + delta1 #print(time_cut1) time_cut2 = time_cut1 + delta2 color_plot = ch0[:, time_cut1:time_cut2] color_plot -= np.min(np.min(color_plot)) color_plot /= np.max(np.max(color_plot)) ### 2D PLOTS if delta1 == 0: plt.figure() plt.pcolor(times[time_cut1:time_cut2], 3*freqs, color_plot) plt.xlabel('Time (us)') plt.ylabel('Relative UV frequency (MHz)') plt.colorbar() signal = np.mean(ch0[:, time_cut1:time_cut2], axis = 1) signal = signal/np.max(np.abs(signal)) (xfit, yfit, result) = fl.fit_line(freqs, signal) cnt_peak = result.params['x0'].value #print('a: ',result.params['a'].value) #print('w: ',result.params['w'].value) #print('x0: ',result.params['x0'].value) #print('y_offset',result.params['y_offset'].value) abs_cnt_peak = 3 * ((offset_freq * 1e12 + cnt_peak * 1e6)/1e12) # in THz abs_cnt_peak_wavenumber = abs_cnt_peak * 1e12/100.0/c ### FIT PLOTS if delta1 == 0: plt.figure() plt.plot(3*freqs, 1 - signal) plt.plot(3*xfit, 1 - yfit, 'r-') plt.ylim(0, .8) plt.ylabel('Absorption signal (a.u.)') plt.xlabel('Frequency (MHz)') plt.title(time_stamp) plt.text(-400, 0.4, "Center peak frequency: \n\n {0:9.6f} THz \n = {1:9.4f} 1/cm".format(abs_cnt_peak, abs_cnt_peak_wavenumber)) wid = result.params['w'].value return abs_cnt_peak,wid,freqs,signal,offset_freq
def scan_callback(scan): """ Extracts lines from the given LaserScan and publishes to /extracted_lines. Extracts lines using the split phase of the split-and-merge algorithm. Publish these lines as an ExtractedLines method on the /extracted_lines topic. """ # Create an ExtractedLines method and initialize some fields in the header. lines = ExtractedLines() lines.header.frame_id = '/laser' #lines.header.frame_id = '/base_laser_link' lines.header.stamp = rospy.Time.now() # Create our big list of index pairs. Each pair gives the start and end # index which specifies a contiguous set of data points in 'scan'. L = [] # Place (0, n-1) in the list which represents the complete list of scan # points. n = len(scan.ranges) L.append((0, n-1)) # We'll keep track of the iteration count with 'I' just for debug purposes. I = 0 # The big loop. At each step we process the sublist stored in the first # pair of indices in L. while len(L) != 0: if debug: rospy.loginfo( "I: " + str(I)) I += 1 # Get the first sublist, i will refer to the starting index and j # the final index. (i, j) = L.pop(0) if debug: rospy.loginfo('Processing: ({}, {})'.format(i, j)) line = fit_line(scan, i, j, maximum_range) if line is None: if debug: rospy.loginfo('\tNo line found') continue if debug: rospy.loginfo('\tLine: ({}, {})'.format(line.r, line.alpha)) # Find the worst fit point. We will consider a point to be the # worst fit if it represents a local maximum in distance from the line. # That is, its absolute distance to the line must be greater than that # of its two neighbours. First compute an array of distances. We will # not consider the first or last min_points_per_line / 2 # points. So the length of this array is the following: nDistances = j - i + 1 - min_points_per_line distances = [0] * nDistances # Fill with zeros mppl_2 = min_points_per_line / 2 # distIndex is the index into distances # laserIndex is the index into the laser scan for distIndex in range(nDistances): laserIndex = i + mppl_2 + distIndex # Compute the orthogonal distance to the line, d rho = scan.ranges[laserIndex] theta = scan.angle_min + laserIndex * scan.angle_increment d = abs(rho * math.cos(theta - line.alpha) - line.r) distances[distIndex] = d # Now check for the worst fit point using these distances. worstLaserIndex = -1 worstDistance = 0 for laserIndex in range(i + mppl_2 + 1, j - mppl_2 - 1): if scan.ranges[laserIndex] > maximum_range: continue distIndex = laserIndex - i - mppl_2 if distances[distIndex] >= distances[distIndex - 1] and \ distances[distIndex] >= distances[distIndex + 1] and \ distances[distIndex] > worstDistance: worstLaserIndex = laserIndex worstDistance = distances[distIndex] if debug: rospy.loginfo( "\tworstLaserIndex: " + str(worstLaserIndex)) rospy.loginfo( "\tworstDistance: " + str(worstDistance)) if worstDistance < orthog_distance_threshold: # We have dealt with this sublist. Add it to lines. if debug: rospy.loginfo('\tAdding line') lines.lines.append(line) else: # Split this sublist into two and add both back into L. if debug: rospy.loginfo('\tSplitting: ({}, {})'.format(i, j)) insert_sublist_if_long(L, 0, (i, worstLaserIndex-1)) insert_sublist_if_long(L, 1, (worstLaserIndex+1, j)) if debug: rospy.loginfo( "\n\n") # Normally the merge step would follow... extracted_publisher.publish(lines)
def scan_callback(scan): """ Fit a single line to the given laser scan. The line is fitted using scan points that are within 'maximum_range' distance (this parameter should be found by the ROS parameter server). An ExtractedLines message is constructed to hold this single line and this message is published on /extracted_lines. """ L2 = list() total_points = len(scan.ranges) endpoints = (0, total_points - 1) L2.append(endpoints) i = 0 while (i <= len(L2) - 1): curr_line_points = L2[i] curr_line_start = curr_line_points[0] curr_line_end = curr_line_points[1] line = fit_line(scan, curr_line_start, curr_line_end, maximum_range) max_error = 0 j = curr_line_start + min_points_per_line / 2 max_error_idx = -1 while ((line is not None) and (j < curr_line_end - min_points_per_line / 2 + 1)): rho = scan.ranges[j] theta = scan.angle_min + scan.angle_increment * j error = distance(line.r, line.alpha, rho, theta) if error > max_error: max_error = error max_error_idx = j j = j + 1 if max_error > orthog_distance_threshold: L2[i] = (curr_line_start, max_error_idx - 1) L2.insert(i + 1, (max_error_idx, curr_line_end)) #Only increment i if no splitting, if we split we need to #reevaluate the new split line at position i else: i = i + 1 #If we haven't split, merge all the points beyond this one if i < len(L2): merge_start = L2[i][0] merge_end = L2[len(L2) - 1][1] L2 = L2[:i] L2.append((merge_start, merge_end)) #Build the lines message and publish lines = ExtractedLines() lines.header.frame_id = scan.header.frame_id #'/base_laser_link' for end_points in L2: line = fit_line(scan, end_points[0], end_points[1], maximum_range) if line is not None: lines.lines.append(line) extracted_publisher.publish(lines)
def annotate_image(img_in): """ Annotate the input image with lane line markings Returns annotated image """ global mtx, dist, left_line, right_line, detected global left_curve, right_curve, left_lane_inds, right_lane_inds # Undistort, threshold, perspective transform undist = cv2.undistort(img_in, mtx, dist, None, mtx) img, abs_bin, mag_bin, dir_bin, imgThres_yellow, imgThres_white = combined_thresh( undist) binary_warped, binary_unwarped, m, m_inv = perspective_transform(img) # Perform polynomial fit if not detected: # Slow line fit ret = fit_line(binary_warped) left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] # Get moving average of line fit coefficients left_fit = left_line.add_fit(left_fit) right_fit = right_line.add_fit(right_fit) # Calculate curvature left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) detected = True # slow line fit always detects the line else: # implies detected == True # Fast line fit left_fit = left_line.get_fit() right_fit = right_line.get_fit() ret = fit_tune(binary_warped, left_fit, right_fit) left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] # Only make updates if we detected lines in current frame if ret is not None: left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] left_fit = left_line.add_fit(left_fit) right_fit = right_line.add_fit(right_fit) left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) else: detected = False vehicle_offset = calc_vehicle_offset(undist, left_fit, right_fit) # Perform final visualization on top of original undistorted image result = final_viz(undist, left_fit, right_fit, m_inv, left_curve, right_curve, vehicle_offset) return result