def removeOriginPoints(polyData):
"""
openni2-lcm driver publishes 0.0 depth for points with invalid range
:param polyData:
:return:
"""
points = vnp.getNumpyFromVtk(polyData, 'Points')
labels = np.array(np.sum(points, axis=1) == 0.0, dtype=int)
vnp.addNumpyToVtk(polyData, labels, 'is_origin_point')
return filterUtils.thresholdPoints(polyData, 'is_origin_point',
[0.0, 0.0])
def cropToLineSegment(polyData, point1, point2, data_type='cells'):
line = np.array(point2) - np.array(point1)
length = np.linalg.norm(line)
axis = line / length
polyData = labelPointDistanceAlongAxis(polyData, axis, origin=point1, resultArrayName='dist_along_line')
if data_type == "cells":
return filterUtils.thresholdCells(polyData, 'dist_along_line', [0.0, length], arrayType="points")
elif data_type == "points":
return filterUtils.thresholdPoints(polyData, 'dist_along_line', [0.0, length])
else:
raise ValueError("unknown data_type = %s" %(data_type))
def cropPointCloudToModel(pointCloud,
objectPointCloud,
distanceThreshold=0.02,
visualize=True,
applyEuclideanClustering=True):
"""
Crops pointCloud to just the points withing distanceThreshold of
objectPointCloud
:param pointCloud:
:param objectPointCloud:
:param distanceThreshold:
:return: cropped pointcloud
"""
registration = GlobalRegistrationUtils
pointCloud = GlobalRegistration.cropPointCloudToModelBoundingBox(
pointCloud, objectPointCloud, scaleFactor=1.5)
arrayName = 'distance_to_mesh'
print "computing point to point distance"
dists = registration.computePointToPointDistance(
pointCloud, objectPointCloud)
vnp.addNumpyToVtk(pointCloud, dists, arrayName)
polyData = filterUtils.thresholdPoints(pointCloud, arrayName,
[0.0, distanceThreshold])
# this stuff may be unecessary
if applyEuclideanClustering:
# polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
polyData = segmentation.applyEuclideanClustering(
polyData, clusterTolerance=0.04)
polyData = segmentation.thresholdPoints(polyData, 'cluster_labels',
[1, 1])
if visualize:
parent = om.getOrCreateContainer('global registration')
vis.updatePolyData(polyData,
'cropped pointcloud',
color=[0, 1, 0],
parent=parent)
return polyData
def cropToLineSegment(polyData, point1, point2, data_type='cells'):
line = np.array(point2) - np.array(point1)
length = np.linalg.norm(line)
axis = line / length
polyData = labelPointDistanceAlongAxis(polyData,
axis,
origin=point1,
resultArrayName='dist_along_line')
if data_type == "cells":
return filterUtils.thresholdCells(polyData,
'dist_along_line', [0.0, length],
arrayType="points")
elif data_type == "points":
return filterUtils.thresholdPoints(polyData, 'dist_along_line',
[0.0, length])
else:
raise ValueError("unknown data_type = %s" % (data_type))
def segmentTable(scenePolyData=None,
searchRadius=0.3,
visualize=True,
thickness=0.01,
pointOnTable=None,
pointAboveTable=None,
computeAboveTablePolyData=False):
"""
This requires two clicks using measurement panel. One on the table, one above the table on one of the objects. Call them point0, point1. Then we will attempt to fit a plane that passes through point0 with approximate normal point1 - point0
:param scenePolyData:
:param searchRadius:
:return:
"""
if scenePolyData is None:
scenePolyData = om.findObjectByName('reconstruction').polyData
assert scenePolyData is not None
assert pointOnTable is not None
assert pointAboveTable is not None
expectedNormal = pointAboveTable - pointOnTable
expectedNormal = expectedNormal / np.linalg.norm(expectedNormal)
polyData, normal = segmentation.applyPlaneFit(
scenePolyData,
searchOrigin=pointOnTable,
searchRadius=searchRadius,
expectedNormal=expectedNormal)
# get points above plane
abovePolyData = None
belowPolyData = None
if computeAboveTablePolyData:
abovePolyData = filterUtils.thresholdPoints(
polyData, 'dist_to_plane', [thickness / 2.0, np.inf])
belowPolyData = filterUtils.thresholdPoints(
polyData, 'dist_to_plane', [-np.inf, -thickness / 2.0])
# some debugging visualization
if visualize:
visFolder = om.getOrCreateContainer('debug')
if abovePolyData is not None:
vis.showPolyData(abovePolyData,
'above table segmentation',
color=[0, 1, 0],
parent=visFolder)
arrowLength = 0.3
headRadius = 0.02
d = DebugData()
visFolder = om.getOrCreateContainer('debug')
d.addArrow(pointOnTable,
pointOnTable + arrowLength * expectedNormal,
headRadius=headRadius)
vis.showPolyData(d.getPolyData(),
'expected normal',
color=[1, 0, 0],
parent=visFolder)
d = DebugData()
d.addArrow(pointOnTable,
pointOnTable + arrowLength * normal,
headRadius=headRadius)
vis.showPolyData(d.getPolyData(),
'computed normal',
color=[0, 1, 0],
parent=visFolder)
returnData = dict()
returnData['abovePolyData'] = abovePolyData
returnData['polyData'] = polyData
returnData['normal'] = normal
returnData['pointOnTable'] = pointOnTable
return returnData
def getNonSelectedPoints(self):
return filterUtils.thresholdPoints(self.polyData, 'is_selected', [0, 0])
def getSelectedPoints(self):
return filterUtils.thresholdPoints(self.polyData, "is_selected",
[1, 1])