def get2DAsPolyData(xy_points):
'''
Convert a 2D np array to a 3D polydata by appending z=0
'''
d = np.vstack((xy_points.T, np.zeros( xy_points.shape[0]) )).T
d2=d.copy()
return vtkNumpy.getVtkPolyDataFromNumpyPoints( d2 )
def get2DAsPolyData(xy_points):
'''
Convert a 2D np array to a 3D polydata by appending z=0
'''
d = np.vstack((xy_points.T, np.zeros( xy_points.shape[0]) )).T
d2=d.copy()
return vtkNumpy.getVtkPolyDataFromNumpyPoints( d2 )
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
