def mark(self):
blocks = []
xy_lim = get_xy_lim(self.points) # [x_min, x_max, y_min, y_max]
self.origin = [xy_lim[0], xy_lim[2]]
row = int(ceil((xy_lim[3] - xy_lim[2]) / self.resolution))
col = int(ceil((xy_lim[1] - xy_lim[0]) / self.resolution))
reg = LinearRegressor()
for i in range(row):
row_blocks = []
for j in range(col):
position = [i, j]
cordinate = self.pos2cordinate(position)
points = filter_points(self.points, cordinate[0], cordinate[0] + self.resolution, \
cordinate[1], cordinate[1] + self.resolution)
if len(
points
) < self.LOWEST_POINTS_COUNT: # very few points in the block, param is None
row_blocks.append(Block(None, points, position))
else:
reg.process(points)
param = reg.get_parameters()[0]
slope = k2slope(param[0])
row_blocks.append(Block(param, points, position))
blocks.append(row_blocks)
return blocks
def get_intersections(self):
k = self.param[0]
b = self.param[1]
xy_lim = get_xy_lim(self.points)
if k == 0:
return [[xy_lim[0], b], [xy_lim[1], b]]
elif k == float('inf') and b != float('inf'):
return [[b, xy_lim[2]], [b, xy_lim[3]]]
else:
x = [
xy_lim[0], xy_lim[1], (xy_lim[2] - b) / k, (xy_lim[3] - b) / k
]
x.sort()
return [[i, k * i + b] for i in x if x[0] < i < x[3]]
def process(self, points):
k, b = get_k_b(points)
self.parameters = [[k, b]]
xy_lim = get_xy_lim(points)
if k == 0:
return [[xy_lim[0], b], [xy_lim[1], b]]
elif k == float('inf') and b != float('inf'):
return [[b, xy_lim[2]], [b, xy_lim[3]]]
else:
x = [
xy_lim[0], xy_lim[1], (xy_lim[2] - b) / k, (xy_lim[3] - b) / k
]
x.sort()
return [[i, k * i + b] for i in x if x[0] < i < x[3]]
1. Maybe rewrite PointsDivider
2. Put outliers inside boxes
"""
verbose = True
# Linear regression
controller = RegressionController(path='0.pcd', verbose=verbose)
divider = PointsDivider()
controller.set_parts(divider, 0.5)
reg = LinearRegressor()
controller.fit(reg)
# set figure
padding = 0.2
xy_lim = get_xy_lim(controller.points)
ratio = (xy_lim[3] - xy_lim[2]) / float(xy_lim[1] - xy_lim[0])
fig = plt.figure(figsize=(10 * ratio, 10))
fig.patch.set_facecolor('#000000')
ax1 = fig.add_subplot(1, 1, 1)
ax1.grid(True, linewidth=0.5, color='#999999', linestyle='dotted')
ax1.set_facecolor('#000000')
ax1.axis([
xy_lim[0] - padding, xy_lim[1] + padding, xy_lim[2] - padding,
xy_lim[3] + padding
])
# scatter points
xy = controller.points
xs = [i[0] for i in xy]
ys = [i[1] for i in xy]
return str(self.position) + ": " + str(self.slope)
def __repr__(self):
return str(self)
if __name__ == "__main__":
import matplotlib
matplotlib.use('TkAgg')
import matplotlib.pyplot as plt
points = get_points_from_pcd("four_walls.pcd")
# Config figure
padding = 0.2
xy_lim = get_xy_lim(points)
ratio = (xy_lim[3] - xy_lim[2]) / float(xy_lim[1] - xy_lim[0])
fig = plt.figure(figsize=(10 * ratio, 10))
ax1 = fig.add_subplot(1, 1, 1)
ax1.grid(True, linewidth=0.5, color='#666666', linestyle='dotted')
ax1.axis([
xy_lim[0] - padding, xy_lim[1] + padding, xy_lim[2] - padding,
xy_lim[3] + padding
])
ax1.set_color_cycle(['red', 'black', 'blue', 'brown', 'green'])
# ax1.scatter([p[0] for p in points], [p[1] for p in points], color='red', s=1)
# Marking
resolution = 0.5
marker = BlockMarker(resolution)
marker.set_points(points)
2. Put outliers inside boxes
"""
verbose = True
# Linear regression
controller = RegressionController(path='four_walls.pcd', verbose=verbose)
divider = PointsDivider()
controller.set_parts(divider)
gau = GaussianRegressor()
controller.fit(gau)
# set figure
padding = 0.2
xy_lim = get_xy_lim(controller.points)
ratio = (xy_lim[3] - xy_lim[2]) / float(xy_lim[1] - xy_lim[0])
fig = plt.figure(figsize=(10 * ratio, 10))
fig.patch.set_facecolor('#000000')
ax1 = fig.add_subplot(1, 1, 1)
ax1.grid(True, linewidth=0.5, color='#999999', linestyle='dotted')
ax1.set_facecolor('#000000')
ax1.axis([
xy_lim[0] - padding, xy_lim[1] + padding, xy_lim[2] - padding,
xy_lim[3] + padding
])
# scatter points
xy = controller.points
print("xy:", len(xy))
xs = [i[0] for i in xy]