@Override public void receivedPacket(PointCloudWorldPacket pointCloud) { numberOfLidarScansConsumed++; // jmeLidarScanVisualizer.addPointCloud(Arrays.asList(pointCloud.getDecayingWorldScan())); try { List<Point3D32> lidarWorldPoints = Arrays.asList(HumanoidMessageTools.getDecayingWorldScan(pointCloud)); numberOfLidarPointsConsumed += lidarWorldPoints.size(); for (Point3D32 lidarWorldPoint : lidarWorldPoints) { if (lidarWorldPoint.getX() > 0.5) { assertEquals(WALL_DISTANCE, lidarWorldPoint.getX(), SCAN_TOLERANCE); } } } catch (AssertionError assertionError) { errorQueue.add(assertionError); } } }
Assert.assertTrue(0 == point.getX()); Assert.assertTrue(0 == point.getY()); Assert.assertTrue(0 == point.getZ());
private void writeRegionData(Path folderPath, PlanarRegionSegmentationMessage[] segmentationData) throws IOException { for (PlanarRegionSegmentationMessage message : segmentationData) { File regionFile = new File(folderPath.toFile(), "region" + message.getRegionId()); FileWriter fileWriter = new FileWriter(regionFile); for (Point3D32 hitLocation : message.getHitLocations()) { fileWriter.write(hitLocation.getX() + ", " + hitLocation.getY() + ", " + hitLocation.getZ() + "\n"); } fileWriter.close(); } }
private void writeHeaderFile(File header, PlanarRegionSegmentationMessage[] segmentationData) throws IOException { FileWriter fileWriter = new FileWriter(header); for (PlanarRegionSegmentationMessage message : segmentationData) { Point3D32 origin = message.getOrigin(); Vector3D32 normal = message.getNormal(); fileWriter.write("regionId: "); fileWriter.write(Integer.toString(message.getRegionId())); fileWriter.write(", origin: "); fileWriter.write(origin.getX() + ", " + origin.getY() + ", " + origin.getZ()); fileWriter.write(", normal: "); fileWriter.write(normal.getX() + ", " + normal.getY() + ", " + normal.getZ()); fileWriter.write("\n"); } fileWriter.close(); }
public static Point2D toPointInPlane(Point3D32 pointToTransform, Point3D planeOrigin, Quaternion planeOrientation) { return toPointInPlane(pointToTransform.getX(), pointToTransform.getY(), pointToTransform.getZ(), planeOrigin, planeOrientation); }
worldToCameraTransform.transform(candidateLidarPoint); if (candidateLidarPoint.getX() > FILTERING_MIN_DISTANCE && candidateLidarPoint.getX() < FILTERING_MAX_DISTANCE) double rayAngleX = Math.atan2(candidateLidarPoint.getZ(), candidateLidarPoint.getX()); double rayAngleY = Math.atan2(candidateLidarPoint.getY(), candidateLidarPoint.getX()); if (Math.abs(rayAngleX - desiredRayAngleX) < FILTERING_ANGLE && Math.abs(rayAngleY - desiredRayAngleY) < FILTERING_ANGLE)
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testNextPoint3D32Array() { Random random = new Random(); Point3D32 min = new Point3D32(-7.0f, -5.0f, -6.0f); Point3D32 max = new Point3D32(5.0f, 4.0f, 8.0f); Point3D32[] randomPoint3fCloud = RandomGeometry.nextPoint3D32Array(random, 1000, min, max); for (int i = 0; i < 1000; i++) { assertTrue((randomPoint3fCloud[i].getX() >= -7.0f) && (randomPoint3fCloud[i].getX() <= 5.0)); assertTrue((randomPoint3fCloud[i].getY() >= -5.0f) && (randomPoint3fCloud[i].getY() <= 4.0)); assertTrue((randomPoint3fCloud[i].getZ() >= -6.0f) && (randomPoint3fCloud[i].getZ() <= 8.0)); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testNextPoint3D32() { Random random = new Random(); Point3D32 min = new Point3D32(-7.0f, -5.0f, -6.0f); Point3D32 max = new Point3D32(5.0f, 4.0f, 8.0f); Point3D32 randomPoint3f; for (int i = 0; i < 1000; i++) { randomPoint3f = RandomGeometry.nextPoint3D32(random, min, max); assertTrue((randomPoint3f.getX() >= -7.0f) && (randomPoint3f.getX() <= 5.0)); assertTrue((randomPoint3f.getY() >= -5.0f) && (randomPoint3f.getY() <= 4.0)); assertTrue((randomPoint3f.getZ() >= -6.0f) && (randomPoint3f.getZ() <= 8.0)); } }
assertEquals(list.get(j).getX(), lidarScan1.getAllPoints3f().get(j).getX(), 1e-3); assertEquals(list.get(j).getY(), lidarScan1.getAllPoints3f().get(j).getY(), 1e-3); assertEquals(list.get(j).getZ(), lidarScan1.getAllPoints3f().get(j).getZ(), 1e-3);
public void rotate(Matrix3DReadOnly rotationMatrix) { for (Point3D32 vertex : vertices) { double x = rotationMatrix.getM00() * vertex.getX() + rotationMatrix.getM01() * vertex.getY(); double y = rotationMatrix.getM10() * vertex.getX() + rotationMatrix.getM11() * vertex.getY(); double z = rotationMatrix.getM20() * vertex.getX() + rotationMatrix.getM21() * vertex.getY(); vertex.set(x, y, z); } for (Vector3D32 normal : normals) { double x = rotationMatrix.getM00() * normal.getX() + rotationMatrix.getM01() * normal.getY(); double y = rotationMatrix.getM10() * normal.getX() + rotationMatrix.getM11() * normal.getY(); double z = rotationMatrix.getM20() * normal.getX() + rotationMatrix.getM21() * normal.getY(); normal.set(x, y, z); } }
for (Point3D32 tmpPoint : fullPoints) pointsNearBy.add(new Point3D_F64(tmpPoint.getX(), tmpPoint.getY(), tmpPoint.getZ()));