private synchronized void onNewPointcloud(RosPointCloudSubscriber.UnpackedPointCloud unpackedPointCloud) { OrganizedPointCloud organizedPointCloud = new OrganizedPointCloud(unpackedPointCloud.getWidth(), unpackedPointCloud.getHeight(), unpackedPointCloud.getXYZRGB()); ArrayList<LineModDetection> detections = new ArrayList<>(); LineModDetection bestDetection = detectObjectAndEstimatePose(organizedPointCloud, detections,false,false); System.out.println(bestDetection); if (bestDetection != null) { BufferedImage image = organizedPointCloud.getRGBImage(); Collections.sort(detections); for(int i=0;i<Math.min(detections.size(),3);i++) { drawDetectionOnImage(detections.get(i), image); } int heightOffset = frame.getHeight() - frame.getContentPane().getHeight(); int widthOffset = frame.getWidth() - frame.getContentPane().getWidth(); frame.setSize(image.getWidth() + widthOffset, image.getHeight() + heightOffset); imagePanel.setBufferedImageSafe(image); } }
private synchronized void onNewPointcloud(RosPointCloudSubscriber.UnpackedPointCloud unpackedPointCloud) { OrganizedPointCloud organizedPointCloud = new OrganizedPointCloud(unpackedPointCloud.getWidth(), unpackedPointCloud.getHeight(), unpackedPointCloud.getXYZRGB()); ArrayList<LineModDetection> detections = new ArrayList<>(); LineModDetection bestDetection = detectObjectAndEstimatePose(organizedPointCloud, detections,false,false); System.out.println(bestDetection); if (bestDetection != null) { BufferedImage image = organizedPointCloud.getRGBImage(); Collections.sort(detections); for(int i=0;i<Math.min(detections.size(),3);i++) { drawDetectionOnImage(detections.get(i), image); } int heightOffset = frame.getHeight() - frame.getContentPane().getHeight(); int widthOffset = frame.getWidth() - frame.getContentPane().getWidth(); frame.setSize(image.getWidth() + widthOffset, image.getHeight() + heightOffset); imagePanel.setBufferedImageSafe(image); } }