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])