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); } }
@Override public void onNewMessage(PointCloud2 pointCloud) { UnpackedPointCloud cloud = unpackPointsAndIntensities(pointCloud); if (cloud.getWidth() > 1200) { int ptr = 0; Point3d[] points = new Point3d[(cloud.getWidth() / 2) * (cloud.getHeight() / 2)]; Color[] pointColors = new Color[(cloud.getWidth() / 2) * (cloud.getHeight() / 2)]; for (int j = 0; j < cloud.getHeight(); j += 2) for (int i = 0; i < cloud.getWidth(); i += 2) { points[ptr] = cloud.getPoints()[j * cloud.getWidth() + i]; pointColors[ptr] = cloud.getPointColors()[j * cloud.getWidth() + i]; ptr++; } UnpackedPointCloud scaledCloud = new UnpackedPointCloud(cloud.getWidth() / 2, cloud.getHeight() / 2, cloud.getPointType(), points, pointColors); onNewPointcloud(scaledCloud); return; } else { onNewPointcloud(cloud); return; } } };
@Override public void onNewMessage(PointCloud2 pointCloud) { UnpackedPointCloud cloud = unpackPointsAndIntensities(pointCloud); if (cloud.getWidth() > 1200) { int ptr = 0; Point3D[] points = new Point3D[(cloud.getWidth() / 2) * (cloud.getHeight() / 2)]; Color[] pointColors = new Color[(cloud.getWidth() / 2) * (cloud.getHeight() / 2)]; for (int j = 0; j < cloud.getHeight(); j += 2) for (int i = 0; i < cloud.getWidth(); i += 2) { points[ptr] = cloud.getPoints()[j * cloud.getWidth() + i]; pointColors[ptr] = cloud.getPointColors()[j * cloud.getWidth() + i]; ptr++; } UnpackedPointCloud scaledCloud = new UnpackedPointCloud(cloud.getWidth() / 2, cloud.getHeight() / 2, cloud.getPointType(), points, pointColors); onNewPointcloud(scaledCloud); return; } else { onNewPointcloud(cloud); return; } } };