def scan_direction_hough(self, S_whole, angle, bmpsize):
pass
# filter by neighboring scan angles S_whole
minx, maxx, miny, maxy = S_whole.minx, S_whole.maxx, S_whole.miny, S_whole.maxy
S_whole = common.PointOperations().filter_points_by_angle_range(
S_whole, angle - 1, angle + 1)
Y = common.Visualization().transform_points_to_bmp_with_bounds(
S_whole, bmpsize, minx, maxx, miny, maxy)
Y = self.circular_mask(Y)
accumulator, thetas, rhos = self.hough_line(Y)
if self.do_visualization:
self.visualize_accumulator(accumulator)
Y = self.insert_resulting_lines(Y, accumulator, rhos, thetas)
if self.do_visualization:
common.HoughTransform().visualize_matrix(Y)
xs, ys = np.where(Y == 2)
scan_direction = [[[xs[0], ys[0]], [xs[len(xs) - 1], ys[len(ys) - 1]]]]
return scan_direction
def scan_direction_derivative(self, S_whole, angle, bmpsize):
pass
# filter by neighboring scan angles S_whole
minx, maxx, miny, maxy = S_whole.minx, S_whole.maxx, S_whole.miny, S_whole.maxy
S_whole = common.PointOperations().filter_points_by_angle_range(
S_whole, angle - 1, angle + 1)
Y = common.Visualization().transform_points_to_bmp_with_bounds(
S_whole, bmpsize, minx, maxx, miny, maxy)
Y = self.circular_mask(Y)
bestangle = self.compute_bestangle(Y)
if self.do_visualization:
self.visualize_at_derivative(Y, angle=bestangle, padding=1)
G = np.zeros(Y.shape)
y = Y.shape[0] / 2
xs = np.linspace(Y.shape[0] / 4, 3 * Y.shape[0] / 4, 50)
for i in range(xs.shape[0]):
G[int(y), int(xs[i])] = 1
G = self.rotate_matrix(G, angle)
xs, ys = np.where(G == 1)
scan_direction = [[[xs[0], ys[0]], [xs[len(xs) - 1], ys[len(ys) - 1]]]]
return scan_direction
def visualize_at_derivative(self, Y, angle, padding):
G = np.zeros(Y.shape)
y = Y.shape[0] / 2
xs = np.linspace(Y.shape[0] / 4, 3 * Y.shape[0] / 4, 50)
for i in range(xs.shape[0]):
G[int(y), int(xs[i])] = 1
G = self.rotate_matrix(G, angle)
# take all points where G = 1
xs, ys = np.where(G == 1)
# make array of points from it
points = [[x, y] for x, y in zip(list(xs), list(ys))]
# again project to bmp
D = common.Visualization().transform_rawpoints_to_bmp_with_bounds(
points, Y.shape[0], 0, Y.shape[0], 0, Y.shape[0])
Y_trans = self.rotate_matrix(Y, angle)
Y_trans_padded = np.hstack((np.zeros(
(padding, Y.shape[0])).T, Y_trans[:, :-padding]))
deriv = np.abs(Y_trans - Y_trans_padded)
tmp = deriv[5:-5, 5:-5]
tmp = self.circular_mask(tmp)
deriv = np.zeros(Y.shape)
deriv[5:-5, 5:-5] = tmp
Y_sides = np.zeros((Y.shape[0], Y.shape[1] * 6))
Y_sides[:, :Y.shape[0]] = Y
Y_sides[:, Y.shape[0]:2 * Y.shape[0]] = Y_trans
Y_sides[:, 2 * Y.shape[0]:3 * Y.shape[0]] = Y_trans_padded
Y_sides[:, 3 * Y.shape[0]:4 * Y.shape[0]] = deriv
Y_sides[:, 4 * Y.shape[0]:5 * Y.shape[0]] = G
Y_sides[:, 5 * Y.shape[0]:] = D
common.HoughTransform.visualize_matrix(Y_sides)
import common
lidar_folder = 'E:/workspaces/LIDAR_WORKSPACE/lidar'
name = "386_95"
names = common.get_dataset_names(lidar_folder)
names.sort()
for name in names:
print(name)
dataset = common.RawLidarDatasetNormXYZRGBAngle(lidar_folder, name)
bmp = common.Visualization().transform_dataset_to_scananglebmp(
dataset, bmpsize=4000)
common.HoughTransform().visualize_scananglematrix(bmp)
def run(self, dataset: common.LidarDatasetNormXYZRGBAngle,
augmentable: common.Augmentable):
########################
## PREP DATA
########################
# find alpha of nearest neighbour
aug_loc = (augmentable.location[0] - dataset.minx,
augmentable.location[1] - dataset.miny)
minptidx = dataset.find_closest_neighbour(aug_loc)
minpt = dataset.points[minptidx]
minptalpha = minpt.scan_angle
# compute params
R = 2.5 * self.length_of_one_degree(minptalpha,
self.default_height) / 2
bmpsizenbrs = int(self.bmpsize_full_dataset / 1000 * R)
self.printifverbose("Finding airplane properties")
# S_whole = assume x = 1000, take 2.5 degrees of range for it, divide by 2 because it's polmer!
nbr_indices = dataset.find_neighbours(aug_loc, R=R)
S_whole = common.PointSet([dataset.points[i] for i in nbr_indices])
if self.do_visualization:
common.Visualization().visualize(S_whole, S_whole.minx,
S_whole.miny, S_whole.maxx,
S_whole.maxy, bmpsizenbrs)
# Find out whether both neighboring degrees are present
angles = set([p.scan_angle for p in S_whole.points])
if len(list(angles)) < 2:
print(
'OMFGGGGGG F*****G PROBLEEEEEEEEEEEM!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!'
)
print('UNIQUE ANGLES IN S_WHOLE:' + str(angles))
hasbothangles = minptalpha - 1 in angles and minptalpha + 1 in angles
if not hasbothangles:
# select new point from the neighboring angle
selected = 0
for p in S_whole.points:
if p.scan_angle == minptalpha - 1 or p.scan_angle == minptalpha + 1:
selected = p
break
minptalpha = p.scan_angle
# compute new S_whole around it and params (because neighboring degree to alpha-1 may not be contained in old S_whole)
R = 2.5 * self.length_of_one_degree(minptalpha,
self.default_height) / 2
bmpsizenbrs = int(self.bmpsize_full_dataset / 1000 * R)
nbr_indices = dataset.find_neighbours(aug_loc, R=R)
S_whole = common.PointSet([dataset.points[i] for i in nbr_indices])
# S_small = from S_big take only the points that are the same degree
S_small = common.PointOperations().filter_points_by_angle(
S_whole, minptalpha)
if self.do_visualization:
common.Visualization().visualize(S_small, S_whole.minx,
S_whole.miny, S_whole.maxx,
S_whole.maxy, bmpsizenbrs)
########################
## FROM DEGREE RANGE ESTIMATE HEIGHT
########################
# find nearest points with the neighboring angle
angle_nbrs = self.nearest_angle_neighbors(dataset=dataset,
S_small=S_small)
if self.do_visualization:
common.Visualization().visualize(angle_nbrs, S_whole.minx,
S_whole.miny, S_whole.maxx,
S_whole.maxy, bmpsizenbrs)
# average points
p_min, p_max = self.average_points_in_clusters(points=angle_nbrs)
if self.do_visualization:
common.Visualization().visualize_points([p_min, p_max],
S_whole.minx, S_whole.miny,
S_whole.maxx, S_whole.maxy,
bmpsizenbrs)
# from p_min and p_max now compute dist, scan_direction and height x
dist = math.sqrt(((p_max[0] - p_min[0])**2) +
((p_max[1] - p_min[1])**2))
scan_direction = np.array([p_max[0] - p_min[0], p_max[1] - p_min[1]])
scan_direction = scan_direction / np.linalg.norm(scan_direction)
height = self.height_at_degree(angle=minptalpha, dist=dist)
# need to determine actual correct direction
way_vector = np.cross(
np.array([scan_direction[0], scan_direction[1], 0.0]),
np.array([0.0, 0.0, 1.0]))
way_vector = way_vector[:2]
if self.do_visualization:
a = p_max + 20 * way_vector[:2]
common.Visualization().visualize_points([p_max, a], S_whole.minx,
S_whole.miny, S_whole.maxx,
S_whole.maxy, bmpsizenbrs)
# Interpolate the true scan angle
aug = np.array([
minpt.X, minpt.Y
]) # because we're working with minpt not with aug directly!
p1 = 0
p2 = 0
for i in range(1, 50):
p = aug + i * scan_direction
nbridxs = dataset.find_neighbours(p, R=1.0)
for i in nbridxs:
if (dataset.points[i].scan_angle == minptalpha + 1):
p2 = p
break
for i in range(1, 50):
p = aug - i * scan_direction
nbridxs = dataset.find_neighbours(p, R=1.0)
for i in nbridxs:
if (dataset.points[i].scan_angle == minptalpha - 1):
p1 = p
break
a1 = minptalpha
a2 = minptalpha + 1
x_m = 0
x_n = np.linalg.norm(p1 - p2)
x_A = np.linalg.norm(p2 - aug)
a_m = a1
a_n = a2
a_aug = a_m + (x_A - x_m) * (a_n - a_m) / (x_n - x_m)
# Compute airplane_position
x = np.array([0, 0, height])
xtana = scan_direction * height * math.tan(a_aug)
xtana = np.array([xtana[0], xtana[1], 0])
A = [minpt.X, minpt.Y, minpt.Z]
airplane_position = A + (x + xtana)
airplane_ortho_direction = [[0, 0],
[scan_direction[0], scan_direction[1]]]
self.printifverbose("Finding scan directions")
# find scan direction
derivdirs = []
houghdirs = []
for bmpsize in self.bmpswathspan:
self.printifverbose("For bmpsize=" + str(bmpsize))
R = 2.5 * self.length_of_one_degree(minptalpha,
self.default_height) / 2
bmpsizenbrs = int(bmpsize / 1000 * R)
self.printifverbose("Using derivative")
scan_direction_derivative = m_derivative.DerivativeMethod(
).scan_direction_derivative(S_whole, minptalpha, bmpsizenbrs)
derivdirs.append(scan_direction_derivative)
self.printifverbose("Using hough")
scan_direction_hough = m_hough.HoughMethod().scan_direction_hough(
S_whole, minptalpha, bmpsizenbrs)
houghdirs.append(scan_direction_hough)
return airplane_position, airplane_ortho_direction, derivdirs, houghdirs