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