def updateFromPointCloud(self, points):
""" update current state from new point cloud """
# filter away non-floor based on previous transform
old_principal_transformed_points = points = self.doTransform(points)
points = array([pt for i, pt in enumerate(points) if is_floor(points[i, :])])
# filter away outliers
outlier_filtered_points = points = array([pt for pt in points if norm(pt) < 5e3])
# take only points near mean
mean = points.mean(0)
points = array([pt for pt in points if norm(pt - mean) < 1e3])
# sanity checks
if len(old_principal_transformed_points) < 20: # = num points before outlier filter
rospy.logwarn("Warning: too little #points")
return
elif len(outlier_filtered_points) < 20: # = num points after outlier filter
rospy.logwarn("Warning: mean of points too large")
return
elif len(points) < 20: # = num points near mean
rospy.logwarn("Warning: too little #points near mean")
return
# perform PCA
mean, eigs = self.performPCA(points)
if not eigs:
return
# get transform-matrix from eigenvalues
self.T = self.getMatrix(mean, eigs) * self.T
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 updateFromPointCloud(self, points):
""" update current state from new point cloud """
# filter away non-floor based on previous transform
old_principal_transformed_points = points = self.doTransform(points)
points = array(
[pt for i, pt in enumerate(points) if is_floor(points[i, :])])
# filter away outliers
outlier_filtered_points = points = array(
[pt for pt in points if norm(pt) < 5e3])
# take only points near mean
mean = points.mean(0)
points = array([pt for pt in points if norm(pt - mean) < 1e3])
# sanity checks
if len(old_principal_transformed_points
) < 20: # = num points before outlier filter
rospy.logwarn("Warning: too little #points")
return
elif len(outlier_filtered_points
) < 20: # = num points after outlier filter
rospy.logwarn("Warning: mean of points too large")
return
elif len(points) < 20: # = num points near mean
rospy.logwarn("Warning: too little #points near mean")
return
# perform PCA
mean, eigs = self.performPCA(points)
if not eigs: return
# get transform-matrix from eigenvalues
self.T = self.getMatrix(mean, eigs) * self.T
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)))
