def preprocess_points(points):
global FANCY_OUTPUT
## divide points ##
wallpoints = array([p for p in points if floorseparator.is_wall(p)])
floorpoints = array([p for p in points if floorseparator.is_floor(p)])
## check if enough points
if len(wallpoints) < MIN_NUM_POINTS: return None
if len(floorpoints) < MIN_NUM_POINTS: return None
## convert 3D points to 2D ##
if wallpoints.shape[1] == 3:
wallpoints = wallpoints[:, :2]
floorpoints = floorpoints[:, :2]
## plot points
if FANCY_OUTPUT:
plot_points([p for p in wallpoints if norm(p) < 10e3])
pl.legend(['raw wall points'], 'best')
pl.draw()
## filter outliers by clustering ##
wallpoints = outlierfilterlib.filter_outliers(wallpoints, threshold=300, min_cluster_size=20)
clusters = outlierfilterlib.filter_outliers(wallpoints, threshold=150, min_cluster_size=1, return_clusters=True)
## plot filtered points
if FANCY_OUTPUT:
pts = []
for c in clusters: pts += c.tolist()
pl.clf()
plot_points(pts, '.y')
pl.legend(['after cluster filter'], 'best')
pl.draw()
## filter outliers by floor points ##
definite_points = prefilterlib.get_definite_points(floorpoints)
envelope_points = prefilterlib.envelope_from_points(definite_points)
wallpoints = prefilterlib.filter_clusters_with_envelope(envelope_points, clusters)
## plot filtered points
if FANCY_OUTPUT:
plot_points(wallpoints, '.b')
pl.legend(['after cluster filter', 'after floor filter'], 'best')
pl.draw()
## enlighten dense areas ##
wallpoints = prefilterlib.enlighten_dense_areas(wallpoints)
## plot filtered points
if FANCY_OUTPUT:
pl.clf()
plot_points(wallpoints)
pl.legend(['final'], 'best')
pl.draw()
## check if enough points
if len(wallpoints) < MIN_NUM_POINTS:
return None
return wallpoints
def plot_points(string):
""" get points """
points_arr = decode_string_to_array(string)
points_arr = [p for p in points_arr if norm(p) < 100e3]
""" publish old, untransformed points """
marker = get_marker('old_points', Marker.SPHERE_LIST)
marker.pose.orientation.w = 1.0
# POINTS markers use x and y scale for width/height respectively
marker.scale.x = marker.scale.y = marker.scale.z = 50
# Points are green
marker.color.r = .2
marker.color.g = .2
marker.color.a = 1.0
# Create the vertices for the points and lines
for pt in points_arr:
p = Point()
p.x = pt[0]
p.y = pt[1]
p.z = pt[2]
marker.points.append(p)
# publish
publisher.publish(marker)
""" transform points """
points_arr = principalplane.updateAndTransform(points_arr)
""" publish floor """
marker = get_marker('floor_points', Marker.SPHERE_LIST)
marker.pose.orientation.w = 1.0
# POINTS markers use x and y scale for width/height respectively
marker.scale.x = marker.scale.y = marker.scale.z = 50
# set color
marker.color.r = .8
marker.color.g = .8
marker.color.a = 1.0
# Create the vertices for the points and lines
for pt in (x for x in points_arr if is_floor(x)):
p = Point()
p.x = pt[0]
p.y = pt[1]
p.z = pt[2]
marker.points.append(p)
# publish
publisher.publish(marker)
""" publish walls """
marker = get_marker('wall_points', Marker.SPHERE_LIST)
marker.pose.orientation.w = 1.0
# POINTS markers use x and y scale for width/height respectively
marker.scale.x = marker.scale.y = marker.scale.z = 50
# set color
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
# Create the vertices for the points and lines
for pt in (x for x in points_arr if is_wall(x)):
p = Point()
p.x = pt[0]
p.y = pt[1]
p.z = pt[2]
marker.points.append(p)
# publish
publisher.publish(marker)
rospy.loginfo("published {} points".format(len(points_arr)))
def preprocess_points(points):
global FANCY_OUTPUT
## divide points ##
wallpoints = array([p for p in points if floorseparator.is_wall(p)])
floorpoints = array([p for p in points if floorseparator.is_floor(p)])
## check if enough points
if len(wallpoints) < MIN_NUM_POINTS: return None
if len(floorpoints) < MIN_NUM_POINTS: return None
## convert 3D points to 2D ##
if wallpoints.shape[1] == 3:
wallpoints = wallpoints[:, :2]
floorpoints = floorpoints[:, :2]
## plot points
if FANCY_OUTPUT:
plot_points([p for p in wallpoints if norm(p) < 10e3])
pl.legend(['raw wall points'], 'best')
pl.draw()
## filter outliers by clustering ##
wallpoints = outlierfilterlib.filter_outliers(wallpoints,
threshold=300,
min_cluster_size=20)
clusters = outlierfilterlib.filter_outliers(wallpoints,
threshold=150,
min_cluster_size=1,
return_clusters=True)
## plot filtered points
if FANCY_OUTPUT:
pts = []
for c in clusters:
pts += c.tolist()
pl.clf()
plot_points(pts, '.y')
pl.legend(['after cluster filter'], 'best')
pl.draw()
## filter outliers by floor points ##
definite_points = prefilterlib.get_definite_points(floorpoints)
envelope_points = prefilterlib.envelope_from_points(definite_points)
wallpoints = prefilterlib.filter_clusters_with_envelope(
envelope_points, clusters)
## plot filtered points
if FANCY_OUTPUT:
plot_points(wallpoints, '.b')
pl.legend(['after cluster filter', 'after floor filter'], 'best')
pl.draw()
## enlighten dense areas ##
wallpoints = prefilterlib.enlighten_dense_areas(wallpoints)
## plot filtered points
if FANCY_OUTPUT:
pl.clf()
plot_points(wallpoints)
pl.legend(['final'], 'best')
pl.draw()
## check if enough points
if len(wallpoints) < MIN_NUM_POINTS:
return None
return wallpoints
def plot_points(string):
""" get points """
points_arr = decode_string_to_array(string)
points_arr = [p for p in points_arr if norm(p) < 100e3]
""" publish old, untransformed points """
marker = get_marker('old_points', Marker.SPHERE_LIST)
marker.pose.orientation.w = 1.0
# POINTS markers use x and y scale for width/height respectively
marker.scale.x = marker.scale.y = marker.scale.z = 50
# Points are green
marker.color.r = .2
marker.color.g = .2
marker.color.a = 1.0
# Create the vertices for the points and lines
for pt in points_arr:
p = Point()
p.x = pt[0]
p.y = pt[1]
p.z = pt[2]
marker.points.append(p)
# publish
publisher.publish(marker)
""" transform points """
points_arr = principalplane.updateAndTransform(points_arr)
""" publish floor """
marker = get_marker('floor_points', Marker.SPHERE_LIST)
marker.pose.orientation.w = 1.0
# POINTS markers use x and y scale for width/height respectively
marker.scale.x = marker.scale.y = marker.scale.z = 50
# set color
marker.color.r = .8
marker.color.g = .8
marker.color.a = 1.0
# Create the vertices for the points and lines
for pt in (x for x in points_arr if is_floor(x)):
p = Point()
p.x = pt[0]
p.y = pt[1]
p.z = pt[2]
marker.points.append(p)
# publish
publisher.publish(marker)
""" publish walls """
marker = get_marker('wall_points', Marker.SPHERE_LIST)
marker.pose.orientation.w = 1.0
# POINTS markers use x and y scale for width/height respectively
marker.scale.x = marker.scale.y = marker.scale.z = 50
# set color
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
# Create the vertices for the points and lines
for pt in (x for x in points_arr if is_wall(x)):
p = Point()
p.x = pt[0]
p.y = pt[1]
p.z = pt[2]
marker.points.append(p)
# publish
publisher.publish(marker)
rospy.loginfo("published {} points".format(len(points_arr)))
def updatePoints(self, points_str):
points_arr = decode_string_to_array(points_str)
points_arr = self.principalplane.updateAndTransform(points_arr)
self.wall_points = [x for x in points_arr if is_wall(x)]
def updatePoints(self, points_str):
points_arr = decode_string_to_array(points_str)
points_arr = self.principalplane.updateAndTransform(points_arr)
self.wall_points = [x for x in points_arr if is_wall(x)]
