diff --git a/fed/mosaic-sumo/src/test/java/org/eclipse/mosaic/fed/sumo/util/TrafficSignManagerTest.java b/fed/mosaic-sumo/src/test/java/org/eclipse/mosaic/fed/sumo/util/TrafficSignManagerTest.java index bddca27ac..7f9df7a3b 100644 --- a/fed/mosaic-sumo/src/test/java/org/eclipse/mosaic/fed/sumo/util/TrafficSignManagerTest.java +++ b/fed/mosaic-sumo/src/test/java/org/eclipse/mosaic/fed/sumo/util/TrafficSignManagerTest.java @@ -16,6 +16,7 @@ package org.eclipse.mosaic.fed.sumo.util; import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertTrue; import static org.mockito.ArgumentMatchers.eq; import static org.mockito.ArgumentMatchers.isA; import static org.mockito.Mockito.verify; @@ -155,13 +156,13 @@ public void addAndGenerateLaneAssignment() throws InternalFederateException, IOE ); long fileSize0 = Files.size(temporaryFolder.getRoot().toPath().resolve(base.resolve("AV-lane0.png"))); - assertEquals(15d, fileSize0 / 1000d, 7d); + assertTrue(fileSize0 > 0); long fileSize1 = Files.size(temporaryFolder.getRoot().toPath().resolve(base.resolve("ALL-lane1.png"))); - assertEquals(15d, fileSize1 / 1000d, 7d); + assertTrue(fileSize1 > 0); long fileSize2 = Files.size(temporaryFolder.getRoot().toPath().resolve(base.resolve("EMPTY-lane2.png"))); - assertEquals(70d, fileSize2 / 1000d, 20d); + assertTrue(fileSize2 > 0); } } \ No newline at end of file diff --git a/lib/mosaic-geomath/src/main/java/org/eclipse/mosaic/lib/spatial/PointCloud.java b/lib/mosaic-geomath/src/main/java/org/eclipse/mosaic/lib/spatial/PointCloud.java new file mode 100644 index 000000000..57bcb37a3 --- /dev/null +++ b/lib/mosaic-geomath/src/main/java/org/eclipse/mosaic/lib/spatial/PointCloud.java @@ -0,0 +1,301 @@ +/* + * Copyright (c) 2024 Fraunhofer FOKUS and others. All rights reserved. + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0 + * + * SPDX-License-Identifier: EPL-2.0 + * + * Contact: mosaic@fokus.fraunhofer.de + */ + +package org.eclipse.mosaic.lib.spatial; + +import org.eclipse.mosaic.lib.math.Matrix3d; +import org.eclipse.mosaic.lib.math.Vector3d; + +import java.io.Serializable; +import java.util.List; +import java.util.function.Predicate; +import java.util.stream.Collectors; + +/** + * This class represents a point cloud based on the {@link Vector3d} coordinate system. + * + * When calling the methods {@link #getRelativeEndPoints()} or {@link #getAbsoluteEndPoints()}, a transformation of the stored points + * may be executed (and cached for re-use). E.g., if the point cloud is constructed using absolute coordinates + * (i.e. {@link PointReference#ABSOLUTE}), a call of {@link #getAbsoluteEndPoints()} will return the stored point list as-is, whereas + * a call of {@link #getRelativeEndPoints()} will transform the stored points to relative coordinates (relatively to {@link #origin} and + * {@link #orientation}). + */ +public final class PointCloud implements Serializable { + + private static final Predicate POINTS_ALL = p -> true; + private static final Predicate POINTS_WITH_HITS = Point::hasHit; + + public enum PointReference { + ABSOLUTE(Transformation::absoluteToRelative, Transformation::noTransformation), + RELATIVE(Transformation::noTransformation, Transformation::relativeToAbsolute); + + PointReference(Transformation toRelative, Transformation toAbsolute) { + this.toRelative = toRelative; + this.toAbsolute = toAbsolute; + } + + private final Transformation toRelative; + private final Transformation toAbsolute; + } + + private static final long serialVersionUID = 1L; + + private final Vector3d origin; + private final RotationMatrix orientation; + private final long creationTime; + + private final PointReference pointsReference; + private final List points; + + private transient List absoluteEndPoints = null; + private transient List absoluteEndPointsWithHit = null; + private transient List relativeEndPoints = null; + private transient List relativeEndPointsWithHit = null; + + /** + * Creates a new PointCloud based on a list of {@link Point}s. The points may be given (and then stored) either in absolute + * world coordinates, or relative coordinates compared to the given origin and orientation field. The reference frame of the points + * in the given {@link Point} list must be declared by using {@link PointReference#ABSOLUTE} or {@link PointReference#RELATIVE}. + * + * @param creationTime the creation time of the point cloud in nanoseconds + * @param origin the origin point of the point cloud in world coordinates + * @param orientation the orientation of the point cloud + * @param points a list of points, containing point coordinates in either absolute or relative format + * @param pointsReference the reference format of the coordinates of the points (absolute world coordinates, relative coordinates) + */ + public PointCloud(long creationTime, Vector3d origin, RotationMatrix orientation, List points, PointReference pointsReference) { + this.creationTime = creationTime; + this.origin = origin; + this.orientation = orientation; + this.points = points; + this.pointsReference = pointsReference; + } + + /** + * Returns the simulation time when the {@link PointCloud} was created. + * + * @return time in nanoseconds + */ + public long getCreationTime() { + return creationTime; + } + + /** + * Returns the origin point of this point cloud in absolute world coordinates. + * Points returned by {@link #getRelativeEndPoints} are relative to this origin. + * + * @return origin of the rays forming this point cloud in absolute coordinates + * @see #getOrientation() + */ + public Vector3d getOrigin() { + return origin; + } + + /** + * Returns the orientation of the point cloud as {@link RotationMatrix}. Points returned by {@link #getRelativeEndPoints} are relative + * to {@link #getOrigin()} and this orientation matrix. Points returned by {@link #getAbsoluteEndPoints()} are translated and rotated + * using {@link #getOrigin()} and this orientation matrix + *

+ * Let o be the ray origin in absolute coordinates, let R be the rotation matrix. Thus, the transformation + * of relative coordinates r into absolute coordinates a is + * a = R * r + o + * + * @return rotation matrix R + * @see #getOrigin() returns o + */ + public RotationMatrix getOrientation() { + return orientation; + } + + /** + * Returns all end points of all rays of this {@link PointCloud} that hit something in relative coordinates. + * Relative coordinates mean the use of a cartesian coordinate system with its origin at + * the {@link PointCloud}'s origin, returned by {@link #getOrigin()}. In addition, the + * coordinate system is rotated. See {@link #getOrientation()} for details. + * + * @return end points of all rays of this {@link PointCloud} in relative coordinates + * @see #getOrigin() origin of ray / translation of coordinates + * @see #getOrientation() rotation of coordinates + */ + public List getRelativeEndPoints() { + if (relativeEndPoints == null) { + relativeEndPoints = pointsReference.toRelative.transform(this, POINTS_ALL); + } + return relativeEndPoints; + } + + + /** + * Returns all end points of all rays of this {@link PointCloud} that hit something in relative coordinates. + * Relative coordinates mean the use of a cartesian coordinate system with its origin (0,0,0) at + * the {@link PointCloud}'s origin, returned by {@link #getOrigin()}. In addition, the + * coordinate system is rotated. See {@link #getOrientation()} for details. + * + * @return end points of all rays of this {@link PointCloud} in relative coordinates + * @see #getOrigin() origin of ray / translation of coordinates + * @see #getOrientation() rotation of coordinates + */ + public List getRelativeEndPointsWithHit() { + if (relativeEndPointsWithHit == null) { + relativeEndPointsWithHit = pointsReference.toRelative.transform(this, POINTS_WITH_HITS); + } + return relativeEndPointsWithHit; + } + + /** + * Returns all end points of all rays of this {@link PointCloud} in absolute coordinates. + * Absolute coordinates represent world coordinates, and have {@link #getOrigin()} and {@link #getOrientation()} + * already applied. + * + * @return a list of points in absolute coordinates + */ + public List getAbsoluteEndPoints() { + if (absoluteEndPoints == null) { + absoluteEndPoints = pointsReference.toAbsolute.transform(this, POINTS_ALL); + } + return absoluteEndPoints; + } + + /** + * Returns all end points of all rays of this {@link PointCloud} that hit something in absolute coordinates. + * Absolute coordinates represent world coordinates, and have {@link #getOrigin()} and {@link #getOrientation()} + * already applied. + * + * @return a list of points in absolute coordinates + */ + public List getAbsoluteEndPointsWithHit() { + if (absoluteEndPointsWithHit == null) { + absoluteEndPointsWithHit = pointsReference.toAbsolute.transform(this, POINTS_WITH_HITS); + } + return absoluteEndPointsWithHit; + } + + + /** + * Returns the type of reference frame the points are stored in this point cloud internally. The usage of + * {@link #getAbsoluteEndPoints()} and {@link #getRelativeEndPoints()} is independent of the value returned + * by this method, as transformation is already done internally if necessary. Therefore, it should + * usually not be required to use this method, except for (de)serialization use-cases. + * + * @return the {@link PointReference} type of the points stored in the point cloud. + */ + public PointReference getReferenceFormat() { + return pointsReference; + } + + /** + * A {@link Point} of the point cloud consists of its coordinates, an identifier + * of the type of object the point has hit, and the distance to the point cloud origin. + */ + public static class Point extends Vector3d { + + private static final long serialVersionUID = 1L; + + private final byte hitType; + private final float distance; + + /** + * Creates a new point to add to a {@link PointCloud} by specifying the distance + * to origin and the type of hit object (encoded in byte) next to the location of the point. + * + * @param endPoint the coordinates of the point cloud + * @param distance the distance to the origin of the point cloud + * @param hitType the type of hit object represented by this point. 0 = no hit + */ + public Point(Vector3d endPoint, float distance, byte hitType) { + x = endPoint.x; + y = endPoint.y; + z = endPoint.z; + this.distance = distance; + this.hitType = hitType; + } + + /** + * Returns true if the ray generating this {@link Point} has hit an object. + */ + public boolean hasHit() { + return hitType != 0; + } + + /** + * Returns the type of the object the ray generating this point has hit. (0 = no hit) + */ + public byte getHitType() { + return hitType; + } + + /** + * Returns the distance to the origin of point cloud this point belongs to. + */ + public float getDistance() { + return distance; + } + + @Override + public boolean equals(Object o) { + if (this == o) { + return true; + } + if (o == null || getClass() != o.getClass()) { + return false; + } + Point other = (Point) o; + return super.equals(other) + && this.hitType == other.hitType + && Float.compare(this.distance, other.distance) == 0; + } + + @Override + public int hashCode() { + int result = super.hashCode(); + result = 31 * result + Byte.hashCode(hitType); + result = 31 * result + Float.hashCode(distance); + return result; + } + } + + private interface Transformation { + + List transform(PointCloud pointCloud, Predicate filter); + + private static List relativeToAbsolute(PointCloud pointCloud, Predicate filter) { + return pointCloud.points.stream() + .filter(filter) + .map(point -> (Point) pointCloud.orientation.multiply(new Point(point, point.getDistance(), point.getHitType())).add(pointCloud.origin)) + .collect(Collectors.toList()); + } + + private static List absoluteToRelative(PointCloud pointCloud, Predicate filter) { + Matrix3d inv = pointCloud.orientation.transpose(new Matrix3d()); + return pointCloud.points.stream() + .filter(filter) + .map(point -> (Point) inv.multiply(point.subtract(pointCloud.origin, new Point(point, point.getDistance(), point.getHitType())))) + .collect(Collectors.toList()); + } + + private static List noTransformation(PointCloud pointCloud, Predicate filter) { + return filter == POINTS_ALL ? pointCloud.points : pointCloud.points.stream() + .filter(filter) + .collect(Collectors.toList()); + } + } +} diff --git a/lib/mosaic-geomath/src/test/java/org/eclipse/mosaic/lib/spatial/PointCloudTest.java b/lib/mosaic-geomath/src/test/java/org/eclipse/mosaic/lib/spatial/PointCloudTest.java new file mode 100644 index 000000000..8d3bd3186 --- /dev/null +++ b/lib/mosaic-geomath/src/test/java/org/eclipse/mosaic/lib/spatial/PointCloudTest.java @@ -0,0 +1,84 @@ +/* + * Copyright (c) 2024 Fraunhofer FOKUS and others. All rights reserved. + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0 + * + * SPDX-License-Identifier: EPL-2.0 + * + * Contact: mosaic@fokus.fraunhofer.de + */ + +package org.eclipse.mosaic.lib.spatial; + +import static org.hamcrest.CoreMatchers.is; +import static org.hamcrest.MatcherAssert.assertThat; + +import org.eclipse.mosaic.lib.math.Vector3d; +import org.eclipse.mosaic.lib.math.VectorUtils; + +import org.hamcrest.core.IsEqual; +import org.junit.Test; + +import java.util.List; + +public class PointCloudTest { + + @Test + public void relativeToAbsolute() { + // SETUP + PointCloud.Point p1 = new PointCloud.Point(new Vector3d(4.0, 5.0, 0.0), 0f, (byte) 0); + PointCloud.Point p2 = new PointCloud.Point(new Vector3d(-1.0, 2.0, 4.0), 0f, (byte) 1); + + PointCloud pc = new PointCloud(0, + new Vector3d(3, 1, 0), new RotationMatrix().rotate(90, VectorUtils.UP), + List.of(p1, p2), PointCloud.PointReference.RELATIVE + ); + + // RUN + List absolutePoints = pc.getAbsoluteEndPointsWithHit(); + + // ASSERT + PointCloud.Point p2absolute = new PointCloud.Point(new Vector3d(7, 3, 1.0), 0f, (byte) 1); + assertThat(absolutePoints.size(), is(1)); + assertThat(absolutePoints.get(0), is(fuzzyEqualTo(p2absolute))); + } + + @Test + public void absoluteToRelative() { + // SETUP + PointCloud.Point p1 = new PointCloud.Point(new Vector3d(4.0, 5.0, 0.0), 0f, (byte) 0); + PointCloud.Point p2 = new PointCloud.Point(new Vector3d(-1.0, 2.0, 4.0), 0f, (byte) 1); + + PointCloud pc = new PointCloud(0, + new Vector3d(3, 1, 0), new RotationMatrix().rotate(90, VectorUtils.UP), + List.of(p1, p2), PointCloud.PointReference.ABSOLUTE + ); + + // RUN + PointCloud.Point p2relative = new PointCloud.Point(new Vector3d(-4, 1, -4), 0f, (byte) 1); + + // ASSERT + List relativePoints = pc.getRelativeEndPointsWithHit(); + assertThat(relativePoints.size(), is(1)); + assertThat(relativePoints.get(0), is(fuzzyEqualTo(p2relative))); + } + + private static IsEqual fuzzyEqualTo(T base) { + return new IsEqual<>(base) { + @Override + public boolean matches(Object actualValue) { + if (actualValue instanceof Vector3d) { + return base.isFuzzyEqual((Vector3d) actualValue); + } + return super.matches(actualValue); + } + }; + } + + +} diff --git a/lib/mosaic-utils/src/main/java/org/eclipse/mosaic/lib/util/PointCloudSerialization.java b/lib/mosaic-utils/src/main/java/org/eclipse/mosaic/lib/util/PointCloudSerialization.java new file mode 100644 index 000000000..024028bd8 --- /dev/null +++ b/lib/mosaic-utils/src/main/java/org/eclipse/mosaic/lib/util/PointCloudSerialization.java @@ -0,0 +1,155 @@ +/* + * Copyright (c) 2024 Fraunhofer FOKUS and others. All rights reserved. + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0 + * + * SPDX-License-Identifier: EPL-2.0 + * + * Contact: mosaic@fokus.fraunhofer.de + */ + +package org.eclipse.mosaic.lib.util; + +import org.eclipse.mosaic.lib.math.MatrixElementOrder; +import org.eclipse.mosaic.lib.math.Vector3d; +import org.eclipse.mosaic.lib.spatial.PointCloud; +import org.eclipse.mosaic.lib.spatial.RotationMatrix; + +import java.io.ByteArrayInputStream; +import java.io.ByteArrayOutputStream; +import java.io.DataInput; +import java.io.DataInputStream; +import java.io.DataOutput; +import java.io.DataOutputStream; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; + +public class PointCloudSerialization { + + private static final Vector3d TMP_VECTOR = new Vector3d(); + + /** + * Creates a byte array from a {@link PointCloud} object (serialization), to be used for transport or storage. + * Use {@link #fromByteArray(byte[])} to deserialize the array back into a {@link PointCloud} object. + * + * @param pointCloud the {@link PointCloud} object to serialize into a byte array. + * @return a byte array to store/transport the {@link PointCloud} object. + */ + public static byte[] toByteArray(PointCloud pointCloud) { + if (pointCloud == null) { + return null; + } + try (ByteArrayOutputStream bos = new ByteArrayOutputStream()) { + DataOutput out = new DataOutputStream(bos); + writePointCloud(pointCloud, out); + return bos.toByteArray(); + } catch (IOException e) { + return new byte[0]; + } + } + + /** + * Creates a {@link PointCloud} from a (valid) sequence of bytes. The input byte array should be created beforehand + * using {@link #toByteArray}. If deserialization does not succeed, this method returns {@code null}. + * + * @param bytes the list of bytes previously generated by serializing a {@link PointCloud} + * @return the deserialized {@link PointCloud} object + */ + public static PointCloud fromByteArray(byte[] bytes) { + if (bytes.length == 0) { + return null; + } + + try (ByteArrayInputStream bos = new ByteArrayInputStream(bytes)) { + DataInput in = new DataInputStream(bos); + return readPointCloud(in); + } catch (IOException e) { + return null; + } + } + + private static void writePointCloud(PointCloud pointCloud, DataOutput out) throws IOException { + out.writeLong(pointCloud.getCreationTime()); + writeVector3d(pointCloud.getOrigin(), out); + writeRotationMatrix(pointCloud.getOrientation(), out); + if (pointCloud.getReferenceFormat() == PointCloud.PointReference.ABSOLUTE) { + out.writeBoolean(true); + out.writeInt(pointCloud.getAbsoluteEndPoints().size()); + for (PointCloud.Point point : pointCloud.getAbsoluteEndPoints()) { + writePoint(point, out); + } + } else { + out.writeBoolean(false); + out.writeInt(pointCloud.getRelativeEndPoints().size()); + for (PointCloud.Point point : pointCloud.getRelativeEndPoints()) { + writePoint(point, out); + } + } + } + + private static void writePoint(PointCloud.Point point, DataOutput dataOutput) throws IOException { + writeVector3d(point, dataOutput); + dataOutput.writeFloat(point.getDistance()); + dataOutput.writeByte(point.getHitType()); + } + + private static void writeVector3d(Vector3d vector3d, DataOutput dataOutput) throws IOException { + writeCoordinate(vector3d.x, dataOutput); + writeCoordinate(vector3d.y, dataOutput); + writeCoordinate(vector3d.z, dataOutput); + } + + private static void writeCoordinate(double v, DataOutput dataOutput) throws IOException { + dataOutput.writeFloat((float) v); + } + + private static void writeRotationMatrix(RotationMatrix rotation, DataOutput dataOutput) throws IOException { + double[] values = rotation.getAsDoubleArray(new double[9], MatrixElementOrder.COLUMN_MAJOR); + for (double value : values) { + dataOutput.writeDouble(value); + } + } + + private static PointCloud readPointCloud(DataInput in) throws IOException { + long timeStamp = in.readLong(); + Vector3d origin = readVector3d(in, new Vector3d()); + RotationMatrix orientation = readRotation(in); + boolean storeAbsolutePoints = in.readBoolean(); + int size = in.readInt(); + List endPoints = new ArrayList<>(size); + for (int i = 0; i < size; i++) { + endPoints.add(readPoint(in)); + } + if (storeAbsolutePoints) { + return new PointCloud(timeStamp, origin, orientation, endPoints, PointCloud.PointReference.ABSOLUTE); + } else { + return new PointCloud(timeStamp, origin, orientation, endPoints, PointCloud.PointReference.RELATIVE); + } + } + + private static PointCloud.Point readPoint(DataInput dataInput) throws IOException { + return new PointCloud.Point(readVector3d(dataInput, TMP_VECTOR), dataInput.readFloat(), dataInput.readByte()); + } + + private static Vector3d readVector3d(DataInput dataInput, Vector3d result) throws IOException { + return result.set(readCoordinate(dataInput), readCoordinate(dataInput), readCoordinate(dataInput)); + } + + private static double readCoordinate(DataInput dataInput) throws IOException { + return dataInput.readFloat(); + } + + private static RotationMatrix readRotation(DataInput dataInput) throws IOException { + double[] values = new double[9]; + for (int i = 0; i < values.length; i++) { + values[i] = dataInput.readDouble(); + } + return new RotationMatrix().set(values, MatrixElementOrder.COLUMN_MAJOR); + } +} diff --git a/lib/mosaic-utils/src/test/java/org/eclipse/mosaic/lib/util/PointCloudSerializationTest.java b/lib/mosaic-utils/src/test/java/org/eclipse/mosaic/lib/util/PointCloudSerializationTest.java new file mode 100644 index 000000000..b7625a757 --- /dev/null +++ b/lib/mosaic-utils/src/test/java/org/eclipse/mosaic/lib/util/PointCloudSerializationTest.java @@ -0,0 +1,60 @@ +/* + * Copyright (c) 2024 Fraunhofer FOKUS and others. All rights reserved. + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0 + * + * SPDX-License-Identifier: EPL-2.0 + * + * Contact: mosaic@fokus.fraunhofer.de + */ + +package org.eclipse.mosaic.lib.util; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertNotNull; +import static org.junit.Assert.assertTrue; + +import org.eclipse.mosaic.lib.math.Vector3d; +import org.eclipse.mosaic.lib.math.VectorUtils; +import org.eclipse.mosaic.lib.spatial.PointCloud; +import org.eclipse.mosaic.lib.spatial.RotationMatrix; +import org.eclipse.mosaic.rti.TIME; + +import org.junit.Test; + +import java.util.List; + +public class PointCloudSerializationTest { + + @Test + public void serialization() { + PointCloud.Point p1 = new PointCloud.Point(new Vector3d(4.0, 5.0, 0.0), 0f, (byte) 0); + PointCloud.Point p2 = new PointCloud.Point(new Vector3d(-1.0, 2.0, 4.0), 0f, (byte) 1); + + PointCloud expected = new PointCloud(4 * TIME.SECOND, + new Vector3d(3, 1, 0), new RotationMatrix().rotate(90, VectorUtils.UP), + List.of(p1, p2), PointCloud.PointReference.RELATIVE + ); + + //RUN + byte[] result = PointCloudSerialization.toByteArray(expected); + PointCloud actual = PointCloudSerialization.fromByteArray(result); + + //ASSERT + assertNotNull(actual); + assertEquals(expected.getCreationTime(), actual.getCreationTime()); + assertTrue(expected.getOrigin().isFuzzyEqual(actual.getOrigin())); + assertTrue(expected.getOrientation().isFuzzyEqual(actual.getOrientation())); + assertEquals(expected.getAbsoluteEndPoints().size(), actual.getAbsoluteEndPoints().size()); + assertEquals(expected.getRelativeEndPoints().size(), actual.getRelativeEndPoints().size()); + assertEquals(expected.getRelativeEndPointsWithHit().size(), actual.getRelativeEndPointsWithHit().size()); + for (int i = 0; i < actual.getRelativeEndPoints().size(); i++) { + assertTrue(expected.getRelativeEndPoints().get(i).isFuzzyEqual(actual.getRelativeEndPoints().get(i))); + } + } +}