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