def onPointCloud(self, msg, channel):
pointcloudName = channel.replace('DRAKE_POINTCLOUD_', '', 1)
polyData = vnp.numpyToPolyData(np.asarray(msg.points), createVertexCells=True)
# If the user provided color channels, then use them to colorize
# the pointcloud.
channels = {msg.channel_names[i]: msg.channels[i] for i in range(msg.n_channels)}
if "r" in channels and "g" in channels and "b" in channels:
colorized = True
colorArray = np.empty((msg.n_points, 3), dtype=np.uint8)
for (colorIndex, color) in enumerate(["r", "g", "b"]):
colorArray[:, colorIndex] = 255 * np.asarray(channels[color])
vnp.addNumpyToVtk(polyData, colorArray, "rgb")
else:
colorized = False
folder = self.getPointCloudFolder()
# If there was an existing point cloud by this name, then just
# set its polyData to the new point cloud.
# This has the effect of preserving all the user-specified properties
# like point size, coloration mode, alpha, etc.
previousPointcloud = folder.findChild(pointcloudName)
if previousPointcloud is not None:
previousPointcloud.setPolyData(polyData)
previousPointcloud._updateColorByProperty()
else:
item = vis.PolyDataItem(pointcloudName, polyData, view=None)
item.addToView(self.view)
if colorized:
item._updateColorByProperty()
item.setProperty("Color By", "rgb")
om.addToObjectModel(item, parentObj=folder)
def labelNonFinitePoints(polyData, arrayName='Points'):
'''
adds is_nonfinite label to polyData. non finite includes nan and +/- inf.
'''
pts = vnp.getNumpyFromVtk(polyData, arrayName)
labels = np.logical_not(np.isfinite(pts)).any(axis=1)
vnp.addNumpyToVtk(polyData, np.array(labels, dtype=np.int32), 'is_nonfinite')
def removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95):
yvalues = vnp.getNumpyFromVtk(polyData, 'Points')[:, whichAxis]
backY = np.percentile(yvalues, percentile)
if ( percentile > 50):
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [backY - 0.1, np.inf])
else:
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [-np.inf, backY + 0.1])
vis.updatePolyData(searchRegion, 'search region', parent="segmentation", colorByName=whichAxisLetter, visible=False)
# find the plane of the back wall, remove it and the points behind it:
_, origin, normal = segmentation.applyPlaneFit(searchRegion, distanceThreshold=0.02, expectedNormal=expectedNormal, perpendicularAxis=expectedNormal, returnOrigin=True)
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
backFrame = transformUtils.getTransformFromOriginAndNormal(origin, normal, normalAxis=2)
vis.updateFrame(backFrame, 'back frame', parent='segmentation', scale=0.15 , visible=False)
vis.updatePolyData(polyData, 'dist to back', parent='segmentation', visible=False)
polyData = segmentation.thresholdPoints(polyData, 'dist_to_plane', filterRange)
vis.updatePolyData(polyData, 'back off and all', parent='segmentation', visible=False)
return polyData
def __init__(self, view, polyData):
self.view = view
self.polyData = shallowCopy(polyData)
self.selectionObj = None
self.selectionColor = [1, 0, 0]
self.selectionPointSize = 3
self.selectMode = 1
self.iren = view.renderWindow().GetInteractor()
self.prevStyle = self.iren.GetInteractorStyle()
self.rubberBandStyle = vtk.vtkInteractorStyleRubberBand3D()
self.rubberBandStyle.AddObserver("SelectionChangedEvent",
self.onRubberBandPickEvent)
self.eventFilter = PointSelector.EventFilter(view)
self.eventFilter.selector = self
vnp.addNumpyToVtk(
self.polyData,
np.arange(self.polyData.GetNumberOfPoints(), dtype=int),
"point_ids",
)
vnp.addNumpyToVtk(
self.polyData,
np.zeros(self.polyData.GetNumberOfPoints(), dtype=int),
"is_selected",
)
def fitObjectsOnShelf(polyData, maxHeight = 0.25):
# find the shelf plane:
polyDataWithoutFront, _ = segmentation.removeMajorPlane(polyData, distanceThreshold=0.02)
polyDataPlaneFit, origin, normal = segmentation.applyPlaneFit(polyDataWithoutFront, expectedNormal=np.array([0.0,0.0,1.0]), perpendicularAxis=np.array([0.0,0.0,1.0]), returnOrigin=True)
vis.updatePolyData(polyDataPlaneFit, 'polyDataPlaneFit', parent='segmentation', visible=False)
shelfSurfacePoints = segmentation.thresholdPoints(polyDataPlaneFit, 'dist_to_plane', [-0.01, 0.01])
shelfCenter = segmentation.computeCentroid(shelfSurfacePoints)
shelfFrame = transformUtils.getTransformFromOriginAndNormal(shelfCenter, normal, normalAxis=2)
vis.showFrame(shelfFrame, 'shelfFrame', parent='segmentation', scale=0.15 , visible=False)
# find the points near to the shelf plane and find objects on it:
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
shelfPoints = segmentation.thresholdPoints(polyData, 'dist_to_plane', [-0.01, maxHeight])
vis.updatePolyData(shelfPoints, 'shelf', parent='segmentation', visible=False)
data = segmentation.segmentTableScene(shelfPoints, shelfCenter, filterClustering = False )
vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')
# remove the points that we considered from the orginal cloud
dists = vnp.getNumpyFromVtk(polyData, 'dist_to_plane')
diffShelf = ( ((dists > maxHeight) + (dists < -0.01))) + 0.1 -0.1
vnp.addNumpyToVtk(polyData, diffShelf, 'diff_shelf')
polyData = segmentation.thresholdPoints(polyData, 'diff_shelf', [1, 1])
vis.updatePolyData(polyData, 'rest', parent='segmentation', visible=False)
return polyData
def fitObjectsOnShelf(polyData, maxHeight = 0.25):
# find the shelf plane:
polyDataWithoutFront, _ = segmentation.removeMajorPlane(polyData, distanceThreshold=0.02)
polyDataPlaneFit, origin, normal = segmentation.applyPlaneFit(polyDataWithoutFront, expectedNormal=np.array([0.0,0.0,1.0]), perpendicularAxis=np.array([0.0,0.0,1.0]), returnOrigin=True)
vis.updatePolyData(polyDataPlaneFit, 'polyDataPlaneFit', parent='segmentation', visible=False)
shelfSurfacePoints = segmentation.thresholdPoints(polyDataPlaneFit, 'dist_to_plane', [-0.01, 0.01])
shelfCenter = segmentation.computeCentroid(shelfSurfacePoints)
shelfFrame = transformUtils.getTransformFromOriginAndNormal(shelfCenter, normal, normalAxis=2)
vis.showFrame(shelfFrame, 'shelfFrame', parent='segmentation', scale=0.15 , visible=False)
# find the points near to the shelf plane and find objects on it:
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
shelfPoints = segmentation.thresholdPoints(polyData, 'dist_to_plane', [-0.01, maxHeight])
vis.updatePolyData(shelfPoints, 'shelf', parent='segmentation', visible=False)
data = segmentation.segmentTableScene(shelfPoints, shelfCenter, filterClustering = False )
vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')
# remove the points that we considered from the orginal cloud
dists = vnp.getNumpyFromVtk(polyData, 'dist_to_plane')
diffShelf = ( ((dists > maxHeight) + (dists < -0.01))) + 0.1 -0.1
vnp.addNumpyToVtk(polyData, diffShelf, 'diff_shelf')
polyData = segmentation.thresholdPoints(polyData, 'diff_shelf', [1, 1])
vis.updatePolyData(polyData, 'rest', parent='segmentation', visible=False)
return polyData
def getSphereGeometry(self, imageName):
sphereObj = self.sphereObjects.get(imageName)
if sphereObj:
return sphereObj
if not self.imageManager.getImage(imageName).GetDimensions()[0]:
return None
sphereResolution = 50
sphereRadii = {"CAMERA_LEFT": 20, "CAMERACHEST_LEFT": 20, "CAMERACHEST_RIGHT": 20}
geometry = makeSphere(sphereRadii[imageName], sphereResolution)
self.imageManager.queue.computeTextureCoords(imageName, geometry)
tcoordsArrayName = "tcoords_%s" % imageName
vtkNumpy.addNumpyToVtk(geometry, vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 0].copy(), "tcoords_U")
vtkNumpy.addNumpyToVtk(geometry, vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 1].copy(), "tcoords_V")
geometry = clipRange(geometry, "tcoords_U", [0.0, 1.0])
geometry = clipRange(geometry, "tcoords_V", [0.0, 1.0])
geometry.GetPointData().SetTCoords(geometry.GetPointData().GetArray(tcoordsArrayName))
sphereObj = vis.showPolyData(geometry, imageName, view=self.view, parent="cameras")
sphereObj.actor.SetTexture(self.imageManager.getTexture(imageName))
sphereObj.actor.GetProperty().LightingOff()
self.view.renderer().RemoveActor(sphereObj.actor)
rendererId = 2 - self.sphereImages.index(imageName)
self.renderers[rendererId].AddActor(sphereObj.actor)
self.sphereObjects[imageName] = sphereObj
return sphereObj
def onPointCloud(self, msg, channel):
pointcloudName = channel.replace("DRAKE_POINTCLOUD_", "", 1)
polyData = vnp.numpyToPolyData(np.asarray(msg.points), createVertexCells=True)
# If the user provided color channels, then use them to colorize
# the pointcloud.
channels = {msg.channel_names[i]: msg.channels[i] for i in range(msg.n_channels)}
if "r" in channels and "g" in channels and "b" in channels:
colorized = True
colorArray = np.empty((msg.n_points, 3), dtype=np.uint8)
for (colorIndex, color) in enumerate(["r", "g", "b"]):
colorArray[:, colorIndex] = 255 * np.asarray(channels[color])
vnp.addNumpyToVtk(polyData, colorArray, "rgb")
else:
colorized = False
folder = self.getPointCloudFolder()
# If there was an existing point cloud by this name, then just
# set its polyData to the new point cloud.
# This has the effect of preserving all the user-specified properties
# like point size, coloration mode, alpha, etc.
previousPointcloud = folder.findChild(pointcloudName)
if previousPointcloud is not None:
previousPointcloud.setPolyData(polyData)
previousPointcloud._updateColorByProperty()
else:
item = vis.PolyDataItem(pointcloudName, polyData, view=None)
item.addToView(self.view)
if colorized:
item._updateColorByProperty()
item.setProperty("Color By", "rgb")
om.addToObjectModel(item, parentObj=folder)
def labelNonFinitePoints(polyData, arrayName='Points'):
'''
adds is_nonfinite label to polyData. non finite includes nan and +/- inf.
'''
pts = vnp.getNumpyFromVtk(polyData, arrayName)
labels = np.logical_not(np.isfinite(pts)).any(axis=1)
vnp.addNumpyToVtk(polyData, np.array(labels, dtype=np.int32), 'is_nonfinite')
def removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95):
yvalues = vnp.getNumpyFromVtk(polyData, 'Points')[:, whichAxis]
backY = np.percentile(yvalues, percentile)
if ( percentile > 50):
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [backY - 0.1, np.inf])
else:
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [-np.inf, backY + 0.1])
vis.updatePolyData(searchRegion, 'search region', parent="segmentation", colorByName=whichAxisLetter, visible=False)
# find the plane of the back wall, remove it and the points behind it:
_, origin, normal = segmentation.applyPlaneFit(searchRegion, distanceThreshold=0.02, expectedNormal=expectedNormal, perpendicularAxis=expectedNormal, returnOrigin=True)
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
backFrame = transformUtils.getTransformFromOriginAndNormal(origin, normal, normalAxis=2)
vis.updateFrame(backFrame, 'back frame', parent='segmentation', scale=0.15 , visible=False)
vis.updatePolyData(polyData, 'dist to back', parent='segmentation', visible=False)
polyData = segmentation.thresholdPoints(polyData, 'dist_to_plane', filterRange)
vis.updatePolyData(polyData, 'back off and all', parent='segmentation', visible=False)
return polyData
def addColorChannels(polyData, channels):
if "intensity" in channels:
colorBy = "intensity"
intensity = np.asarray(channels["intensity"]) * 255
vnp.addNumpyToVtk(polyData, intensity.astype(np.uint8),
"intensity")
if "rgb" in channels:
colorBy = "rgb" # default to rgb if provided
colorArray = np.asarray(channels["rgb"]) * 255
vnp.addNumpyToVtk(polyData, colorArray.astype(np.uint8), "rgb")
def addColorChannels(polyData, channels):
if "intensity" in channels:
colorBy = "intensity"
intensity = np.asarray(channels["intensity"]) * 255
vnp.addNumpyToVtk(polyData,
intensity.astype(np.uint8),
"intensity")
if "rgb" in channels:
colorBy = "rgb" # default to rgb if provided
colorArray = np.asarray(channels["rgb"]) * 255
vnp.addNumpyToVtk(polyData, colorArray.astype(np.uint8), "rgb")
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 _update_cloud(self, xyz, rgb):
poly_data = vnp.numpyToPolyData(xyz)
vnp.addNumpyToVtk(poly_data, rgb, "rgb")
item = self._parent_folder.findChild(self._name)
if item is not None:
item.setPolyData(poly_data)
else:
view = applogic.getCurrentRenderView()
item = vis.PolyDataItem(self._name, poly_data, view=view)
item.setProperty("Color By", "rgb")
om.addToObjectModel(item, parentObj=self._parent_folder)
item._updateColorByProperty()
def getSphereGeometry(self, imageName):
sphereObj = self.sphereObjects.get(imageName)
if sphereObj:
return sphereObj
if not self.imageManager.getImage(imageName,
self.robotName).GetDimensions()[0]:
return None
sphereResolution = 50
sphereRadii = 20
geometry = makeSphere(sphereRadii, sphereResolution)
self.imageManager.queue[
self.robotName][imageName].compute_texture_coords(
imageName, geometry)
tcoordsArrayName = "tcoords_%s" % imageName
vtkNumpy.addNumpyToVtk(
geometry,
vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 0].copy(),
"tcoords_U",
)
vtkNumpy.addNumpyToVtk(
geometry,
vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 1].copy(),
"tcoords_V",
)
geometry = clipRange(geometry, "tcoords_U", [0.0, 1.0])
geometry = clipRange(geometry, "tcoords_V", [0.0, 1.0])
geometry.GetPointData().SetTCoords(
geometry.GetPointData().GetArray(tcoordsArrayName))
sphereObj = vis.showPolyData(geometry,
imageName,
view=self.view,
parent="cameras")
sphereObj.actor.SetTexture(
self.imageManager.getTexture(imageName, self.robotName))
sphereObj.actor.GetProperty().LightingOff()
self.view.renderer().RemoveActor(sphereObj.actor)
rendererId = 2 - self.images.index(imageName)
self.renderers[rendererId].AddActor(sphereObj.actor)
self.sphereObjects[imageName] = sphereObj
return sphereObj
def __init__(self, view, polyData):
self.view = view
self.polyData = shallowCopy(polyData)
self.selectionObj = None
self.selectionColor = [1, 0, 0]
self.selectionPointSize = 3
self.selectMode = 1
self.iren = view.renderWindow().GetInteractor()
self.prevStyle = self.iren.GetInteractorStyle()
self.rubberBandStyle = vtk.vtkInteractorStyleRubberBand3D()
self.rubberBandStyle.AddObserver('SelectionChangedEvent', self.onRubberBandPickEvent)
self.eventFilter = PointSelector.EventFilter(view)
self.eventFilter.selector = self
vnp.addNumpyToVtk(self.polyData, np.arange(self.polyData.GetNumberOfPoints(), dtype=int), 'point_ids')
vnp.addNumpyToVtk(self.polyData, np.zeros(self.polyData.GetNumberOfPoints(), dtype=int), 'is_selected')
def addFrame(self, frame, scale, tubeRadius=0.0):
axes = vtk.vtkAxes()
axes.ComputeNormalsOff()
axes.SetScaleFactor(scale)
transformFilter = vtk.vtkTransformPolyDataFilter()
transformFilter.SetTransform(frame)
transformFilter.SetInputConnection(axes.GetOutputPort())
transformFilter.Update()
polyData = transformFilter.GetOutput()
colors = np.array([[255, 0, 0], [255, 0, 0], [0, 255, 0], [0, 255, 0],
[0, 0, 255], [0, 0, 255]],
dtype=np.uint8)
vnp.addNumpyToVtk(polyData, colors, 'RGB255')
if tubeRadius:
polyData = applyTubeFilter(polyData, tubeRadius)
self.addPolyData(polyData, color=None)
def addFrame(self, frame, scale, tubeRadius=0.0):
axes = vtk.vtkAxes()
axes.ComputeNormalsOff()
axes.SetScaleFactor(scale)
transformFilter=vtk.vtkTransformPolyDataFilter()
transformFilter.SetTransform(frame)
transformFilter.SetInputConnection(axes.GetOutputPort())
transformFilter.Update()
polyData = transformFilter.GetOutput()
colors = np.array(
[[255, 0, 0], [255, 0, 0],
[0, 255, 0], [0, 255, 0],
[0, 0, 255], [0, 0, 255]], dtype=np.uint8)
vnp.addNumpyToVtk(polyData, colors, 'RGB255')
if tubeRadius:
polyData = applyTubeFilter(polyData, tubeRadius)
self.addPolyData(polyData, color=None)
def getSphereGeometry(self, imageName):
sphereObj = self.sphereObjects.get(imageName)
if sphereObj:
return sphereObj
if not self.imageManager.getImage(imageName).GetDimensions()[0]:
return None
sphereResolution = 50
sphereRadii = {
'CAMERA_LEFT': 20,
'CAMERACHEST_LEFT': 20,
'CAMERACHEST_RIGHT': 20
}
geometry = makeSphere(sphereRadii[imageName], sphereResolution)
self.imageManager.queue.computeTextureCoords(imageName, geometry)
tcoordsArrayName = 'tcoords_%s' % imageName
vtkNumpy.addNumpyToVtk(
geometry,
vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 0].copy(),
'tcoords_U')
vtkNumpy.addNumpyToVtk(
geometry,
vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 1].copy(),
'tcoords_V')
geometry = clipRange(geometry, 'tcoords_U', [0.0, 1.0])
geometry = clipRange(geometry, 'tcoords_V', [0.0, 1.0])
geometry.GetPointData().SetTCoords(
geometry.GetPointData().GetArray(tcoordsArrayName))
sphereObj = vis.showPolyData(geometry,
imageName,
view=self.view,
parent='cameras')
sphereObj.actor.SetTexture(self.imageManager.getTexture(imageName))
sphereObj.actor.GetProperty().LightingOff()
self.view.renderer().RemoveActor(sphereObj.actor)
rendererId = 2 - self.sphereImages.index(imageName)
self.renderers[rendererId].AddActor(sphereObj.actor)
self.sphereObjects[imageName] = sphereObj
return sphereObj
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 addHSVArrays(polyData, rgbArrayName='rgb_colors'):
import colorsys
rgb = vnp.getNumpyFromVtk(polyData, rgbArrayName) / 255.0
hsv = np.array([colorsys.rgb_to_hsv(*t) for t in rgb])
vnp.addNumpyToVtk(polyData, hsv[:, 0].copy(), 'hue')
vnp.addNumpyToVtk(polyData, hsv[:, 1].copy(), 'saturation')
vnp.addNumpyToVtk(polyData, hsv[:, 2].copy(), 'value')
def addPolyData(self, polyData, color=[1,1,1], extraLabels=None):
'''
Add a vtkPolyData to the debug data. A color can be provided.
If the extraLabels argument is used, it should be a list of tuples,
each tuple is (labelName, labelValue) where labelName is a string and
labelValue is an int or float. An array with labelName will be filled
with labelValue and added to the poly data.
'''
polyData = shallowCopy(polyData)
if color is not None:
colorArray = np.empty((polyData.GetNumberOfPoints(), 3), dtype=np.uint8)
colorArray[:,:] = np.array(color)*255
vnp.addNumpyToVtk(polyData, colorArray, 'RGB255')
if extraLabels is not None:
for labelName, labelValue in extraLabels:
extraArray = np.empty((polyData.GetNumberOfPoints(), 1), dtype=type(labelValue))
extraArray[:] = labelValue
vnp.addNumpyToVtk(polyData, extraArray, labelName)
self.append.AddInputData(polyData)
def computeAndColorByDistance(ptsName,
meshName,
colorByRange=[0.0, 0.05],
arrayName='distance_to_mesh'):
pointCloud = om.findObjectByName(ptsName)
mesh = om.findObjectByName(meshName)
isPointCloud = mesh._isPointCloud()
assert pointCloud is not None
assert mesh is not None
dists = computePointToPointDistance(pointCloud.polyData, mesh.polyData)
vnp.addNumpyToVtk(pointCloud.polyData, dists, arrayName)
pointCloud._updateColorByProperty()
pointCloud.setProperty('Color By', arrayName)
pointCloud.colorBy(arrayName, colorByRange)
return pointCloud
def addPolyData(self, polyData, color=[1, 1, 1], extraLabels=None):
'''
Add a vtkPolyData to the debug data. A color can be provided.
If the extraLabels argument is used, it should be a list of tuples,
each tuple is (labelName, labelValue) where labelName is a string and
labelValue is an int or float. An array with labelName will be filled
with labelValue and added to the poly data.
'''
polyData = shallowCopy(polyData)
if color is not None:
colorArray = np.empty((polyData.GetNumberOfPoints(), 3),
dtype=np.uint8)
colorArray[:, :] = np.array(color) * 255
vnp.addNumpyToVtk(polyData, colorArray, 'RGB255')
if extraLabels is not None:
for labelName, labelValue in extraLabels:
extraArray = np.empty((polyData.GetNumberOfPoints(), 1),
dtype=type(labelValue))
extraArray[:] = labelValue
vnp.addNumpyToVtk(polyData, extraArray, labelName)
self.append.AddInputData(polyData)
f.SetMaxEstimationError(0.01)
f.SetMaxCenterError(0.02)
f.SetComputeCurvature(True)
f.SetRadius(0.1)
f.Update()
polyData = shallowCopy(f.GetOutput())
print 'done.'
# filter points without normals
normals = vnp.getNumpyFromVtk(polyData, 'normals')
segmentation.flipNormalsWithViewDirection(polyData, [1, -1, -1])
normalsValid = np.any(normals, axis=1)
vnp.addNumpyToVtk(polyData, np.array(normalsValid, dtype=np.int32), 'normals_valid')
vis.showPolyData(polyData, 'scene points', colorByName='normals_valid', visible=False)
numPoints = polyData.GetNumberOfPoints()
polyData = segmentation.thresholdPoints(polyData, 'normals_valid', [1, 1])
vis.showPolyData(polyData, 'cloud normals', colorByName='curvature', visible=True)
print 'number of filtered points:', numPoints - polyData.GetNumberOfPoints()
if showGlyphs:
polyData.GetPointData().SetNormals(polyData.GetPointData().GetArray('normals'))
arrows = segmentation.applyArrowGlyphs(polyData, computeNormals=False)
disks = segmentation.applyDiskGlyphs(polyData, computeNormals=False)
polyData.GetPointData().SetNormals(None)
def fit(self, polyData, points):
iiwaplanning.fitSupport(pickPoint=points[0])
return
pickPoint = points[0]
t = vtk.vtkTransform()
t.Translate(pickPoint)
print 'pick point:', pickPoint
print 'crop'
polyData = segmentation.cropToBox(polyData, t, [0.3,0.3,0.5])
import colorsys
rgb = vnp.getNumpyFromVtk(polyData, 'rgb_colors')/255.0
hsv = np.array([colorsys.rgb_to_hsv(*t) for t in rgb])
vnp.addNumpyToVtk(polyData, hsv[:,0].copy(), 'hue')
vnp.addNumpyToVtk(polyData, hsv[:,1].copy(), 'saturation')
vnp.addNumpyToVtk(polyData, hsv[:,2].copy(), 'value')
vis.updatePolyData(polyData, 'crop region', colorByName='rgb_colors', visible=False)
# hide input data
#om.findObjectByName(self.pointCloudObjectName).setProperty('Visible', False)
#cluster = segmentation.makePolyDataFields(polyData)
#vis.showClusterObjects([cluster], parent='segmentation')
hueRange = [0.12, 0.14]
valueRange = [0.5, 1.0]
print 'thresh'
points = segmentation.thresholdPoints(segmentation.thresholdPoints(polyData, 'hue', hueRange), 'value', valueRange)
#points = segmentation.extractLargestCluster(points, minClusterSize=10, clusterTolerance=0.02)
vis.updatePolyData(points, 'pole points', color=[0,0,1], visible=False)
maxZ = np.nanmax(vnp.getNumpyFromVtk(points, 'Points')[:,2])
print 'maxZ', maxZ
pickPoint = pickPoint[0], pickPoint[1], maxZ+0.2
print pickPoint
t = vtk.vtkTransform()
t.Translate(pickPoint)
print 'crop2'
polyData = segmentation.cropToBox(polyData, t, [0.15,0.15,0.4])
vis.updatePolyData(polyData, 'object points', colorByName='rgb_colors', visible=False)
if polyData.GetNumberOfPoints() > 5:
print 'make fields'
cluster = segmentation.makePolyDataFields(polyData)
vis.showClusterObjects([cluster], parent='segmentation')
print 'done'
import numpy as np
app = consoleapp.ConsoleApp()
view = app.createView()
cellSize = 0.5
numberOfCells = 20
grid = vtk.vtkGridSource()
grid.SetScale(cellSize)
grid.SetGridSize(numberOfCells)
grid.SetSurfaceEnabled(True)
grid.Update()
grid = grid.GetOutput()
pts = vnp.getNumpyFromVtk(grid, 'Points')
heatMap = np.random.randn(len(pts))
vnp.addNumpyToVtk(grid, heatMap, 'heat_map')
gridObj = vis.showPolyData(grid,
'heat map',
colorByName='heat_map',
parent='scene')
gridObj.setProperty('Surface Mode', 'Surface with edges')
applogic.resetCamera()
view.show()
app.start()
d = DebugData()
d.addCone(origin=(0,0,0), normal=(0,1,0), radius=0.3, height=0.8, color=[1, 1, 0])
show(d, (0, 4, 0))
d = DebugData()
d.addCube(dimensions=[0.8, 0.5, 0.3], center=[0, 0, 0], color=[0, 1, 1])
show(d, (2, 4, 0))
d = DebugData()
d.addPlane(origin=[0, 0, 0], normal=[0, 0, 1], width=0.8, height=0.7, resolution=10, color=[0, 1, 0])
show(d, (4, 4, 0)).setProperty('Surface Mode', 'Surface with edges')
d = DebugData()
d.addCapsule(center=[0, 0, 0], axis=[1, 0, 0], length=1.0, radius=0.1, color=[0.5, 0.5, 1])
show(d, (6, 4, 0))
d = DebugData()
polyData = vnp.numpyToPolyData(np.random.random((1000, 3)))
vnp.addNumpyToVtk(polyData, np.arange(polyData.GetNumberOfPoints()), 'point_ids')
d.addPolyData(polyData)
show(d, (2.5, 5, 0)).setProperty('Color By', 'point_ids')
applogic.resetCamera(viewDirection=[0, 0.1, -1])
app.start()
f.SetMaxIterations(100)
f.SetMaxEstimationError(0.01)
f.SetMaxCenterError(0.02)
f.SetComputeCurvature(True)
f.SetRadius(0.1)
f.Update()
polyData = shallowCopy(f.GetOutput())
print 'done.'
# filter points without normals
normals = vnp.getNumpyFromVtk(polyData, 'normals')
segmentation.flipNormalsWithViewDirection(polyData, [1, -1, -1])
normalsValid = np.any(normals, axis=1)
vnp.addNumpyToVtk(polyData, np.array(normalsValid, dtype=np.int32),
'normals_valid')
vis.showPolyData(polyData,
'scene points',
colorByName='normals_valid',
visible=False)
numPoints = polyData.GetNumberOfPoints()
polyData = segmentation.thresholdPoints(polyData, 'normals_valid', [1, 1])
vis.showPolyData(polyData,
'cloud normals',
colorByName='curvature',
visible=True)
print 'number of filtered points:', numPoints - polyData.GetNumberOfPoints(
def evalXYpoly(x,y):
return x**2 + 3*y**2 + x - y
# # # This distorts the map in z
# for row in pts:
# print "row before, ", row
# row[2] = evalXYpoly(row[0],row[1])
# print "row after, ", row
polyHeatMap = np.zeros((len(pts)))
print np.shape(polyHeatMap)
for i in range(len(polyHeatMap)):
polyHeatMap[i] = evalXYpoly(pts[i][0],pts[i][1])
vnp.addNumpyToVtk(grid, polyHeatMap, 'heat_map')
# ## This adds random heat map
# randomHeatMap = np.random.randn(len(pts))
# print "heatMap ", np.shape(randomHeatMap)
# vnp.addNumpyToVtk(grid, randomHeatMap, 'heat_map')
def clipByPlane(polyData, planeOrigin, planeNormal):
f = vtk.vtkClipPolyData()
f.SetInput(polyData)
p = vtk.vtkPlane()
p.SetOrigin(planeOrigin)
p.SetNormal(planeNormal)
f.SetClipFunction(p)
