def extractClusters(polyData, clusterInXY=False, **kwargs):
''' Segment a single point cloud into smaller clusters
using Euclidean Clustering
'''
if not polyData.GetNumberOfPoints():
return []
if (clusterInXY == True):
''' If Points are seperated in X&Y, then cluster outside this '''
polyDataXY = vtk.vtkPolyData()
polyDataXY.DeepCopy(polyData)
points=vtkNumpy.getNumpyFromVtk(polyDataXY , 'Points') # shared memory
points[:,2] = 0.0
#showPolyData(polyDataXY, 'polyDataXY', visible=False, parent=getDebugFolder())
polyDataXY = applyEuclideanClustering(polyDataXY, **kwargs)
clusterLabels = vtkNumpy.getNumpyFromVtk(polyDataXY, 'cluster_labels')
vtkNumpy.addNumpyToVtk(polyData, clusterLabels, 'cluster_labels')
else:
polyData = applyEuclideanClustering(polyData, **kwargs)
clusterLabels = vtkNumpy.getNumpyFromVtk(polyData, 'cluster_labels')
clusters = []
for i in xrange(1, clusterLabels.max() + 1):
cluster = thresholdPoints(polyData, 'cluster_labels', [i, i])
clusters.append(cluster)
return clusters
def findFarRightCorner(self, polyData, linkFrame):
'''
Within a point cloud find the point to the far right from the link
The input is typically the 4 corners of a minimum bounding box
'''
diagonalTransform = transformUtils.frameFromPositionAndRPY([0,0,0], [0,0,45])
diagonalTransform.Concatenate(linkFrame)
vis.updateFrame(diagonalTransform, 'diagonal frame', parent='cont debug', visible=False)
#polyData = shallowCopy(polyData)
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
#vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
#vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
#vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')
viewOrigin = diagonalTransform.TransformPoint([0.0, 0.0, 0.0])
viewX = diagonalTransform.TransformVector([1.0, 0.0, 0.0])
viewY = diagonalTransform.TransformVector([0.0, 1.0, 0.0])
viewZ = diagonalTransform.TransformVector([0.0, 0.0, 1.0])
#polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
#polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')
vis.updatePolyData( polyData, 'cornerPoints', parent='cont debug', visible=False)
farRightIndex = vtkNumpy.getNumpyFromVtk(polyData, 'distance_along_foot_y').argmin()
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
return points[farRightIndex,:]
def findFarRightCorner(self, polyData, linkFrame):
'''
Within a point cloud find the point to the far right from the link
The input is typically the 4 corners of a minimum bounding box
'''
diagonalTransform = transformUtils.frameFromPositionAndRPY([0,0,0], [0,0,45])
diagonalTransform.Concatenate(linkFrame)
vis.updateFrame(diagonalTransform, 'diagonal frame', parent='cont debug', visible=False)
#polyData = shallowCopy(polyData)
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
#vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
#vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
#vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')
viewOrigin = diagonalTransform.TransformPoint([0.0, 0.0, 0.0])
viewX = diagonalTransform.TransformVector([1.0, 0.0, 0.0])
viewY = diagonalTransform.TransformVector([0.0, 1.0, 0.0])
viewZ = diagonalTransform.TransformVector([0.0, 0.0, 1.0])
#polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
#polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')
vis.updatePolyData( polyData, 'cornerPoints', parent='cont debug', visible=False)
farRightIndex = vtkNumpy.getNumpyFromVtk(polyData, 'distance_along_foot_y').argmin()
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
return points[farRightIndex,:]
def fitDrillBarrel ( drillPoints, forwardDirection, plane_origin, plane_normal):
''' Given a point cloud which ONLY contains points from a barrell drill, standing upright
and the equations of a table its resting on, and the general direction of the robot
Fit a barrell drill
'''
if not drillPoints.GetNumberOfPoints():
return
vis.updatePolyData(drillPoints, 'drill cluster', parent=getDebugFolder(), visible=False)
drillBarrelPoints = thresholdPoints(drillPoints, 'dist_to_plane', [0.177, 0.30])
if not drillBarrelPoints.GetNumberOfPoints():
return
# fit line to drill barrel points
linePoint, lineDirection, _ = applyLineFit(drillBarrelPoints, distanceThreshold=0.5)
if np.dot(lineDirection, forwardDirection) < 0:
lineDirection = -lineDirection
vis.updatePolyData(drillBarrelPoints, 'drill barrel points', parent=getDebugFolder(), visible=False)
pts = vtkNumpy.getNumpyFromVtk(drillBarrelPoints, 'Points')
dists = np.dot(pts-linePoint, lineDirection)
p1 = linePoint + lineDirection*np.min(dists)
p2 = linePoint + lineDirection*np.max(dists)
p1 = projectPointToPlane(p1, plane_origin, plane_normal)
p2 = projectPointToPlane(p2, plane_origin, plane_normal)
d = DebugData()
d.addSphere(p1, radius=0.01)
d.addSphere(p2, radius=0.01)
d.addLine(p1, p2)
vis.updatePolyData(d.getPolyData(), 'drill debug points', color=[0,1,0], parent=getDebugFolder(), visible=False)
drillToBasePoint = np.array([-0.07, 0.0 , -0.12])
zaxis = plane_normal
xaxis = lineDirection
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
t = getTransformFromAxes(xaxis, yaxis, zaxis)
t.PreMultiply()
t.Translate(-drillToBasePoint)
t.PostMultiply()
t.Translate(p1)
return t
def labelPointDistanceAlongAxis(polyData, axis, origin=None, resultArrayName='distance_along_axis'):
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
if origin is not None:
points = points - origin
distanceValues = np.dot(points, axis)
if origin is None:
distanceValues -= np.nanmin(distanceValues)
newData = shallowCopy(polyData)
vtkNumpy.addNumpyToVtk(newData, distanceValues, resultArrayName)
return newData
def getRecedingTerrainRegion(self, polyData, linkFrame):
''' Find the point cloud in front of the foot frame'''
#polyData = shallowCopy(polyData)
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
#vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
#vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
#vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')
viewOrigin = linkFrame.TransformPoint([0.0, 0.0, 0.0])
viewX = linkFrame.TransformVector([1.0, 0.0, 0.0])
viewY = linkFrame.TransformVector([0.0, 1.0, 0.0])
viewZ = linkFrame.TransformVector([0.0, 0.0, 1.0])
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_x', [0.12, 1.6])
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_y', [-0.4, 0.4])
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_z', [-0.4, 0.4])
vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='cont debug', visible=True)
return polyData
def getRecedingTerrainRegion(self, polyData, linkFrame):
''' Find the point cloud in front of the foot frame'''
#polyData = shallowCopy(polyData)
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
#vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
#vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
#vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')
viewOrigin = linkFrame.TransformPoint([0.0, 0.0, 0.0])
viewX = linkFrame.TransformVector([1.0, 0.0, 0.0])
viewY = linkFrame.TransformVector([0.0, 1.0, 0.0])
viewZ = linkFrame.TransformVector([0.0, 0.0, 1.0])
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_x', [0.12, 1.6])
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_y', [-0.4, 0.4])
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_z', [-0.4, 0.4])
vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='cont debug', visible=True)
return polyData
def findMinimumBoundingRectangle(self, polyData, linkFrame):
'''
Find minimum bounding rectangle.
The input is assumed to be a rectangular point cloud of cinder blocks
Returns transform of far right corner (pointing away from robot)
'''
# TODO: for non-z up surfaces, this needs work
# TODO: return other parameters
# Originally From: https://github.com/dbworth/minimum-area-bounding-rectangle
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.02)
#vis.updatePolyData( polyData, 'block top', parent='cont debug', visible=False)
polyDataCentroid = segmentation.computeCentroid(polyData)
pts =vtkNumpy.getNumpyFromVtk( polyData , 'Points' )
xy_points = pts[:,[0,1]]
vis.updatePolyData( get2DAsPolyData(xy_points) , 'xy_points', parent='cont debug', visible=False)
hull_points = qhull_2d.qhull2D(xy_points)
vis.updatePolyData( get2DAsPolyData(hull_points) , 'hull_points', parent='cont debug', visible=False)
# Reverse order of points, to match output from other qhull implementations
hull_points = hull_points[::-1]
# print 'Convex hull points: \n', hull_points, "\n"
# Find minimum area bounding rectangle
(rot_angle, rectArea, rectDepth, rectWidth, center_point, corner_points_ground) = min_bounding_rect.minBoundingRect(hull_points)
vis.updatePolyData( get2DAsPolyData(corner_points_ground) , 'corner_points_ground', parent='cont debug', visible=False)
cornerPoints = np.vstack((corner_points_ground.T, polyDataCentroid[2]*np.ones( corner_points_ground.shape[0]) )).T
cornerPolyData = vtkNumpy.getVtkPolyDataFromNumpyPoints(cornerPoints)
# Create a frame at the far right point - which points away from the robot
farRightCorner = self.findFarRightCorner(cornerPolyData , linkFrame)
viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
robotYaw = math.atan2( viewDirection[1], viewDirection[0] )*180.0/np.pi
blockAngle = rot_angle*(180/math.pi)
#print "robotYaw ", robotYaw
#print "blockAngle ", blockAngle
blockAngleAll = np.array([blockAngle , blockAngle+90 , blockAngle+180, blockAngle+270])
#print blockAngleAll
for i in range(0,4):
if(blockAngleAll[i]>180):
blockAngleAll[i]=blockAngleAll[i]-360
#print blockAngleAll
values = abs(blockAngleAll - robotYaw)
#print values
min_idx = np.argmin(values)
if ( (min_idx==1) or (min_idx==3) ):
#print "flip rectDepth and rectWidth as angle is not away from robot"
temp = rectWidth ; rectWidth = rectDepth ; rectDepth = temp
#print "best angle", blockAngleAll[min_idx]
rot_angle = blockAngleAll[min_idx]*math.pi/180.0
# Optional: overwrite all of the above with the yaw of the robt when it was standing in front of the blocks:
if (self.fixBlockYawWithInitial):
rot_angle = self.initialRobotYaw
cornerTransform = transformUtils.frameFromPositionAndRPY( farRightCorner , [0,0, np.rad2deg(rot_angle) ] )
#print "Minimum area bounding box:"
#print "Rotation angle:", rot_angle, "rad (", rot_angle*(180/math.pi), "deg )"
#print "rectDepth:", rectDepth, " rectWidth:", rectWidth, " Area:", rectArea
#print "Center point: \n", center_point # numpy array
#print "Corner points: \n", cornerPoints, "\n" # numpy array
return cornerTransform, rectDepth, rectWidth, rectArea
def findMinimumBoundingRectangle(self, polyData, linkFrame):
'''
Find minimum bounding rectangle.
The input is assumed to be a rectangular point cloud of cinder blocks
Returns transform of far right corner (pointing away from robot)
'''
# TODO: for non-z up surfaces, this needs work
# TODO: return other parameters
# Originally From: https://github.com/dbworth/minimum-area-bounding-rectangle
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.02)
#vis.updatePolyData( polyData, 'block top', parent='cont debug', visible=False)
polyDataCentroid = segmentation.computeCentroid(polyData)
pts =vtkNumpy.getNumpyFromVtk( polyData , 'Points' )
xy_points = pts[:,[0,1]]
vis.updatePolyData( get2DAsPolyData(xy_points) , 'xy_points', parent='cont debug', visible=False)
hull_points = qhull_2d.qhull2D(xy_points)
vis.updatePolyData( get2DAsPolyData(hull_points) , 'hull_points', parent='cont debug', visible=False)
# Reverse order of points, to match output from other qhull implementations
hull_points = hull_points[::-1]
# print 'Convex hull points: \n', hull_points, "\n"
# Find minimum area bounding rectangle
(rot_angle, rectArea, rectDepth, rectWidth, center_point, corner_points_ground) = min_bounding_rect.minBoundingRect(hull_points)
vis.updatePolyData( get2DAsPolyData(corner_points_ground) , 'corner_points_ground', parent='cont debug', visible=False)
cornerPoints = np.vstack((corner_points_ground.T, polyDataCentroid[2]*np.ones( corner_points_ground.shape[0]) )).T
cornerPolyData = vtkNumpy.getVtkPolyDataFromNumpyPoints(cornerPoints)
# Create a frame at the far right point - which points away from the robot
farRightCorner = self.findFarRightCorner(cornerPolyData , linkFrame)
viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
robotYaw = math.atan2( viewDirection[1], viewDirection[0] )*180.0/np.pi
blockAngle = rot_angle*(180/math.pi)
#print "robotYaw ", robotYaw
#print "blockAngle ", blockAngle
blockAngleAll = np.array([blockAngle , blockAngle+90 , blockAngle+180, blockAngle+270])
#print blockAngleAll
for i in range(0,4):
if(blockAngleAll[i]>180):
blockAngleAll[i]=blockAngleAll[i]-360
#print blockAngleAll
values = abs(blockAngleAll - robotYaw)
#print values
min_idx = np.argmin(values)
if ( (min_idx==1) or (min_idx==3) ):
#print "flip rectDepth and rectWidth as angle is not away from robot"
temp = rectWidth ; rectWidth = rectDepth ; rectDepth = temp
#print "best angle", blockAngleAll[min_idx]
rot_angle = blockAngleAll[min_idx]*math.pi/180.0
# Optional: overwrite all of the above with the yaw of the robt when it was standing in front of the blocks:
if (self.fixBlockYawWithInitial):
rot_angle = self.initialRobotYaw
cornerTransform = transformUtils.frameFromPositionAndRPY( farRightCorner , [0,0, np.rad2deg(rot_angle) ] )
#print "Minimum area bounding box:"
#print "Rotation angle:", rot_angle, "rad (", rot_angle*(180/math.pi), "deg )"
#print "rectDepth:", rectDepth, " rectWidth:", rectWidth, " Area:", rectArea
#print "Center point: \n", center_point # numpy array
#print "Corner points: \n", cornerPoints, "\n" # numpy array
return cornerTransform, rectDepth, rectWidth, rectArea
