From 97363324feef8521ec15f292b34abcc692c2bd7d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Thu, 17 Aug 2023 09:18:45 +0200 Subject: [PATCH 01/17] fix(perception): some fixes in perception model MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Moritz Schweppenhäuser --- .../perception/AbstractPerceptionModule.java | 2 +- .../errormodels/DistanceModifier.java | 4 ++-- .../errormodels/PerceptionModifier.java | 2 +- .../errormodels/PositionErrorModifier.java | 2 +- .../errormodels/SimpleOcclusionModifier.java | 4 ++-- .../errormodels/WallOcclusionModifier.java | 4 ++-- .../index/objects/TrafficLightObject.java | 2 +- .../index/objects/VehicleObject.java | 2 +- .../index/providers/VehicleIndex.java | 2 +- .../app/api/perception/PerceptionModule.java | 2 +- .../perception/PerceptionModifierTest.java | 21 ++++++++++++++++--- 11 files changed, 31 insertions(+), 16 deletions(-) diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/AbstractPerceptionModule.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/AbstractPerceptionModule.java index bbc34cb8c..614223ac4 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/AbstractPerceptionModule.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/AbstractPerceptionModule.java @@ -112,7 +112,7 @@ public List getPerceivedObjects() { private > List applyPerceptionModifiers(List objectsInRange) { List filteredList = new ArrayList<>(objectsInRange); - // create copy of all perceived objects to avoid interference with modifiers of other perception modules. + // create a copy of all perceived objects to avoid interference with modifiers of other perception modules. filteredList.replaceAll(T::copy); for (PerceptionModifier perceptionModifier : configuration.getPerceptionModifiers()) { filteredList = perceptionModifier.apply(owner, filteredList); // apply filters in sequence diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/DistanceModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/DistanceModifier.java index b50ed5f7a..df924b735 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/DistanceModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/DistanceModifier.java @@ -51,8 +51,8 @@ public DistanceModifier(RandomNumberGenerator rng, double offset) { } @Override - public List apply(PerceptionModuleOwner owner, List spatialObjects) { - if (spatialObjects.size() == 0) { + public > List apply(PerceptionModuleOwner owner, List spatialObjects) { + if (spatialObjects.isEmpty()) { return spatialObjects; } Vector3d ownerPosition = owner.getVehicleData().getProjectedPosition().toVector3d(); diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PerceptionModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PerceptionModifier.java index f980b6219..edb2caa62 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PerceptionModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PerceptionModifier.java @@ -27,5 +27,5 @@ public interface PerceptionModifier { * * @return the filtered/modified list */ - List apply(PerceptionModuleOwner owner, List spatialObjects); + > List apply(PerceptionModuleOwner owner, List spatialObjects); } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionErrorModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionErrorModifier.java index 41146508c..db314af0f 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionErrorModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionErrorModifier.java @@ -78,7 +78,7 @@ public PositionErrorModifier(RandomNumberGenerator rng, double longitudinalStand } @Override - public List apply(PerceptionModuleOwner owner, List spatialObjects) { + public > List apply(PerceptionModuleOwner owner, List spatialObjects) { Vector3d ownerPosition = owner.getVehicleData().getProjectedPosition().toVector3d(); Vector3d ownerDirection = new Vector3d(); VectorUtils.getDirectionVectorFromHeading(owner.getVehicleData().getHeading(), ownerDirection); diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/SimpleOcclusionModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/SimpleOcclusionModifier.java index a39778903..bc85b6467 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/SimpleOcclusionModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/SimpleOcclusionModifier.java @@ -62,8 +62,8 @@ public SimpleOcclusionModifier(double minDetectionAngle, double maxDetectionAngl } @Override - public List apply(PerceptionModuleOwner owner, List spatialObjects) { - if (spatialObjects.size() == 0) { + public > List apply(PerceptionModuleOwner owner, List spatialObjects) { + if (spatialObjects.isEmpty()) { return spatialObjects; } Vector3d ownerPosition = owner.getVehicleData().getProjectedPosition().toVector3d(); diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusionModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusionModifier.java index 6ae19d765..99e4c7ab1 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusionModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusionModifier.java @@ -36,8 +36,8 @@ public class WallOcclusionModifier implements PerceptionModifier { private final Vector3d intersectionResult = new Vector3d(); @Override - public List apply(PerceptionModuleOwner owner, List spatialObjects) { - if (spatialObjects.size() == 0) { + public > List apply(PerceptionModuleOwner owner, List spatialObjects) { + if (spatialObjects.isEmpty()) { return spatialObjects; } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java index 026ac0727..a63d62fe3 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java @@ -114,7 +114,7 @@ public boolean equals(Object o) { */ @Override public TrafficLightObject copy() { - return (TrafficLightObject) new TrafficLightObject(getId()) + return new TrafficLightObject(getId()) .setIncomingLane(getIncomingLane()) .setOutgoingLane(getOutgoingLane()) .setTrafficLightState(getTrafficLightState()) diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java index 08e56723a..6bdee9675 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java @@ -147,7 +147,7 @@ public boolean equals(Object o) { */ @Override public VehicleObject copy() { - return (VehicleObject) new VehicleObject(getId()) + return new VehicleObject(getId()) .setHeading(getHeading()) .setSpeed(getSpeed()) .setEdgeAndLane(getEdgeId(), getLaneIndex()) diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/providers/VehicleIndex.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/providers/VehicleIndex.java index a551d014b..b4fb34f6f 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/providers/VehicleIndex.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/providers/VehicleIndex.java @@ -77,7 +77,7 @@ VehicleObject addOrGetVehicle(VehicleData vehicleData) { } /** - * Returns the amount of indexed vehicles. + * Returns the number of indexed vehicles. * * @return the number of vehicles */ diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/app/api/perception/PerceptionModule.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/app/api/perception/PerceptionModule.java index 54c943029..a11d73d55 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/app/api/perception/PerceptionModule.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/app/api/perception/PerceptionModule.java @@ -61,7 +61,7 @@ public interface PerceptionModule * * @return a list of all {@link SpatialObject}s inside the perception range */ - > List getPerceivedObjects(); + > List> getPerceivedObjects(); /** * Call to get surrounding building walls. diff --git a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java index bb74de3bd..841e7ba5f 100644 --- a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java +++ b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java @@ -28,6 +28,7 @@ import org.eclipse.mosaic.fed.application.ambassador.SimulationKernelRule; import org.eclipse.mosaic.fed.application.ambassador.navigation.CentralNavigationComponent; import org.eclipse.mosaic.fed.application.ambassador.simulation.VehicleUnit; +import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.BoundingBoxOcclusionModifier; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.DistanceModifier; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.PositionErrorModifier; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.SimpleOcclusionModifier; @@ -123,7 +124,7 @@ public void setup() { when(egoVehicleData.getHeading()).thenReturn(90d); when(egoVehicleData.getProjectedPosition()).thenReturn(EGO_POSITION); - List randomPoints = getRandomlyDistributedPointsInRange(EGO_POSITION, VIEWING_RANGE, VEHICLE_AMOUNT); + List randomPoints = createRandomlyDistributedPointsInRange(EGO_POSITION, VIEWING_RANGE, VEHICLE_AMOUNT); if (PRINT_POSITIONS) { for (CartesianPoint randomPoint : randomPoints) { System.out.println(randomPoint.getX() + ", " + randomPoint.getY()); @@ -224,12 +225,25 @@ public void testIndexedObjectsNotChanged() { }); // ASSERT that all modified positions differ from the positions before (or after) applying the modifier - for (VehicleObject object: perceivedAndAlteredObjects) { + for (VehicleObject object : perceivedAndAlteredObjects) { assertFalse(object.getPosition().isFuzzyEqual(allVehiclesInIndexPre.get(object.getId()))); } } - private List getRandomlyDistributedPointsInRange(CartesianPoint origin, double range, int amount) { + @Test + public void testBoundingBoxOcclusionModifier() { + BoundingBoxOcclusionModifier boundingBoxOcclusionModifier = new BoundingBoxOcclusionModifier(); + simplePerceptionModule.enable( + new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(boundingBoxOcclusionModifier).build() + ); + List perceivedVehicles = simplePerceptionModule.getPerceivedVehicles(); + if (PRINT_POSITIONS) { + printPerceivedPositions(perceivedVehicles); + } + assertTrue("The occlusion filter should remove vehicles", VEHICLE_AMOUNT > perceivedVehicles.size()); + } + + private List createRandomlyDistributedPointsInRange(CartesianPoint origin, double range, int amount) { List points = new ArrayList<>(); for (int i = 0; i < amount; i++) { points.add(getRandomPointInRange(origin, range)); @@ -261,6 +275,7 @@ private void setupSpatialIndex(CartesianPoint... positions) { VehicleData vehicleDataMock = mock(VehicleData.class); when(vehicleDataMock.getProjectedPosition()).thenReturn(position); when(vehicleDataMock.getName()).thenReturn(vehicleId); + when(vehicleDataMock.getHeading()).thenReturn(rng.nextDouble() * 360d); vehiclesInIndex.add(vehicleDataMock); trafficObjectIndex.registerVehicleType(vehicleId, vehicleType); } From 3bad5e649a48fd2232ad443e91ce93eecae0793e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Thu, 17 Aug 2023 09:19:21 +0200 Subject: [PATCH 02/17] feat(perception): initial, non-functional bounding box occlusion MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Moritz Schweppenhäuser --- .../BoundingBoxOcclusionModifier.java | 167 ++++++++++++++++++ 1 file changed, 167 insertions(+) create mode 100644 fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java new file mode 100644 index 000000000..2a3dfa6bd --- /dev/null +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java @@ -0,0 +1,167 @@ +/* + * Copyright (c) 2023 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.fed.application.ambassador.simulation.perception.errormodels; + +import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.PerceptionModuleOwner; +import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.SpatialObject; +import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.VehicleObject; +import org.eclipse.mosaic.lib.math.Vector3d; +import org.eclipse.mosaic.lib.math.VectorUtils; +import org.eclipse.mosaic.lib.spatial.Edge; + +import com.google.common.collect.Lists; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +public class BoundingBoxOcclusionModifier implements PerceptionModifier { + + /** + * This defines how many equidistant points shall be evaluated per edge of a vehicle. + * Note generally for the front and rear edge this will result in a higher resolution compared to the sides of the vehicle. + */ + private final int edgeResolution; + + /** + * Threshold that defines how many of the points defined through {@link #edgeResolution} need to be visible in order for a + * vehicle to be treated as detected. + */ + private final int detectionThreshold; + + public BoundingBoxOcclusionModifier(int edgeResolution, int detectionThreshold) { + this.edgeResolution = edgeResolution; + this.detectionThreshold = detectionThreshold; + } + + @Override + public > List apply(PerceptionModuleOwner owner, List spatialObjects) { + List newObjects = new ArrayList<>(); + Map boundingBoxes = new HashMap<>(); + for (T spatialObject : spatialObjects) { + if (!(spatialObject instanceof VehicleObject)) { // skip non-vehicle spatial object + newObjects.add(spatialObject); + continue; + } + if (spatialObject.getId().equals(owner.getId())) { // skip calculation of own bounding box + continue; + } + VehicleObject currObject = (VehicleObject) spatialObject; + boundingBoxes.put(currObject, VehicleBoundingBox.createFromVehicleObject(currObject)); + } + for (Map.Entry boundingBoxEntry : boundingBoxes.entrySet()) { + VehicleObject currentVehicleObject = boundingBoxEntry.getKey(); + VehicleBoundingBox currentBoundingBox = boundingBoxEntry.getValue(); + List pointsToEvaluate = Lists.newArrayList( + currentBoundingBox.getFrontLeftCorner(), + currentBoundingBox.getFrontRightCorner(), + currentBoundingBox.getBackLeftCorner(), + currentBoundingBox.getFrontRightCorner() + ); + double spreadDistanceWidth = currentVehicleObject.getWidth() / edgeResolution; + double spreadDistanceLength = currentVehicleObject.getLength() / edgeResolution; + for (int i = 0; i < edgeResolution - 2; i++) { +// pointsToEvaluate.add() + } + int nonOccludedPoints = 0; + + } + return newObjects; + } + + + + /** + * An object representing a vehicles' bounding box. + * 
+     *   frontRightCorner----------rightEdge----------backRightCorner
+     *                  |                             |
+     *   Heading <== frontEdge                     backEdge
+     *                  |                             |
+     *    frontLeftCorner----------leftEdge-----------backLeftCorner
+     *
+ */ + static class VehicleBoundingBox { + private final Vector3d frontRightCorner; + private final Vector3d backRightCorner; + private final Vector3d backLeftCorner; + private final Vector3d frontLeftCorner; + private final Edge frontEdge; + private final Edge rightEdge; + private final Edge backEdge; + private final Edge leftEdge; + + private VehicleBoundingBox(Vector3d frontRightCorner, Vector3d backRightCorner, Vector3d backLeftCorner, Vector3d frontLeftCorner) { + this.frontRightCorner = frontRightCorner; + this.backRightCorner = backRightCorner; + this.backLeftCorner = backLeftCorner; + this.frontLeftCorner = frontLeftCorner; + frontEdge = new Edge<>(frontLeftCorner, frontRightCorner); + rightEdge = new Edge<>(frontRightCorner, backRightCorner); + backEdge = new Edge<>(backRightCorner, backLeftCorner); + leftEdge = new Edge<>(backLeftCorner, frontLeftCorner); + } + + public static VehicleBoundingBox createFromVehicleObject(VehicleObject vehicleObject) { + Vector3d headingVector = new Vector3d(); + VectorUtils.getDirectionVectorFromHeading(vehicleObject.getHeading(), headingVector).norm(); + double length = vehicleObject.getLength(); + double halfWidth = vehicleObject.getWidth() / 2; + Vector3d pointA = new Vector3d(headingVector).rotateDeg(90d, VectorUtils.UP); + pointA = pointA.multiply(halfWidth).add(vehicleObject); + Vector3d pointD = new Vector3d(headingVector).rotateDeg(-90d, VectorUtils.UP); + pointD = pointD.multiply(halfWidth).add(vehicleObject); + Vector3d pointB = new Vector3d(headingVector).multiply(-1d); + pointB = pointB.multiply(length).add(pointA); + Vector3d pointC = new Vector3d(headingVector).multiply(-1d); + pointC = pointC.multiply(length).add(pointD); + return new VehicleBoundingBox(pointA, pointB, pointC, pointD); + } + + public Vector3d getFrontRightCorner() { + return frontRightCorner; + } + + public Vector3d getBackRightCorner() { + return backRightCorner; + } + + public Vector3d getBackLeftCorner() { + return backLeftCorner; + } + + public Vector3d getFrontLeftCorner() { + return frontLeftCorner; + } + + public Edge getFrontEdge() { + return frontEdge; + } + + public Edge getRightEdge() { + return rightEdge; + } + + public Edge getBackEdge() { + return backEdge; + } + + public Edge getLeftEdge() { + return leftEdge; + } + } +} From ce70fc23d83e642f98990ffcccee482181aa4de9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Thu, 17 Aug 2023 13:38:01 +0200 Subject: [PATCH 03/17] feat(perception): added bounding box occlusion modifier (needs to be further tested) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Moritz Schweppenhäuser --- .../BoundingBoxOcclusionModifier.java | 121 ++++++++++-------- .../errormodels/DistanceModifier.java | 2 +- .../errormodels/PerceptionModifier.java | 2 +- .../errormodels/PositionErrorModifier.java | 2 +- .../errormodels/SimpleOcclusionModifier.java | 2 +- .../errormodels/WallOcclusionModifier.java | 2 +- .../perception/PerceptionModifierTest.java | 2 +- 7 files changed, 72 insertions(+), 61 deletions(-) diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java index 2a3dfa6bd..fd3b71284 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java @@ -31,30 +31,67 @@ public class BoundingBoxOcclusionModifier implements PerceptionModifier { + private final Vector3d intersectionResult = new Vector3d(); /** * This defines how many equidistant points shall be evaluated per edge of a vehicle. * Note generally for the front and rear edge this will result in a higher resolution compared to the sides of the vehicle. */ - private final int edgeResolution; + private final int pointsPerEdge; /** - * Threshold that defines how many of the points defined through {@link #edgeResolution} need to be visible in order for a - * vehicle to be treated as detected. + * Threshold that defines how many of the points defined through {@link #pointsPerEdge} need to be visible in order for a + * object to be treated as detected. */ private final int detectionThreshold; - public BoundingBoxOcclusionModifier(int edgeResolution, int detectionThreshold) { - this.edgeResolution = edgeResolution; + public BoundingBoxOcclusionModifier(int pointsPerEdge, int detectionThreshold) { + this.pointsPerEdge = pointsPerEdge; this.detectionThreshold = detectionThreshold; } @Override - public > List apply(PerceptionModuleOwner owner, List spatialObjects) { + public List apply(PerceptionModuleOwner owner, List spatialObjects) { List newObjects = new ArrayList<>(); + Map boundingBoxes = createBoundingBoxes(owner, spatialObjects); + for (T spatialObject : spatialObjects) { + List pointsToEvaluate = createPointsToEvaluate(spatialObject); + final int requiredVisiblePoints = pointsToEvaluate.size() == 1 ? 1 : detectionThreshold; + int numberOfPointsVisible = 0; + for (Vector3d point : pointsToEvaluate) { + boolean pointOccluded = false; + boundingBoxLoop: + for (Map.Entry occludingBoundingBoxEntry : boundingBoxes.entrySet()) { + if (occludingBoundingBoxEntry.getKey().getId().equals(spatialObject.getId())) { + continue; // cannot be occluded by itself + } + VehicleBoundingBox boundingBox = occludingBoundingBoxEntry.getValue(); + for (Edge vehicleSide : boundingBox.allEdges) { + boolean isOccluded = VectorUtils.computeXZEdgeIntersectionPoint( + owner.getVehicleData().getProjectedPosition().toVector3d(), + point, vehicleSide.a, vehicleSide.b, intersectionResult + ); + if (isOccluded) { + pointOccluded = true; + break boundingBoxLoop; + } + } + } + if (!pointOccluded) { + numberOfPointsVisible++; + } + if (numberOfPointsVisible == requiredVisiblePoints) { + newObjects.add(spatialObject); + break; + } + } + } + return newObjects; + } + + private static Map createBoundingBoxes(PerceptionModuleOwner owner, List spatialObjects) { Map boundingBoxes = new HashMap<>(); for (T spatialObject : spatialObjects) { if (!(spatialObject instanceof VehicleObject)) { // skip non-vehicle spatial object - newObjects.add(spatialObject); continue; } if (spatialObject.getId().equals(owner.getId())) { // skip calculation of own bounding box @@ -63,24 +100,25 @@ public > List apply(PerceptionModuleOwner owner, L VehicleObject currObject = (VehicleObject) spatialObject; boundingBoxes.put(currObject, VehicleBoundingBox.createFromVehicleObject(currObject)); } - for (Map.Entry boundingBoxEntry : boundingBoxes.entrySet()) { - VehicleObject currentVehicleObject = boundingBoxEntry.getKey(); - VehicleBoundingBox currentBoundingBox = boundingBoxEntry.getValue(); - List pointsToEvaluate = Lists.newArrayList( - currentBoundingBox.getFrontLeftCorner(), - currentBoundingBox.getFrontRightCorner(), - currentBoundingBox.getBackLeftCorner(), - currentBoundingBox.getFrontRightCorner() - ); - double spreadDistanceWidth = currentVehicleObject.getWidth() / edgeResolution; - double spreadDistanceLength = currentVehicleObject.getLength() / edgeResolution; - for (int i = 0; i < edgeResolution - 2; i++) { -// pointsToEvaluate.add() - } - int nonOccludedPoints = 0; + return boundingBoxes; + } + /** + * Creates a map of all points that shall be tested for occlusion. Initially, all booleans are set to {@code false} indicating + * that they're NOT occluded. + * + * @param spatialObject a {@link SpatialObject} for which the occlusion should be evaluated + */ + private List createPointsToEvaluate(T spatialObject) { + List pointsToEvaluate = new ArrayList<>(); + if (!(spatialObject instanceof VehicleObject)) { + // for all objects that don't have a bounding box, just evaluate the position for occlusion + pointsToEvaluate.add(spatialObject); + } else { + // for vehicles evaluate the four corners + pointsToEvaluate.addAll(VehicleBoundingBox.createFromVehicleObject((VehicleObject) spatialObject).allCorners); } - return newObjects; + return pointsToEvaluate; } @@ -96,10 +134,13 @@ public > List apply(PerceptionModuleOwner owner, L * */ static class VehicleBoundingBox { + private final List allCorners; private final Vector3d frontRightCorner; private final Vector3d backRightCorner; private final Vector3d backLeftCorner; private final Vector3d frontLeftCorner; + + private final List> allEdges; private final Edge frontEdge; private final Edge rightEdge; private final Edge backEdge; @@ -110,10 +151,12 @@ private VehicleBoundingBox(Vector3d frontRightCorner, Vector3d backRightCorner, this.backRightCorner = backRightCorner; this.backLeftCorner = backLeftCorner; this.frontLeftCorner = frontLeftCorner; + allCorners = Lists.newArrayList(frontRightCorner, backRightCorner, backLeftCorner, frontLeftCorner); frontEdge = new Edge<>(frontLeftCorner, frontRightCorner); rightEdge = new Edge<>(frontRightCorner, backRightCorner); backEdge = new Edge<>(backRightCorner, backLeftCorner); leftEdge = new Edge<>(backLeftCorner, frontLeftCorner); + allEdges = Lists.newArrayList(frontEdge, rightEdge, backEdge, leftEdge); } public static VehicleBoundingBox createFromVehicleObject(VehicleObject vehicleObject) { @@ -131,37 +174,5 @@ public static VehicleBoundingBox createFromVehicleObject(VehicleObject vehicleOb pointC = pointC.multiply(length).add(pointD); return new VehicleBoundingBox(pointA, pointB, pointC, pointD); } - - public Vector3d getFrontRightCorner() { - return frontRightCorner; - } - - public Vector3d getBackRightCorner() { - return backRightCorner; - } - - public Vector3d getBackLeftCorner() { - return backLeftCorner; - } - - public Vector3d getFrontLeftCorner() { - return frontLeftCorner; - } - - public Edge getFrontEdge() { - return frontEdge; - } - - public Edge getRightEdge() { - return rightEdge; - } - - public Edge getBackEdge() { - return backEdge; - } - - public Edge getLeftEdge() { - return leftEdge; - } } } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/DistanceModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/DistanceModifier.java index df924b735..88912b0ce 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/DistanceModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/DistanceModifier.java @@ -51,7 +51,7 @@ public DistanceModifier(RandomNumberGenerator rng, double offset) { } @Override - public > List apply(PerceptionModuleOwner owner, List spatialObjects) { + public List apply(PerceptionModuleOwner owner, List spatialObjects) { if (spatialObjects.isEmpty()) { return spatialObjects; } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PerceptionModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PerceptionModifier.java index edb2caa62..f980b6219 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PerceptionModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PerceptionModifier.java @@ -27,5 +27,5 @@ public interface PerceptionModifier { * * @return the filtered/modified list */ - > List apply(PerceptionModuleOwner owner, List spatialObjects); + List apply(PerceptionModuleOwner owner, List spatialObjects); } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionErrorModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionErrorModifier.java index db314af0f..41146508c 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionErrorModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionErrorModifier.java @@ -78,7 +78,7 @@ public PositionErrorModifier(RandomNumberGenerator rng, double longitudinalStand } @Override - public > List apply(PerceptionModuleOwner owner, List spatialObjects) { + public List apply(PerceptionModuleOwner owner, List spatialObjects) { Vector3d ownerPosition = owner.getVehicleData().getProjectedPosition().toVector3d(); Vector3d ownerDirection = new Vector3d(); VectorUtils.getDirectionVectorFromHeading(owner.getVehicleData().getHeading(), ownerDirection); diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/SimpleOcclusionModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/SimpleOcclusionModifier.java index bc85b6467..6dd3920bf 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/SimpleOcclusionModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/SimpleOcclusionModifier.java @@ -62,7 +62,7 @@ public SimpleOcclusionModifier(double minDetectionAngle, double maxDetectionAngl } @Override - public > List apply(PerceptionModuleOwner owner, List spatialObjects) { + public List apply(PerceptionModuleOwner owner, List spatialObjects) { if (spatialObjects.isEmpty()) { return spatialObjects; } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusionModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusionModifier.java index 99e4c7ab1..3f4cbcadc 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusionModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusionModifier.java @@ -36,7 +36,7 @@ public class WallOcclusionModifier implements PerceptionModifier { private final Vector3d intersectionResult = new Vector3d(); @Override - public > List apply(PerceptionModuleOwner owner, List spatialObjects) { + public List apply(PerceptionModuleOwner owner, List spatialObjects) { if (spatialObjects.isEmpty()) { return spatialObjects; } diff --git a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java index 841e7ba5f..e29516dac 100644 --- a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java +++ b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java @@ -232,7 +232,7 @@ public void testIndexedObjectsNotChanged() { @Test public void testBoundingBoxOcclusionModifier() { - BoundingBoxOcclusionModifier boundingBoxOcclusionModifier = new BoundingBoxOcclusionModifier(); + BoundingBoxOcclusionModifier boundingBoxOcclusionModifier = new BoundingBoxOcclusionModifier(2, 2); simplePerceptionModule.enable( new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(boundingBoxOcclusionModifier).build() ); From 96dd40ec1480954f7ed910b381a6ec98800d8f5f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Fri, 18 Aug 2023 15:34:00 +0200 Subject: [PATCH 04/17] feat(perception): added bounding box attribute to SpatialObject and properly implemented MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * BoundingBoxOcclusionModifier now evaluates if points are hidden behind other bounding boxes * WallOcclusionModifier now uses bounding boxes to see if objects are hidden Signed-off-by: Moritz Schweppenhäuser --- .../perception/SumoPerceptionModule.java | 1 + .../BoundingBoxOcclusionModifier.java | 175 +++++++++--------- .../errormodels/PositionErrorModifier.java | 3 +- .../errormodels/WallOcclusionModifier.java | 46 +++-- .../index/objects/PointBoundingBox.java | 51 +++++ .../index/objects/SpatialObject.java | 4 +- .../objects/SpatialObjectBoundingBox.java | 34 ++++ .../index/objects/TrafficLightObject.java | 12 ++ .../index/objects/VehicleBoundingBox.java | 119 ++++++++++++ .../index/objects/VehicleObject.java | 24 ++- .../index/providers/VehicleIndex.java | 6 +- .../perception/PerceptionModifierTest.java | 47 +++-- 12 files changed, 398 insertions(+), 124 deletions(-) create mode 100644 fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/PointBoundingBox.java create mode 100644 fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObjectBoundingBox.java create mode 100644 fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleBoundingBox.java diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/SumoPerceptionModule.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/SumoPerceptionModule.java index 87078ebda..2138783b0 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/SumoPerceptionModule.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/SumoPerceptionModule.java @@ -56,6 +56,7 @@ List getVehiclesInRange() { .setSpeed(v.getSpeed()) .setHeading(v.getHeading()) .setDimensions(v.getLength(), v.getWidth(), v.getHeight()) + .resetBoundingBox() ).collect(Collectors.toList()); } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java index fd3b71284..737f25c98 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java @@ -17,17 +17,16 @@ import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.PerceptionModuleOwner; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.SpatialObject; -import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.VehicleObject; +import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.SpatialObjectBoundingBox; +import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.TrafficLightObject; +import org.eclipse.mosaic.lib.math.MathUtils; import org.eclipse.mosaic.lib.math.Vector3d; import org.eclipse.mosaic.lib.math.VectorUtils; import org.eclipse.mosaic.lib.spatial.Edge; -import com.google.common.collect.Lists; - import java.util.ArrayList; -import java.util.HashMap; import java.util.List; -import java.util.Map; +import java.util.stream.Collectors; public class BoundingBoxOcclusionModifier implements PerceptionModifier { @@ -35,40 +34,71 @@ public class BoundingBoxOcclusionModifier implements PerceptionModifier { /** * This defines how many equidistant points shall be evaluated per edge of a vehicle. * Note generally for the front and rear edge this will result in a higher resolution compared to the sides of the vehicle. + * Default: 2 */ - private final int pointsPerEdge; + private final int pointsPerSide; /** - * Threshold that defines how many of the points defined through {@link #pointsPerEdge} need to be visible in order for a + * Threshold that defines how many of the points defined through {@link #pointsPerSide} need to be visible in order for a * object to be treated as detected. + * Default: 2 */ private final int detectionThreshold; - public BoundingBoxOcclusionModifier(int pointsPerEdge, int detectionThreshold) { - this.pointsPerEdge = pointsPerEdge; + /** + * Default constructor for the {@link BoundingBoxOcclusionModifier}. + */ + public BoundingBoxOcclusionModifier() { + this.pointsPerSide = 2; + this.detectionThreshold = 2; + } + + /** + * Constructor for {@link BoundingBoxOcclusionModifier}, validates and sets + * the parameters {@link #pointsPerSide} and {@link #detectionThreshold}. + * + * @param pointsPerSide the amount of points that will be evaluated per object (corners count towards 2 edges) + * @param detectionThreshold how many points have to be visible in order for an object to be treated as detected + */ + public BoundingBoxOcclusionModifier(int pointsPerSide, int detectionThreshold) { + if (pointsPerSide < 2) { + throw new RuntimeException("Need at least 2 points per edge, meaning every corner will be checked for occlusion."); + } + if (detectionThreshold < 1) { + throw new RuntimeException("At least one point has to be checked for occlusion, else no objects will be occluded"); + } + this.pointsPerSide = pointsPerSide; this.detectionThreshold = detectionThreshold; } @Override public List apply(PerceptionModuleOwner owner, List spatialObjects) { List newObjects = new ArrayList<>(); - Map boundingBoxes = createBoundingBoxes(owner, spatialObjects); + // the ego object cannot occlude vision + List occludingObjects = spatialObjects.stream() + .filter(object -> !object.getId().equals(owner.getId())) + .collect(Collectors.toList()); for (T spatialObject : spatialObjects) { + if (spatialObject instanceof TrafficLightObject) { // Traffic Lights are treated to not be occluded + newObjects.add(spatialObject); + continue; + } List pointsToEvaluate = createPointsToEvaluate(spatialObject); final int requiredVisiblePoints = pointsToEvaluate.size() == 1 ? 1 : detectionThreshold; int numberOfPointsVisible = 0; for (Vector3d point : pointsToEvaluate) { boolean pointOccluded = false; boundingBoxLoop: - for (Map.Entry occludingBoundingBoxEntry : boundingBoxes.entrySet()) { - if (occludingBoundingBoxEntry.getKey().getId().equals(spatialObject.getId())) { + for (T occludingObject : occludingObjects) { + if (occludingObject.getId().equals(spatialObject.getId())) { continue; // cannot be occluded by itself } - VehicleBoundingBox boundingBox = occludingBoundingBoxEntry.getValue(); - for (Edge vehicleSide : boundingBox.allEdges) { + SpatialObjectBoundingBox boundingBox = occludingObject.getBoundingBox(); + // SpatialObjects with PointBoundingBoxes won't occlude anything, as they have no edges defined + for (Edge side : boundingBox.getAllEdges()) { boolean isOccluded = VectorUtils.computeXZEdgeIntersectionPoint( owner.getVehicleData().getProjectedPosition().toVector3d(), - point, vehicleSide.a, vehicleSide.b, intersectionResult + point, side.a, side.b, intersectionResult ); if (isOccluded) { pointOccluded = true; @@ -88,91 +118,52 @@ public List apply(PerceptionModuleOwner owner, List return newObjects; } - private static Map createBoundingBoxes(PerceptionModuleOwner owner, List spatialObjects) { - Map boundingBoxes = new HashMap<>(); - for (T spatialObject : spatialObjects) { - if (!(spatialObject instanceof VehicleObject)) { // skip non-vehicle spatial object - continue; - } - if (spatialObject.getId().equals(owner.getId())) { // skip calculation of own bounding box - continue; - } - VehicleObject currObject = (VehicleObject) spatialObject; - boundingBoxes.put(currObject, VehicleBoundingBox.createFromVehicleObject(currObject)); - } - return boundingBoxes; - } - /** - * Creates a map of all points that shall be tested for occlusion. Initially, all booleans are set to {@code false} indicating - * that they're NOT occluded. - * + * Creates a list of all points that shall be tested for occlusion. If {@link #pointsPerSide} is set to a value larger than 2, + * each side will have additional equidistant points added. + * Example for {@code pointsPerSide = 3} (x's are the corners which will be evaluated anyway, o's are the added points): + * 
+     *     x-----o-----x
+     *     |           |
+     *     |           |
+     *     o           o
+     *     |           |
+     *     |           |
+     *     x-----o-----y
+     *
* @param spatialObject a {@link SpatialObject} for which the occlusion should be evaluated */ private List createPointsToEvaluate(T spatialObject) { List pointsToEvaluate = new ArrayList<>(); - if (!(spatialObject instanceof VehicleObject)) { - // for all objects that don't have a bounding box, just evaluate the position for occlusion - pointsToEvaluate.add(spatialObject); - } else { - // for vehicles evaluate the four corners - pointsToEvaluate.addAll(VehicleBoundingBox.createFromVehicleObject((VehicleObject) spatialObject).allCorners); + SpatialObjectBoundingBox boundingBox = spatialObject.getBoundingBox(); + // if object has edges and more than 2 points per side are to be evaluated, calculate points that have to be evaluated + if (pointsPerSide > 2 && !boundingBox.getAllEdges().isEmpty()) { + for (Edge edge : boundingBox.getAllEdges()) { + Vector3d start = edge.a; + if (pointNotPresent(pointsToEvaluate, start)) { + pointsToEvaluate.add(start); + } + Vector3d end = edge.b; + if (pointNotPresent(pointsToEvaluate, end)) { + pointsToEvaluate.add(end); + } + for (int i = 1; i < pointsPerSide - 1; i++) { + double ratio = (double) i / (pointsPerSide + 1); + double xNew = start.x + ratio * (end.x - start.x); + double zNew = start.z + ratio * (end.z - start.z); + Vector3d newPoint = new Vector3d(xNew, 0, zNew); + if (pointNotPresent(pointsToEvaluate, newPoint)) { + pointsToEvaluate.add(newPoint); + } + } + } + } else { // else just add all corners + pointsToEvaluate.addAll(boundingBox.getAllCorners()); } return pointsToEvaluate; } - - - /** - * An object representing a vehicles' bounding box. - * 
-     *   frontRightCorner----------rightEdge----------backRightCorner
-     *                  |                             |
-     *   Heading <== frontEdge                     backEdge
-     *                  |                             |
-     *    frontLeftCorner----------leftEdge-----------backLeftCorner
-     *
- */ - static class VehicleBoundingBox { - private final List allCorners; - private final Vector3d frontRightCorner; - private final Vector3d backRightCorner; - private final Vector3d backLeftCorner; - private final Vector3d frontLeftCorner; - - private final List> allEdges; - private final Edge frontEdge; - private final Edge rightEdge; - private final Edge backEdge; - private final Edge leftEdge; - - private VehicleBoundingBox(Vector3d frontRightCorner, Vector3d backRightCorner, Vector3d backLeftCorner, Vector3d frontLeftCorner) { - this.frontRightCorner = frontRightCorner; - this.backRightCorner = backRightCorner; - this.backLeftCorner = backLeftCorner; - this.frontLeftCorner = frontLeftCorner; - allCorners = Lists.newArrayList(frontRightCorner, backRightCorner, backLeftCorner, frontLeftCorner); - frontEdge = new Edge<>(frontLeftCorner, frontRightCorner); - rightEdge = new Edge<>(frontRightCorner, backRightCorner); - backEdge = new Edge<>(backRightCorner, backLeftCorner); - leftEdge = new Edge<>(backLeftCorner, frontLeftCorner); - allEdges = Lists.newArrayList(frontEdge, rightEdge, backEdge, leftEdge); - } - - public static VehicleBoundingBox createFromVehicleObject(VehicleObject vehicleObject) { - Vector3d headingVector = new Vector3d(); - VectorUtils.getDirectionVectorFromHeading(vehicleObject.getHeading(), headingVector).norm(); - double length = vehicleObject.getLength(); - double halfWidth = vehicleObject.getWidth() / 2; - Vector3d pointA = new Vector3d(headingVector).rotateDeg(90d, VectorUtils.UP); - pointA = pointA.multiply(halfWidth).add(vehicleObject); - Vector3d pointD = new Vector3d(headingVector).rotateDeg(-90d, VectorUtils.UP); - pointD = pointD.multiply(halfWidth).add(vehicleObject); - Vector3d pointB = new Vector3d(headingVector).multiply(-1d); - pointB = pointB.multiply(length).add(pointA); - Vector3d pointC = new Vector3d(headingVector).multiply(-1d); - pointC = pointC.multiply(length).add(pointD); - return new VehicleBoundingBox(pointA, pointB, pointC, pointD); - } + private boolean pointNotPresent(List points, Vector3d newPoint) { + return points.stream().noneMatch(vector3d -> vector3d.isFuzzyEqual(newPoint)); } } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionErrorModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionErrorModifier.java index 41146508c..d8e95cbc7 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionErrorModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionErrorModifier.java @@ -17,6 +17,7 @@ import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.PerceptionModuleOwner; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.SpatialObject; +import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.VehicleObject; import org.eclipse.mosaic.lib.math.RandomNumberGenerator; import org.eclipse.mosaic.lib.math.Vector3d; import org.eclipse.mosaic.lib.math.VectorUtils; @@ -85,7 +86,7 @@ public List apply(PerceptionModuleOwner owner, List double angleToNorth = ownerDirection.angle(VectorUtils.NORTH); spatialObjects.forEach( spatialObject -> { - Vector3d relativePosition = getVectorRelativeTo(ownerPosition, spatialObject.getPosition()); // get position relative to owner + Vector3d relativePosition = getVectorRelativeTo(ownerPosition, spatialObject.getPosition()); // pos relative to owner Vector3d adjustedVector = new Vector3d(relativePosition); adjustedVector.rotate(-angleToNorth, VectorUtils.UP); // rotate vector according to orientation // add lateral and longitudinal errors diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusionModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusionModifier.java index 3f4cbcadc..10f90e3f3 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusionModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusionModifier.java @@ -33,8 +33,17 @@ */ public class WallOcclusionModifier implements PerceptionModifier { + private final int requiredVisibleVehicleCorners; private final Vector3d intersectionResult = new Vector3d(); + public WallOcclusionModifier() { + requiredVisibleVehicleCorners = 2; + } + + public WallOcclusionModifier(int requiredVisibleVehicleCorners) { + this.requiredVisibleVehicleCorners = requiredVisibleVehicleCorners; + } + @Override public List apply(PerceptionModuleOwner owner, List spatialObjects) { if (spatialObjects.isEmpty()) { @@ -45,25 +54,34 @@ public List apply(PerceptionModuleOwner owner, List if (walls.isEmpty()) { return spatialObjects; } - final Vector3d ownerPosition = owner.getVehicleData().getProjectedPosition().toVector3d(); - final Vector3d otherPosition = new Vector3d(); - final List result = new ArrayList<>(); - - vehicleLoop: for (T spatialObject : spatialObjects) { - spatialObject.getProjectedPosition().toVector3d(otherPosition); - - for (Edge wall : walls) { - boolean isHidden = - VectorUtils.computeXZEdgeIntersectionPoint(ownerPosition, otherPosition, wall.a, wall.b, intersectionResult); - if (isHidden) { - continue vehicleLoop; + List pointsToEvaluate = spatialObject.getBoundingBox().getAllCorners(); + // we say that at least half of the corners have to be visible rounding up for odd numbers + final int requiredVisiblePoints = (int) Math.ceil((double) pointsToEvaluate.size() / 2); + int numberOfPointsVisible = 0; + for (Vector3d point : pointsToEvaluate) { + boolean pointOccluded = false; + for (Edge wall : walls) { + // SpatialObjects with PointBoundingBoxes won't occlude anything, as they have no edges defined + boolean isOccluded = VectorUtils.computeXZEdgeIntersectionPoint( + owner.getVehicleData().getProjectedPosition().toVector3d(), + point, wall.a, wall.b, intersectionResult + ); + if (isOccluded) { + pointOccluded = true; + break; + } + } + if (!pointOccluded) { + numberOfPointsVisible++; + } + if (numberOfPointsVisible == requiredVisiblePoints) { + result.add(spatialObject); + break; } } - result.add(spatialObject); } - return result; } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/PointBoundingBox.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/PointBoundingBox.java new file mode 100644 index 000000000..8cae105aa --- /dev/null +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/PointBoundingBox.java @@ -0,0 +1,51 @@ +/* + * Copyright (c) 2023 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.fed.application.ambassador.simulation.perception.index.objects; + +import org.eclipse.mosaic.lib.math.Vector3d; +import org.eclipse.mosaic.lib.spatial.Edge; + +import java.util.ArrayList; +import java.util.List; + +/** + * This bounding box represents + */ +public class PointBoundingBox implements SpatialObjectBoundingBox { + + /** + * A one element list, defining the position of a {@link SpatialObject} as the only corner. + */ + private final List allCorners = new ArrayList<>(); + /** + * An empty list as a no bounding box is spanned by a single point. + */ + private final List> allEdges = new ArrayList<>(); + + public PointBoundingBox(Vector3d position) { + allCorners.add(position); + } + + @Override + public List getAllCorners() { + return allCorners; + } + + @Override + public List> getAllEdges() { + return allEdges; + } +} diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObject.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObject.java index e550dada6..764d62ea3 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObject.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObject.java @@ -25,7 +25,7 @@ public abstract class SpatialObject> extends Vector3d private final String id; - final MutableCartesianPoint cartesianPosition = new MutableCartesianPoint(); + protected final MutableCartesianPoint cartesianPosition = new MutableCartesianPoint(); public SpatialObject(String id) { this.id = id; @@ -33,6 +33,8 @@ public SpatialObject(String id) { public abstract T setPosition(CartesianPoint position); + public abstract SpatialObjectBoundingBox getBoundingBox(); + /** * Sets the position of the {@link SpatialObject}. * diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObjectBoundingBox.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObjectBoundingBox.java new file mode 100644 index 000000000..b56d43e76 --- /dev/null +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObjectBoundingBox.java @@ -0,0 +1,34 @@ +/* + * Copyright (c) 2023 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.fed.application.ambassador.simulation.perception.index.objects; + +import org.eclipse.mosaic.lib.math.Vector3d; +import org.eclipse.mosaic.lib.spatial.Edge; + +import java.util.List; + +public interface SpatialObjectBoundingBox { + + /** + * Returns all corners spanning the 2D bounding box of a {@link SpatialObject}. + */ + List getAllCorners(); + + /** + * Returns all sides spanning the 2D bounding box of a {@link SpatialObject}. + */ + List> getAllEdges(); +} diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java index a63d62fe3..864cae7a9 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java @@ -37,6 +37,10 @@ public class TrafficLightObject extends SpatialObject { * The outgoing lane controlled by this traffic light. */ private String outgoingLane; + /** + * The bounding box of a traffic light is represented by a single point. + */ + private PointBoundingBox boundingBox; public TrafficLightObject(String id) { super(id); @@ -46,9 +50,17 @@ public TrafficLightObject(String id) { public TrafficLightObject setPosition(CartesianPoint position) { cartesianPosition.set(position); position.toVector3d(this); + if (boundingBox == null) { + boundingBox = new PointBoundingBox(this); + } return this; } + @Override + public SpatialObjectBoundingBox getBoundingBox() { + return boundingBox; + } + public String getTrafficLightGroupId() { return trafficLightGroupId; } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleBoundingBox.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleBoundingBox.java new file mode 100644 index 000000000..f043d753d --- /dev/null +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleBoundingBox.java @@ -0,0 +1,119 @@ +/* + * Copyright (c) 2023 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.fed.application.ambassador.simulation.perception.index.objects; + +import org.eclipse.mosaic.lib.math.Vector3d; +import org.eclipse.mosaic.lib.math.VectorUtils; +import org.eclipse.mosaic.lib.spatial.Edge; + +import com.google.common.collect.Lists; + +import java.util.List; + +/** + * An object representing a vehicles' 2D bounding box. + * 
+ *   frontRightCorner----------rightEdge----------backRightCorner
+ *                  |                             |
+ *   Heading <== frontEdge                     backEdge
+ *                  |                             |
+ *    frontLeftCorner----------leftEdge-----------backLeftCorner
+ *
+ */ +public class VehicleBoundingBox implements SpatialObjectBoundingBox { + private final List allCorners; + private final Vector3d frontRightCorner; + private final Vector3d backRightCorner; + private final Vector3d backLeftCorner; + private final Vector3d frontLeftCorner; + + private final List> allEdges; + private final Edge frontEdge; + private final Edge rightEdge; + private final Edge backEdge; + private final Edge leftEdge; + + private VehicleBoundingBox(Vector3d frontRightCorner, Vector3d backRightCorner, Vector3d backLeftCorner, Vector3d frontLeftCorner) { + this.frontRightCorner = frontRightCorner; + this.backRightCorner = backRightCorner; + this.backLeftCorner = backLeftCorner; + this.frontLeftCorner = frontLeftCorner; + allCorners = Lists.newArrayList(frontRightCorner, backRightCorner, backLeftCorner, frontLeftCorner); + frontEdge = new Edge<>(frontLeftCorner, frontRightCorner); + rightEdge = new Edge<>(frontRightCorner, backRightCorner); + backEdge = new Edge<>(backRightCorner, backLeftCorner); + leftEdge = new Edge<>(backLeftCorner, frontLeftCorner); + allEdges = Lists.newArrayList(frontEdge, rightEdge, backEdge, leftEdge); + } + + public static VehicleBoundingBox createFromVehicleObject(VehicleObject vehicleObject) { + Vector3d headingVector = new Vector3d(); + VectorUtils.getDirectionVectorFromHeading(vehicleObject.getHeading(), headingVector).norm(); + double length = vehicleObject.getLength(); + double halfWidth = vehicleObject.getWidth() / 2; + Vector3d pointA = new Vector3d(headingVector).rotateDeg(90d, VectorUtils.UP); + pointA = pointA.multiply(halfWidth).add(vehicleObject); + Vector3d pointD = new Vector3d(headingVector).rotateDeg(-90d, VectorUtils.UP); + pointD = pointD.multiply(halfWidth).add(vehicleObject); + Vector3d pointB = new Vector3d(headingVector).multiply(-1d); + pointB = pointB.multiply(length).add(pointA); + Vector3d pointC = new Vector3d(headingVector).multiply(-1d); + pointC = pointC.multiply(length).add(pointD); + return new VehicleBoundingBox(pointA, pointB, pointC, pointD); + } + + @Override + public List getAllCorners() { + return allCorners; + } + + @Override + public List> getAllEdges() { + return allEdges; + } + + public Vector3d getFrontRightCorner() { + return frontRightCorner; + } + + public Vector3d getBackRightCorner() { + return backRightCorner; + } + + public Vector3d getBackLeftCorner() { + return backLeftCorner; + } + + public Vector3d getFrontLeftCorner() { + return frontLeftCorner; + } + + public Edge getFrontEdge() { + return frontEdge; + } + + public Edge getRightEdge() { + return rightEdge; + } + + public Edge getBackEdge() { + return backEdge; + } + + public Edge getLeftEdge() { + return leftEdge; + } +} diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java index 6bdee9675..3d3979768 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java @@ -51,6 +51,10 @@ public class VehicleObject extends SpatialObject { * The height of the vehicle. [m] */ private double height; + /** + * The 2D bounding box of a vehicle from birds eye view. + */ + private VehicleBoundingBox boundingBox = null; public VehicleObject(String id) { super(id); @@ -115,6 +119,23 @@ public double getHeight() { return height; } + public VehicleObject resetBoundingBox() { + boundingBox = null; + return this; + } + + /** + * Returns the bounding box for a spatial object if requested. + * Calculation is only triggered if bounding box is requested. + */ + @Override + public SpatialObjectBoundingBox getBoundingBox() { + if (boundingBox == null) { + boundingBox = VehicleBoundingBox.createFromVehicleObject(this); + } + return boundingBox; + } + @Override public boolean equals(Object o) { if (this == o) { @@ -152,7 +173,8 @@ public VehicleObject copy() { .setSpeed(getSpeed()) .setEdgeAndLane(getEdgeId(), getLaneIndex()) .setDimensions(getLength(), getWidth(), getHeight()) - .setPosition(getProjectedPosition()); + .setPosition(getProjectedPosition()) + .resetBoundingBox(); } } \ No newline at end of file diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/providers/VehicleIndex.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/providers/VehicleIndex.java index b4fb34f6f..677783670 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/providers/VehicleIndex.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/providers/VehicleIndex.java @@ -61,7 +61,8 @@ VehicleObject addOrGetVehicle(VehicleData vehicleData) { vehicleObject = new VehicleObject(vehicleId) .setHeading(vehicleData.getHeading()) .setSpeed(vehicleData.getSpeed()) - .setPosition(vehicleData.getProjectedPosition()); + .setPosition(vehicleData.getProjectedPosition()) + .resetBoundingBox(); VehicleType vehicleType = registeredVehicleTypes.get(vehicleId); if (vehicleType != null) { // vehicles with no cached type will have (0,0,0) dimensions vehicleObject.setDimensions( @@ -156,7 +157,8 @@ public void updateVehicles(Iterable vehiclesToUpdate) { VehicleObject vehicleObject = addOrGetVehicle(v) .setHeading(v.getHeading()) .setSpeed(v.getSpeed()) - .setPosition(vehiclePosition); + .setPosition(vehiclePosition) + .resetBoundingBox(); if (v.getRoadPosition() != null) { vehicleObject.setEdgeAndLane(v.getRoadPosition().getConnectionId(), v.getRoadPosition().getLaneIndex()); } diff --git a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java index e29516dac..986e5784f 100644 --- a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java +++ b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java @@ -47,6 +47,7 @@ import org.eclipse.mosaic.lib.math.Vector3d; import org.eclipse.mosaic.lib.objects.vehicle.VehicleData; import org.eclipse.mosaic.lib.objects.vehicle.VehicleType; +import org.eclipse.mosaic.lib.spatial.BoundingBox; import org.eclipse.mosaic.lib.spatial.Edge; import org.eclipse.mosaic.lib.util.scheduling.EventManager; @@ -125,13 +126,11 @@ public void setup() { when(egoVehicleData.getProjectedPosition()).thenReturn(EGO_POSITION); List randomPoints = createRandomlyDistributedPointsInRange(EGO_POSITION, VIEWING_RANGE, VEHICLE_AMOUNT); + setupSpatialIndex(randomPoints.toArray(new CartesianPoint[0])); if (PRINT_POSITIONS) { - for (CartesianPoint randomPoint : randomPoints) { - System.out.println(randomPoint.getX() + ", " + randomPoint.getY()); - } + printBoundingBoxes(getAllVehicles()); System.out.println(); } - setupSpatialIndex(randomPoints.toArray(new CartesianPoint[0])); } @Test @@ -142,7 +141,7 @@ public void testOcclusionModifier() { ); List perceivedVehicles = simplePerceptionModule.getPerceivedVehicles(); if (PRINT_POSITIONS) { - printPerceivedPositions(perceivedVehicles); + printBoundingBoxes(perceivedVehicles); } assertTrue("The occlusion filter should remove vehicles", VEHICLE_AMOUNT > perceivedVehicles.size()); } @@ -156,7 +155,7 @@ public void testDistanceErrorModifier() { List perceivedVehicles = simplePerceptionModule.getPerceivedVehicles(); if (PRINT_POSITIONS) { - printPerceivedPositions(perceivedVehicles); + printBoundingBoxes(perceivedVehicles); } assertTrue("The distance filter should remove vehicles", VEHICLE_AMOUNT > perceivedVehicles.size()); } @@ -170,7 +169,7 @@ public void testPositionErrorModifier() { List perceivedVehicles = simplePerceptionModule.getPerceivedVehicles(); if (PRINT_POSITIONS) { - printPerceivedPositions(perceivedVehicles); + printBoundingBoxes(perceivedVehicles); } assertEquals("The position error filter shouldn't remove vehicles", VEHICLE_AMOUNT, perceivedVehicles.size()); } @@ -188,7 +187,7 @@ public void testWallOcclusionModifier() { ); List perceivedVehicles = simplePerceptionModule.getPerceivedVehicles(); if (PRINT_POSITIONS) { - printPerceivedPositions(perceivedVehicles); + printBoundingBoxes(perceivedVehicles); } assertTrue("The occlusion filter should remove vehicles", VEHICLE_AMOUNT > perceivedVehicles.size()); // assert roughly that every perceived vehicle right of the wall is not hidden by the wall @@ -232,13 +231,13 @@ public void testIndexedObjectsNotChanged() { @Test public void testBoundingBoxOcclusionModifier() { - BoundingBoxOcclusionModifier boundingBoxOcclusionModifier = new BoundingBoxOcclusionModifier(2, 2); + BoundingBoxOcclusionModifier boundingBoxOcclusionModifier = new BoundingBoxOcclusionModifier(); simplePerceptionModule.enable( new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(boundingBoxOcclusionModifier).build() ); List perceivedVehicles = simplePerceptionModule.getPerceivedVehicles(); if (PRINT_POSITIONS) { - printPerceivedPositions(perceivedVehicles); + printBoundingBoxes(perceivedVehicles); } assertTrue("The occlusion filter should remove vehicles", VEHICLE_AMOUNT > perceivedVehicles.size()); } @@ -282,10 +281,32 @@ private void setupSpatialIndex(CartesianPoint... positions) { trafficObjectIndex.updateVehicles(vehiclesInIndex); } - private void printPerceivedPositions(List perceivedVehicles) { + private void printBoundingBoxes(List perceivedVehicles) { for (VehicleObject vehicleObject : perceivedVehicles) { - CartesianPoint point = vehicleObject.toCartesian(); - System.out.println(point.getX() + ", " + point.getY()); + List toPrint = new ArrayList<>(); + toPrint.add(vehicleObject.toCartesian().getX() + "," + vehicleObject.toCartesian().getY()); + for (Vector3d corner : vehicleObject.getBoundingBox().getAllCorners()) { + CartesianPoint point = corner.toCartesian(); + toPrint.add(point.getX() + "," + point.getY()); + } + System.out.println(String.join(";", toPrint)); } } + + private List getAllVehicles() { + return trafficObjectIndex.getVehiclesInRange(new PerceptionModel() { + @Override + public boolean isInRange(SpatialObject other) { + return true; + } + + @Override + public BoundingBox getBoundingBox() { + BoundingBox boundingBox = new BoundingBox(); + boundingBox.add(CartesianPoint.xy(-200, -200).toVector3d()); + boundingBox.add(CartesianPoint.xy(200, 200).toVector3d()); + return boundingBox; + } + }); + } } From 0a4e4e4f529e9a630777df2c9b4de4f1c3b409d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Mon, 21 Aug 2023 10:23:44 +0200 Subject: [PATCH 05/17] feat(perception): added additional check in BoundingBoxOcclusionModifier constructor MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Moritz Schweppenhäuser --- .../perception/errormodels/BoundingBoxOcclusionModifier.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java index 737f25c98..0eb6582c5 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java @@ -67,6 +67,9 @@ public BoundingBoxOcclusionModifier(int pointsPerSide, int detectionThreshold) { if (detectionThreshold < 1) { throw new RuntimeException("At least one point has to be checked for occlusion, else no objects will be occluded"); } + if (detectionThreshold > pointsPerSide * 4 - 4) { + throw new RuntimeException("The detection threshold exceeds the number of points evaluated per object"); + } this.pointsPerSide = pointsPerSide; this.detectionThreshold = detectionThreshold; } From 3727cca9f564558e0f1f68950e938a4167191471 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Mon, 21 Aug 2023 10:40:44 +0200 Subject: [PATCH 06/17] feat(perception): extended bounding box occlusion test MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Moritz Schweppenhäuser --- .../perception/PerceptionModifierTest.java | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java index 986e5784f..d57a62d6a 100644 --- a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java +++ b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java @@ -236,12 +236,27 @@ public void testBoundingBoxOcclusionModifier() { new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(boundingBoxOcclusionModifier).build() ); List perceivedVehicles = simplePerceptionModule.getPerceivedVehicles(); + // create a modifier with more point but same detection threshold -> should result in more detections + BoundingBoxOcclusionModifier boundingBoxOcclusionModifierCustomParams = new BoundingBoxOcclusionModifier(5, 2); + simplePerceptionModule.enable( + new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE) + .addModifier(boundingBoxOcclusionModifierCustomParams).build() + ); + List perceivedVehiclesCustomModifier = simplePerceptionModule.getPerceivedVehicles(); + if (PRINT_POSITIONS) { printBoundingBoxes(perceivedVehicles); + printBoundingBoxes(perceivedVehiclesCustomModifier); } assertTrue("The occlusion filter should remove vehicles", VEHICLE_AMOUNT > perceivedVehicles.size()); + assertTrue("The occlusion filter should remove vehicles", VEHICLE_AMOUNT > perceivedVehiclesCustomModifier.size()); + assertTrue( + "The \"stricter\" occlusion filter should remove more vehicles", + perceivedVehicles.size() < perceivedVehiclesCustomModifier.size() + ); } + private List createRandomlyDistributedPointsInRange(CartesianPoint origin, double range, int amount) { List points = new ArrayList<>(); for (int i = 0; i < amount; i++) { From 1a2beb24dfa63c8aeec0958d7217edd0cb801ad2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Wed, 30 Aug 2023 13:11:44 +0200 Subject: [PATCH 07/17] fix(perception): working on PR-review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * renaming of occlusion modifiers Signed-off-by: Moritz Schweppenhäuser --- .../app/tutorial/vehicle/PerceptionApp.java | 6 +++--- ...odifier.java => BoundingBoxOcclusion.java} | 20 +++++++++---------- ...sionModifier.java => SimpleOcclusion.java} | 6 +++--- ...lusionModifier.java => WallOcclusion.java} | 6 +++--- .../perception/PerceptionModifierTest.java | 18 ++++++++--------- 5 files changed, 28 insertions(+), 28 deletions(-) rename fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/{BoundingBoxOcclusionModifier.java => BoundingBoxOcclusion.java} (88%) rename fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/{SimpleOcclusionModifier.java => SimpleOcclusion.java} (96%) rename fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/{WallOcclusionModifier.java => WallOcclusion.java} (95%) diff --git a/app/tutorials/example-applications/src/main/java/org/eclipse/mosaic/app/tutorial/vehicle/PerceptionApp.java b/app/tutorials/example-applications/src/main/java/org/eclipse/mosaic/app/tutorial/vehicle/PerceptionApp.java index aabc33d52..057b3838d 100644 --- a/app/tutorials/example-applications/src/main/java/org/eclipse/mosaic/app/tutorial/vehicle/PerceptionApp.java +++ b/app/tutorials/example-applications/src/main/java/org/eclipse/mosaic/app/tutorial/vehicle/PerceptionApp.java @@ -18,7 +18,7 @@ import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.SimplePerceptionConfiguration; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.DistanceModifier; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.PositionErrorModifier; -import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.SimpleOcclusionModifier; +import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.SimpleOcclusion; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.VehicleObject; import org.eclipse.mosaic.fed.application.app.AbstractApplication; import org.eclipse.mosaic.fed.application.app.api.VehicleApplication; @@ -66,7 +66,7 @@ public void onStartup() { private void enablePerceptionModule() { // filter to emulate occlusion - SimpleOcclusionModifier simpleOcclusionModifier = new SimpleOcclusionModifier(3, 5); + SimpleOcclusion simpleOcclusion = new SimpleOcclusion(3, 5); // filter to reduce perception probability based on distance to ego vehicle DistanceModifier distanceModifier = new DistanceModifier(getRandom(), 0.0); // filter adding noise to longitudinal and lateral @@ -74,7 +74,7 @@ private void enablePerceptionModule() { SimplePerceptionConfiguration perceptionModuleConfiguration = new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE) - .addModifiers(simpleOcclusionModifier, distanceModifier, positionErrorModifier) + .addModifiers(simpleOcclusion, distanceModifier, positionErrorModifier) .build(); getOs().getPerceptionModule().enable(perceptionModuleConfiguration); } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusion.java similarity index 88% rename from fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java rename to fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusion.java index 0eb6582c5..70d8f1752 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusionModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusion.java @@ -19,7 +19,6 @@ import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.SpatialObject; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.SpatialObjectBoundingBox; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.TrafficLightObject; -import org.eclipse.mosaic.lib.math.MathUtils; import org.eclipse.mosaic.lib.math.Vector3d; import org.eclipse.mosaic.lib.math.VectorUtils; import org.eclipse.mosaic.lib.spatial.Edge; @@ -28,7 +27,7 @@ import java.util.List; import java.util.stream.Collectors; -public class BoundingBoxOcclusionModifier implements PerceptionModifier { +public class BoundingBoxOcclusion implements PerceptionModifier { private final Vector3d intersectionResult = new Vector3d(); /** @@ -46,29 +45,30 @@ public class BoundingBoxOcclusionModifier implements PerceptionModifier { private final int detectionThreshold; /** - * Default constructor for the {@link BoundingBoxOcclusionModifier}. + * Default constructor for the {@link BoundingBoxOcclusion}. + * Uses {@link #pointsPerSide} = 2 and {@link #detectionThreshold} = 2 as default values. */ - public BoundingBoxOcclusionModifier() { + public BoundingBoxOcclusion() { this.pointsPerSide = 2; this.detectionThreshold = 2; } /** - * Constructor for {@link BoundingBoxOcclusionModifier}, validates and sets + * Constructor for {@link BoundingBoxOcclusion}, validates and sets * the parameters {@link #pointsPerSide} and {@link #detectionThreshold}. * - * @param pointsPerSide the amount of points that will be evaluated per object (corners count towards 2 edges) + * @param pointsPerSide the number of points that will be evaluated per object side (corners count towards 2 edges) * @param detectionThreshold how many points have to be visible in order for an object to be treated as detected */ - public BoundingBoxOcclusionModifier(int pointsPerSide, int detectionThreshold) { + public BoundingBoxOcclusion(int pointsPerSide, int detectionThreshold) { if (pointsPerSide < 2) { - throw new RuntimeException("Need at least 2 points per edge, meaning every corner will be checked for occlusion."); + throw new IllegalArgumentException("Need at least 2 points per edge, meaning every corner will be checked for occlusion."); } if (detectionThreshold < 1) { - throw new RuntimeException("At least one point has to be checked for occlusion, else no objects will be occluded"); + throw new IllegalArgumentException("At least one point has to be checked for occlusion, else no objects will be occluded"); } if (detectionThreshold > pointsPerSide * 4 - 4) { - throw new RuntimeException("The detection threshold exceeds the number of points evaluated per object"); + throw new IllegalArgumentException("The detection threshold exceeds the number of points evaluated per object"); } this.pointsPerSide = pointsPerSide; this.detectionThreshold = detectionThreshold; diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/SimpleOcclusionModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/SimpleOcclusion.java similarity index 96% rename from fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/SimpleOcclusionModifier.java rename to fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/SimpleOcclusion.java index 6dd3920bf..60336fe05 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/SimpleOcclusionModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/SimpleOcclusion.java @@ -36,7 +36,7 @@ * Additionally, a linear function between {@link #minDetectionAngle} and {@link #maxDetectionAngle} * is fitted, which makes it necessary for further vehicles to have a larger "free" angle. */ -public class SimpleOcclusionModifier implements PerceptionModifier { +public class SimpleOcclusion implements PerceptionModifier { /** * The "free" angle required by the closest vehicle. [rad] @@ -49,12 +49,12 @@ public class SimpleOcclusionModifier implements PerceptionModifier { private final double maxDetectionAngle; /** - * Constructor for the {@link SimpleOcclusionModifier}. + * Constructor for the {@link SimpleOcclusion}. * * @param minDetectionAngle the "free" angle that will be required by closest vehicles [degree] * @param maxDetectionAngle the "free" angle that will be required by furthest vehicles [degree] */ - public SimpleOcclusionModifier(double minDetectionAngle, double maxDetectionAngle) { + public SimpleOcclusion(double minDetectionAngle, double maxDetectionAngle) { Validate.isTrue(minDetectionAngle > 0 && maxDetectionAngle >= minDetectionAngle, "Angles have to be larger than 0 and maxDetectionAngle >= minDetectionAngle."); this.minDetectionAngle = Math.toRadians(minDetectionAngle); diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusionModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusion.java similarity index 95% rename from fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusionModifier.java rename to fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusion.java index 10f90e3f3..8c8966639 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusionModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusion.java @@ -31,16 +31,16 @@ * The check for hidden vehicles is done by finding intersection of vectors between * ego and all other vehicles. and all walls in its vicinity. */ -public class WallOcclusionModifier implements PerceptionModifier { +public class WallOcclusion implements PerceptionModifier { private final int requiredVisibleVehicleCorners; private final Vector3d intersectionResult = new Vector3d(); - public WallOcclusionModifier() { + public WallOcclusion() { requiredVisibleVehicleCorners = 2; } - public WallOcclusionModifier(int requiredVisibleVehicleCorners) { + public WallOcclusion(int requiredVisibleVehicleCorners) { this.requiredVisibleVehicleCorners = requiredVisibleVehicleCorners; } diff --git a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java index d57a62d6a..b30336b73 100644 --- a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java +++ b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java @@ -28,11 +28,11 @@ import org.eclipse.mosaic.fed.application.ambassador.SimulationKernelRule; import org.eclipse.mosaic.fed.application.ambassador.navigation.CentralNavigationComponent; import org.eclipse.mosaic.fed.application.ambassador.simulation.VehicleUnit; -import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.BoundingBoxOcclusionModifier; +import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.BoundingBoxOcclusion; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.DistanceModifier; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.PositionErrorModifier; -import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.SimpleOcclusionModifier; -import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.WallOcclusionModifier; +import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.SimpleOcclusion; +import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.WallOcclusion; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.TrafficObjectIndex; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.SpatialObject; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.VehicleObject; @@ -135,7 +135,7 @@ public void setup() { @Test public void testOcclusionModifier() { - SimpleOcclusionModifier occlusionModifier = new SimpleOcclusionModifier(3, 10); + SimpleOcclusion occlusionModifier = new SimpleOcclusion(3, 10); simplePerceptionModule.enable( new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(occlusionModifier).build() ); @@ -181,7 +181,7 @@ public void testWallOcclusionModifier() { ); doReturn(surroundingWalls).when(simplePerceptionModule).getSurroundingWalls(); - WallOcclusionModifier occlusionModifier = new WallOcclusionModifier(); + WallOcclusion occlusionModifier = new WallOcclusion(); simplePerceptionModule.enable( new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(occlusionModifier).build() ); @@ -231,16 +231,16 @@ public void testIndexedObjectsNotChanged() { @Test public void testBoundingBoxOcclusionModifier() { - BoundingBoxOcclusionModifier boundingBoxOcclusionModifier = new BoundingBoxOcclusionModifier(); + BoundingBoxOcclusion boundingBoxOcclusion = new BoundingBoxOcclusion(); simplePerceptionModule.enable( - new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(boundingBoxOcclusionModifier).build() + new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(boundingBoxOcclusion).build() ); List perceivedVehicles = simplePerceptionModule.getPerceivedVehicles(); // create a modifier with more point but same detection threshold -> should result in more detections - BoundingBoxOcclusionModifier boundingBoxOcclusionModifierCustomParams = new BoundingBoxOcclusionModifier(5, 2); + BoundingBoxOcclusion boundingBoxOcclusionCustomParams = new BoundingBoxOcclusion(5, 2); simplePerceptionModule.enable( new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE) - .addModifier(boundingBoxOcclusionModifierCustomParams).build() + .addModifier(boundingBoxOcclusionCustomParams).build() ); List perceivedVehiclesCustomModifier = simplePerceptionModule.getPerceivedVehicles(); From 1146d2e51c22189a80f5bb96f1b77e739c4531e6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Wed, 30 Aug 2023 14:40:10 +0200 Subject: [PATCH 08/17] fix(perception): working on PR-review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * improved a lot of documentation * improved the place where bounding boxes are calculated * extracted method in BoundingBoxOcclusion Signed-off-by: Moritz Schweppenhäuser --- .../perception/SumoPerceptionModule.java | 1 - .../errormodels/BoundingBoxOcclusion.java | 64 ++++++++------ .../perception/errormodels/WallOcclusion.java | 9 -- .../index/objects/PointBoundingBox.java | 13 +-- .../objects/SpatialObjectBoundingBox.java | 9 +- .../index/objects/TrafficLightObject.java | 6 +- .../index/objects/VehicleBoundingBox.java | 85 ++++++++----------- .../index/objects/VehicleObject.java | 12 +-- .../index/providers/VehicleIndex.java | 6 +- .../perception/PerceptionModifierTest.java | 1 + 10 files changed, 101 insertions(+), 105 deletions(-) diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/SumoPerceptionModule.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/SumoPerceptionModule.java index 2138783b0..87078ebda 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/SumoPerceptionModule.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/SumoPerceptionModule.java @@ -56,7 +56,6 @@ List getVehiclesInRange() { .setSpeed(v.getSpeed()) .setHeading(v.getHeading()) .setDimensions(v.getLength(), v.getWidth(), v.getHeight()) - .resetBoundingBox() ).collect(Collectors.toList()); } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusion.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusion.java index 70d8f1752..1043bc5d9 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusion.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusion.java @@ -81,39 +81,23 @@ public List apply(PerceptionModuleOwner owner, List List occludingObjects = spatialObjects.stream() .filter(object -> !object.getId().equals(owner.getId())) .collect(Collectors.toList()); - for (T spatialObject : spatialObjects) { - if (spatialObject instanceof TrafficLightObject) { // Traffic Lights are treated to not be occluded - newObjects.add(spatialObject); + Vector3d egoPosition = owner.getVehicleData().getProjectedPosition().toVector3d(); + for (T objectToEvaluate : spatialObjects) { + if (objectToEvaluate instanceof TrafficLightObject) { // Traffic Lights are treated to not be occluded + newObjects.add(objectToEvaluate); continue; } - List pointsToEvaluate = createPointsToEvaluate(spatialObject); + List pointsToEvaluate = createPointsToEvaluate(objectToEvaluate); final int requiredVisiblePoints = pointsToEvaluate.size() == 1 ? 1 : detectionThreshold; int numberOfPointsVisible = 0; for (Vector3d point : pointsToEvaluate) { - boolean pointOccluded = false; - boundingBoxLoop: - for (T occludingObject : occludingObjects) { - if (occludingObject.getId().equals(spatialObject.getId())) { - continue; // cannot be occluded by itself - } - SpatialObjectBoundingBox boundingBox = occludingObject.getBoundingBox(); - // SpatialObjects with PointBoundingBoxes won't occlude anything, as they have no edges defined - for (Edge side : boundingBox.getAllEdges()) { - boolean isOccluded = VectorUtils.computeXZEdgeIntersectionPoint( - owner.getVehicleData().getProjectedPosition().toVector3d(), - point, side.a, side.b, intersectionResult - ); - if (isOccluded) { - pointOccluded = true; - break boundingBoxLoop; - } - } - } - if (!pointOccluded) { + boolean pointVisible = isVisible(egoPosition, point, objectToEvaluate.getId(), occludingObjects); + if (pointVisible) { // increment visible counter numberOfPointsVisible++; } + // if the required number of points is visible, we don't need to evaluate more if (numberOfPointsVisible == requiredVisiblePoints) { - newObjects.add(spatialObject); + newObjects.add(objectToEvaluate); break; } } @@ -121,6 +105,35 @@ public List apply(PerceptionModuleOwner owner, List return newObjects; } + /** + * Method to evaluate whether a point is visible by any edge spanned by any other bounding box of any other vehicle. + * + * @param egoPosition position of the ego vehicle + * @param pointToEvaluate the point that should be checked for occlusion + * @param objectId id that the point belongs to (required for points not to be occluded by the same vehicle) + * @param occludingObjects all objects that potentially occlude the vehicle + * @return {@code true} if the point is visible, else {@code false} + */ + private boolean isVisible(Vector3d egoPosition, Vector3d pointToEvaluate, String objectId, List occludingObjects) { + for (T occludingObject : occludingObjects) { + if (occludingObject.getId().equals(objectId)) { + continue; // cannot be occluded by itself + } + SpatialObjectBoundingBox boundingBox = occludingObject.getBoundingBox(); + // SpatialObjects with PointBoundingBoxes won't occlude anything, as they have no edges defined + for (Edge side : boundingBox.getAllEdges()) { + boolean isOccluded = VectorUtils.computeXZEdgeIntersectionPoint( + egoPosition, + pointToEvaluate, side.a, side.b, intersectionResult + ); + if (isOccluded) { + return false; + } + } + } + return true; + } + /** * Creates a list of all points that shall be tested for occlusion. If {@link #pointsPerSide} is set to a value larger than 2, * each side will have additional equidistant points added. @@ -134,6 +147,7 @@ public List apply(PerceptionModuleOwner owner, List * | | * x-----o-----y * + * * @param spatialObject a {@link SpatialObject} for which the occlusion should be evaluated */ private List createPointsToEvaluate(T spatialObject) { diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusion.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusion.java index 8c8966639..1d1c9f661 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusion.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusion.java @@ -33,17 +33,8 @@ */ public class WallOcclusion implements PerceptionModifier { - private final int requiredVisibleVehicleCorners; private final Vector3d intersectionResult = new Vector3d(); - public WallOcclusion() { - requiredVisibleVehicleCorners = 2; - } - - public WallOcclusion(int requiredVisibleVehicleCorners) { - this.requiredVisibleVehicleCorners = requiredVisibleVehicleCorners; - } - @Override public List apply(PerceptionModuleOwner owner, List spatialObjects) { if (spatialObjects.isEmpty()) { diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/PointBoundingBox.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/PointBoundingBox.java index 8cae105aa..a5d2028e3 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/PointBoundingBox.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/PointBoundingBox.java @@ -19,24 +19,27 @@ import org.eclipse.mosaic.lib.spatial.Edge; import java.util.ArrayList; +import java.util.Collections; import java.util.List; /** - * This bounding box represents + * This bounding box can be used for objects that should be treated to have a single-point position and not span into any dimensions. */ public class PointBoundingBox implements SpatialObjectBoundingBox { /** - * A one element list, defining the position of a {@link SpatialObject} as the only corner. + * A singleton list, defining the position of a {@link SpatialObject} as the only corner. + * This point is in global coordinates. */ - private final List allCorners = new ArrayList<>(); + private final List allCorners; /** * An empty list as a no bounding box is spanned by a single point. */ - private final List> allEdges = new ArrayList<>(); + private final List> allEdges; public PointBoundingBox(Vector3d position) { - allCorners.add(position); + allCorners = Collections.singletonList(position); + allEdges = Collections.emptyList(); } @Override diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObjectBoundingBox.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObjectBoundingBox.java index b56d43e76..b0cc8c4e6 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObjectBoundingBox.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObjectBoundingBox.java @@ -20,15 +20,20 @@ import java.util.List; +/** + * This interface shall be implemented by all classes representing the bounding box of a {@link SpatialObject}. + * All points are returned in the global coordinate system as {@link Vector3d}. + */ public interface SpatialObjectBoundingBox { /** - * Returns all corners spanning the 2D bounding box of a {@link SpatialObject}. + * Returns all corners spanning the 2D bounding box of a {@link SpatialObject} as global coordinates. */ List getAllCorners(); /** - * Returns all sides spanning the 2D bounding box of a {@link SpatialObject}. + * Returns all sides spanning the 2D bounding box of a {@link SpatialObject} as {@link Edge edges} in the global + * coordinate system. */ List> getAllEdges(); } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java index 864cae7a9..1cb20d4df 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java @@ -50,14 +50,14 @@ public TrafficLightObject(String id) { public TrafficLightObject setPosition(CartesianPoint position) { cartesianPosition.set(position); position.toVector3d(this); - if (boundingBox == null) { - boundingBox = new PointBoundingBox(this); - } return this; } @Override public SpatialObjectBoundingBox getBoundingBox() { + if (boundingBox == null) { + boundingBox = new PointBoundingBox(this); + } return boundingBox; } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleBoundingBox.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleBoundingBox.java index f043d753d..44cb2359c 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleBoundingBox.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleBoundingBox.java @@ -32,19 +32,20 @@ * | | * frontLeftCorner----------leftEdge-----------backLeftCorner * + * All points are in global coordinates. */ public class VehicleBoundingBox implements SpatialObjectBoundingBox { private final List allCorners; - private final Vector3d frontRightCorner; - private final Vector3d backRightCorner; - private final Vector3d backLeftCorner; - private final Vector3d frontLeftCorner; + public final Vector3d frontRightCorner; + public final Vector3d backRightCorner; + public final Vector3d backLeftCorner; + public final Vector3d frontLeftCorner; private final List> allEdges; - private final Edge frontEdge; - private final Edge rightEdge; - private final Edge backEdge; - private final Edge leftEdge; + public final Edge frontEdge; + public final Edge rightEdge; + public final Edge backEdge; + public final Edge leftEdge; private VehicleBoundingBox(Vector3d frontRightCorner, Vector3d backRightCorner, Vector3d backLeftCorner, Vector3d frontLeftCorner) { this.frontRightCorner = frontRightCorner; @@ -59,19 +60,39 @@ private VehicleBoundingBox(Vector3d frontRightCorner, Vector3d backRightCorner, allEdges = Lists.newArrayList(frontEdge, rightEdge, backEdge, leftEdge); } + /** + * This method is used to generate a {@link VehicleBoundingBox}-object given its position, dimensions, and heading. + * Note: We assume that the position is defined in the middle of the front bumper and the heading is defined relative to + * this position. + * + * @param vehicleObject the {@link VehicleObject} containing its dimensions and heading + * @return the build {@link VehicleBoundingBox} + */ public static VehicleBoundingBox createFromVehicleObject(VehicleObject vehicleObject) { Vector3d headingVector = new Vector3d(); VectorUtils.getDirectionVectorFromHeading(vehicleObject.getHeading(), headingVector).norm(); double length = vehicleObject.getLength(); double halfWidth = vehicleObject.getWidth() / 2; - Vector3d pointA = new Vector3d(headingVector).rotateDeg(90d, VectorUtils.UP); - pointA = pointA.multiply(halfWidth).add(vehicleObject); - Vector3d pointD = new Vector3d(headingVector).rotateDeg(-90d, VectorUtils.UP); - pointD = pointD.multiply(halfWidth).add(vehicleObject); - Vector3d pointB = new Vector3d(headingVector).multiply(-1d); - pointB = pointB.multiply(length).add(pointA); - Vector3d pointC = new Vector3d(headingVector).multiply(-1d); - pointC = pointC.multiply(length).add(pointD); + // we get the two front corners by rotating the normalized heading vector by +/- 90° and multiplying it by half the vehicle width + // and then add it on the vehicle position (rotation -> translation) + Vector3d pointA = new Vector3d(headingVector) + .rotateDeg(90d, VectorUtils.UP) // rotate to 90° to the right + .multiply(halfWidth) // adjust the length of the vector to half the vehicles' width + .add(vehicleObject); // add on top of the vehicle position + Vector3d pointD = new Vector3d(headingVector) + .rotateDeg(-90d, VectorUtils.UP) // rotate to 90° to the left + .multiply(halfWidth) // adjust the length of the vector to half the vehicles' width + .add(vehicleObject); // add on top of the vehicle position + // similarly to the front corners, we get the back corners by rotating the heading vector by 180° (i.e., multiplying by -1) + // then set the appropriate length using the vehicle length and finally add it to the respective corners (rotation -> translation) + Vector3d pointB = new Vector3d(headingVector) + .multiply(-1d) // rotate heading vector by 180° + .multiply(length) // adjust the length of the vector to vehicle length + .add(pointA); // add on top of the front right corner + Vector3d pointC = new Vector3d(headingVector) + .multiply(-1d) // rotate heading vector by 180° + .multiply(length) // adjust the length of the vector to vehicle length + .add(pointD); // add on top of the front left corner return new VehicleBoundingBox(pointA, pointB, pointC, pointD); } @@ -84,36 +105,4 @@ public List getAllCorners() { public List> getAllEdges() { return allEdges; } - - public Vector3d getFrontRightCorner() { - return frontRightCorner; - } - - public Vector3d getBackRightCorner() { - return backRightCorner; - } - - public Vector3d getBackLeftCorner() { - return backLeftCorner; - } - - public Vector3d getFrontLeftCorner() { - return frontLeftCorner; - } - - public Edge getFrontEdge() { - return frontEdge; - } - - public Edge getRightEdge() { - return rightEdge; - } - - public Edge getBackEdge() { - return backEdge; - } - - public Edge getLeftEdge() { - return leftEdge; - } } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java index 3d3979768..0dc431ddf 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java @@ -64,6 +64,7 @@ public VehicleObject(String id) { public VehicleObject setPosition(CartesianPoint position) { cartesianPosition.set(position); position.toVector3d(this); + this.boundingBox = null; return this; } @@ -93,6 +94,7 @@ public double getSpeed() { public VehicleObject setHeading(double heading) { this.heading = heading; + this.boundingBox = null; return this; } @@ -104,6 +106,7 @@ public VehicleObject setDimensions(double length, double width, double height) { this.length = length; this.width = width; this.height = height; + this.boundingBox = null; return this; } @@ -119,11 +122,6 @@ public double getHeight() { return height; } - public VehicleObject resetBoundingBox() { - boundingBox = null; - return this; - } - /** * Returns the bounding box for a spatial object if requested. * Calculation is only triggered if bounding box is requested. @@ -173,8 +171,6 @@ public VehicleObject copy() { .setSpeed(getSpeed()) .setEdgeAndLane(getEdgeId(), getLaneIndex()) .setDimensions(getLength(), getWidth(), getHeight()) - .setPosition(getProjectedPosition()) - .resetBoundingBox(); - + .setPosition(getProjectedPosition()); } } \ No newline at end of file diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/providers/VehicleIndex.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/providers/VehicleIndex.java index 677783670..b4fb34f6f 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/providers/VehicleIndex.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/providers/VehicleIndex.java @@ -61,8 +61,7 @@ VehicleObject addOrGetVehicle(VehicleData vehicleData) { vehicleObject = new VehicleObject(vehicleId) .setHeading(vehicleData.getHeading()) .setSpeed(vehicleData.getSpeed()) - .setPosition(vehicleData.getProjectedPosition()) - .resetBoundingBox(); + .setPosition(vehicleData.getProjectedPosition()); VehicleType vehicleType = registeredVehicleTypes.get(vehicleId); if (vehicleType != null) { // vehicles with no cached type will have (0,0,0) dimensions vehicleObject.setDimensions( @@ -157,8 +156,7 @@ public void updateVehicles(Iterable vehiclesToUpdate) { VehicleObject vehicleObject = addOrGetVehicle(v) .setHeading(v.getHeading()) .setSpeed(v.getSpeed()) - .setPosition(vehiclePosition) - .resetBoundingBox(); + .setPosition(vehiclePosition); if (v.getRoadPosition() != null) { vehicleObject.setEdgeAndLane(v.getRoadPosition().getConnectionId(), v.getRoadPosition().getLaneIndex()); } diff --git a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java index b30336b73..735bccaa7 100644 --- a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java +++ b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java @@ -246,6 +246,7 @@ public void testBoundingBoxOcclusionModifier() { if (PRINT_POSITIONS) { printBoundingBoxes(perceivedVehicles); + System.out.println(); printBoundingBoxes(perceivedVehiclesCustomModifier); } assertTrue("The occlusion filter should remove vehicles", VEHICLE_AMOUNT > perceivedVehicles.size()); From b5493fac7360520b2dcffd87ba8f36ee54b96401 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Wed, 30 Aug 2023 14:47:48 +0200 Subject: [PATCH 09/17] fix(perception): working on PR-review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * adjusted names of perception modifiers Signed-off-by: Moritz Schweppenhäuser --- .../app/tutorial/vehicle/PerceptionApp.java | 10 +++++----- ...DistanceModifier.java => DistanceFilter.java} | 4 ++-- ...nErrorModifier.java => PositionModifier.java} | 7 +++---- .../perception/PerceptionModifierTest.java | 16 ++++++++-------- 4 files changed, 18 insertions(+), 19 deletions(-) rename fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/{DistanceModifier.java => DistanceFilter.java} (96%) rename fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/{PositionErrorModifier.java => PositionModifier.java} (93%) diff --git a/app/tutorials/example-applications/src/main/java/org/eclipse/mosaic/app/tutorial/vehicle/PerceptionApp.java b/app/tutorials/example-applications/src/main/java/org/eclipse/mosaic/app/tutorial/vehicle/PerceptionApp.java index 057b3838d..63fed3562 100644 --- a/app/tutorials/example-applications/src/main/java/org/eclipse/mosaic/app/tutorial/vehicle/PerceptionApp.java +++ b/app/tutorials/example-applications/src/main/java/org/eclipse/mosaic/app/tutorial/vehicle/PerceptionApp.java @@ -16,8 +16,8 @@ package org.eclipse.mosaic.app.tutorial.vehicle; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.SimplePerceptionConfiguration; -import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.DistanceModifier; -import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.PositionErrorModifier; +import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.DistanceFilter; +import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.PositionModifier; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.SimpleOcclusion; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.VehicleObject; import org.eclipse.mosaic.fed.application.app.AbstractApplication; @@ -68,13 +68,13 @@ private void enablePerceptionModule() { // filter to emulate occlusion SimpleOcclusion simpleOcclusion = new SimpleOcclusion(3, 5); // filter to reduce perception probability based on distance to ego vehicle - DistanceModifier distanceModifier = new DistanceModifier(getRandom(), 0.0); + DistanceFilter distanceFilter = new DistanceFilter(getRandom(), 0.0); // filter adding noise to longitudinal and lateral - PositionErrorModifier positionErrorModifier = new PositionErrorModifier(getRandom()); + PositionModifier positionModifier = new PositionModifier(getRandom()); SimplePerceptionConfiguration perceptionModuleConfiguration = new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE) - .addModifiers(simpleOcclusion, distanceModifier, positionErrorModifier) + .addModifiers(simpleOcclusion, distanceFilter, positionModifier) .build(); getOs().getPerceptionModule().enable(perceptionModuleConfiguration); } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/DistanceModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/DistanceFilter.java similarity index 96% rename from fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/DistanceModifier.java rename to fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/DistanceFilter.java index 88912b0ce..5a0c82a29 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/DistanceModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/DistanceFilter.java @@ -34,7 +34,7 @@ * This modifier can also be configured using {@link #offset}, which allows adjusting the * stochastic component to allow for more or less perceptions. */ -public class DistanceModifier implements PerceptionModifier { +public class DistanceFilter implements PerceptionModifier { private final RandomNumberGenerator rng; @@ -44,7 +44,7 @@ public class DistanceModifier implements PerceptionModifier { */ private final double offset; - public DistanceModifier(RandomNumberGenerator rng, double offset) { + public DistanceFilter(RandomNumberGenerator rng, double offset) { Validate.isTrue(offset >= -1 && offset <= 1, "The offset has to be between -1 and 1."); this.rng = rng; this.offset = offset; diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionErrorModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionModifier.java similarity index 93% rename from fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionErrorModifier.java rename to fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionModifier.java index d8e95cbc7..bceb99b9e 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionErrorModifier.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/PositionModifier.java @@ -17,7 +17,6 @@ import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.PerceptionModuleOwner; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.SpatialObject; -import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.VehicleObject; import org.eclipse.mosaic.lib.math.RandomNumberGenerator; import org.eclipse.mosaic.lib.math.Vector3d; import org.eclipse.mosaic.lib.math.VectorUtils; @@ -33,7 +32,7 @@ * To calculate these, all points are transformed into a coordinate system relative to the position and * orientation of the ego vehicle, and after error calculation re-transformed. */ -public class PositionErrorModifier implements PerceptionModifier { +public class PositionModifier implements PerceptionModifier { /** * Default standard deviation for longitudinal error. (Taken from referenced source) */ @@ -59,7 +58,7 @@ public class PositionErrorModifier implements PerceptionModifier { * * @param rng {@link RandomNumberGenerator} to draw gaussian variables from */ - public PositionErrorModifier(RandomNumberGenerator rng) { + public PositionModifier(RandomNumberGenerator rng) { this.rng = rng; this.longitudinalStandardDeviation = SIGMA_LON_OFFSET; this.lateralStandardDeviation = SIGMA_LAT_OFFSET; @@ -72,7 +71,7 @@ public PositionErrorModifier(RandomNumberGenerator rng) { * @param longitudinalStandardDeviation sigma for longitudinal error * @param lateralStandardDeviation sigma for lateral error */ - public PositionErrorModifier(RandomNumberGenerator rng, double longitudinalStandardDeviation, double lateralStandardDeviation) { + public PositionModifier(RandomNumberGenerator rng, double longitudinalStandardDeviation, double lateralStandardDeviation) { this.rng = rng; this.longitudinalStandardDeviation = longitudinalStandardDeviation; this.lateralStandardDeviation = lateralStandardDeviation; diff --git a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java index 735bccaa7..8b04d65c0 100644 --- a/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java +++ b/fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java @@ -29,8 +29,8 @@ import org.eclipse.mosaic.fed.application.ambassador.navigation.CentralNavigationComponent; import org.eclipse.mosaic.fed.application.ambassador.simulation.VehicleUnit; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.BoundingBoxOcclusion; -import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.DistanceModifier; -import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.PositionErrorModifier; +import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.DistanceFilter; +import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.PositionModifier; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.SimpleOcclusion; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.WallOcclusion; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.TrafficObjectIndex; @@ -148,9 +148,9 @@ public void testOcclusionModifier() { @Test public void testDistanceErrorModifier() { - DistanceModifier distanceModifier = new DistanceModifier(rng, 0); + DistanceFilter distanceFilter = new DistanceFilter(rng, 0); simplePerceptionModule.enable( - new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(distanceModifier).build() + new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(distanceFilter).build() ); List perceivedVehicles = simplePerceptionModule.getPerceivedVehicles(); @@ -162,9 +162,9 @@ public void testDistanceErrorModifier() { @Test public void testPositionErrorModifier() { - PositionErrorModifier positionErrorModifier = new PositionErrorModifier(rng, 1, 1); + PositionModifier positionModifier = new PositionModifier(rng, 1, 1); simplePerceptionModule.enable( - new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(positionErrorModifier).build() + new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(positionModifier).build() ); List perceivedVehicles = simplePerceptionModule.getPerceivedVehicles(); @@ -200,9 +200,9 @@ public void testWallOcclusionModifier() { @Test public void testIndexedObjectsNotChanged() { - PositionErrorModifier positionErrorModifier = new PositionErrorModifier(rng, 1, 1); + PositionModifier positionModifier = new PositionModifier(rng, 1, 1); simplePerceptionModule.enable( - new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(positionErrorModifier).build() + new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(positionModifier).build() ); // collect positions of perceived objects BEFORE applying modifier From c602258d5b8537582fd3fbec17b71528b796fff8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Mon, 4 Sep 2023 09:47:18 +0200 Subject: [PATCH 10/17] cleanup(mapping): fixed check style warnings MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Moritz Schweppenhäuser --- .../fed/mapping/ambassador/weighting/FixedOrderSelector.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/fed/mosaic-mapping/src/main/java/org/eclipse/mosaic/fed/mapping/ambassador/weighting/FixedOrderSelector.java b/fed/mosaic-mapping/src/main/java/org/eclipse/mosaic/fed/mapping/ambassador/weighting/FixedOrderSelector.java index 8eec53577..dca22e6a4 100644 --- a/fed/mosaic-mapping/src/main/java/org/eclipse/mosaic/fed/mapping/ambassador/weighting/FixedOrderSelector.java +++ b/fed/mosaic-mapping/src/main/java/org/eclipse/mosaic/fed/mapping/ambassador/weighting/FixedOrderSelector.java @@ -59,7 +59,7 @@ public double getWeight() { private int totalSelections = 0; /** - * Number of objects already generated without counting the same object twice + * Number of objects already generated without counting the same object twice. */ private int totalDistinctSelections = 0; @@ -120,7 +120,8 @@ public T nextItem() { StochasticSelector> firstItemSelector = init(); selectedItem = firstItemSelector.nextItem(); } else { - Comparator> compareBySelectionToWeightRatio = Comparator.comparingDouble((item) -> (item.selections / (double) totalSelections) - item.getWeight()); + Comparator> compareBySelectionToWeightRatio = + Comparator.comparingDouble((item) -> (item.selections / (double) totalSelections) - item.getWeight()); Comparator> compareByPriority = Comparator.comparingInt((item) -> item.priority); // choose item which has been selected the least according to its weight selectedItem = items.stream() From ab4e5112cbf988da3d70f2fc913b07c738689484 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Mon, 4 Sep 2023 09:57:15 +0200 Subject: [PATCH 11/17] cleanup(apps): fixing spotbugs warning in example app MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Moritz Schweppenhäuser --- .../examples/commuter/SimpleCommuterApp.java | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/app/tutorials/example-applications/src/main/java/org/eclipse/mosaic/app/examples/commuter/SimpleCommuterApp.java b/app/tutorials/example-applications/src/main/java/org/eclipse/mosaic/app/examples/commuter/SimpleCommuterApp.java index d7fcc876b..e6bddae3b 100644 --- a/app/tutorials/example-applications/src/main/java/org/eclipse/mosaic/app/examples/commuter/SimpleCommuterApp.java +++ b/app/tutorials/example-applications/src/main/java/org/eclipse/mosaic/app/examples/commuter/SimpleCommuterApp.java @@ -33,6 +33,8 @@ import org.eclipse.mosaic.lib.util.scheduling.EventProcessor; import org.eclipse.mosaic.rti.TIME; +import org.apache.commons.lang3.builder.EqualsBuilder; + import javax.annotation.Nonnull; import javax.annotation.Nullable; @@ -166,6 +168,24 @@ GeoPoint getTargetPosition() { return homePosition; } + @Override + public boolean equals(Object obj) { + if (obj == null) { + return false; + } + if (obj == this) { + return true; + } + if (obj.getClass() != getClass()) { + return false; + } + DriveBackEvent event = (DriveBackEvent) obj; + return new EqualsBuilder() + .appendSuper(super.equals(obj)) + .append(this.currentPosition, event.currentPosition) + .append(this.homePosition, event.homePosition) + .isEquals(); + } } /** From ee82c00dade38accec466b5c63d78efaa45b0b1f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Mon, 4 Sep 2023 10:07:31 +0200 Subject: [PATCH 12/17] cleanup(perception): fixing spotbugs warning in perception objects MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Moritz Schweppenhäuser --- .../perception/CentralPerceptionComponent.java | 18 ++++++------------ .../index/objects/TrafficLightObject.java | 12 ++++++++++++ .../index/objects/VehicleObject.java | 15 +++++++++++++++ 3 files changed, 33 insertions(+), 12 deletions(-) diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/CentralPerceptionComponent.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/CentralPerceptionComponent.java index 047133224..d07fc47a5 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/CentralPerceptionComponent.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/CentralPerceptionComponent.java @@ -226,8 +226,7 @@ static class MonitoringTrafficObjectIndexProvider extends TrafficObjectIndex { @Override public List getVehiclesInRange(PerceptionModel searchRange) { try (PerformanceMonitor.Measurement m = monitor.start("search-vehicle")) { - m.setProperties(getNumberOfVehicles(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()) - .restart(); + m.setProperties(getNumberOfVehicles(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); return super.getVehiclesInRange(searchRange); } } @@ -235,8 +234,7 @@ public List getVehiclesInRange(PerceptionModel searchRange) { @Override public void removeVehicles(Iterable vehiclesToRemove) { try (PerformanceMonitor.Measurement m = monitor.start("remove-vehicle")) { - m.setProperties(getNumberOfVehicles(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()) - .restart(); + m.setProperties(getNumberOfVehicles(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); super.removeVehicles(vehiclesToRemove); } } @@ -244,8 +242,7 @@ public void removeVehicles(Iterable vehiclesToRemove) { @Override public void updateVehicles(Iterable vehiclesToUpdate) { try (PerformanceMonitor.Measurement m = monitor.start("update-vehicle")) { - m.setProperties(getNumberOfVehicles(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()) - .restart(); + m.setProperties(getNumberOfVehicles(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); super.updateVehicles(vehiclesToUpdate); } } @@ -258,8 +255,7 @@ public int getNumberOfVehicles() { @Override public List getTrafficLightsInRange(PerceptionModel perceptionModel) { try (PerformanceMonitor.Measurement m = monitor.start("search-traffic-light")) { - m.setProperties(getNumberOfTrafficLights(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()) - .restart(); + m.setProperties(getNumberOfTrafficLights(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); return super.getTrafficLightsInRange(perceptionModel); } } @@ -272,8 +268,7 @@ public void addTrafficLightGroup(TrafficLightGroup trafficLightGroup) { @Override public void updateTrafficLights(Map trafficLightsToUpdate) { try (PerformanceMonitor.Measurement m = monitor.start("update-traffic-light")) { - m.setProperties(getNumberOfTrafficLights(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()) - .restart(); + m.setProperties(getNumberOfTrafficLights(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); super.updateTrafficLights(trafficLightsToUpdate); } } @@ -286,8 +281,7 @@ public int getNumberOfTrafficLights() { @Override public Collection> getSurroundingWalls(PerceptionModel perceptionModel) { try (PerformanceMonitor.Measurement m = monitor.start("search-walls")) { - m.setProperties(getNumberOfTrafficLights(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()) - .restart(); + m.setProperties(getNumberOfTrafficLights(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); return super.getSurroundingWalls(perceptionModel); } } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java index 1cb20d4df..025ac4292 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java @@ -19,6 +19,7 @@ import org.eclipse.mosaic.lib.objects.trafficlight.TrafficLightState; import org.apache.commons.lang3.builder.EqualsBuilder; +import org.apache.commons.lang3.builder.HashCodeBuilder; public class TrafficLightObject extends SpatialObject { /** @@ -118,6 +119,17 @@ public boolean equals(Object o) { .isEquals(); } + @Override + public int hashCode() { + return new HashCodeBuilder(5, 11) + .appendSuper(super.hashCode()) + .append(trafficLightGroupId) + .append(trafficLightState) + .append(incomingLane) + .append(outgoingLane) + .toHashCode(); + } + /** * Returns a hard copy of the {@link TrafficLightObject}, this should be used * when the data of a perceived traffic light is to be altered or stored in memory. diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java index 0dc431ddf..18173b06c 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java @@ -18,6 +18,7 @@ import org.eclipse.mosaic.lib.geo.CartesianPoint; import org.apache.commons.lang3.builder.EqualsBuilder; +import org.apache.commons.lang3.builder.HashCodeBuilder; import javax.annotation.Nullable; @@ -158,6 +159,20 @@ public boolean equals(Object o) { .isEquals(); } + @Override + public int hashCode() { + return new HashCodeBuilder(5, 11) + .appendSuper(super.hashCode()) + .append(speed) + .append(heading) + .append(edgeId) + .append(laneIndex) + .append(length) + .append(width) + .append(height) + .toHashCode(); + } + /** * Returns a hard copy of the {@link VehicleObject}, this should be used * when the data of a perceived vehicle is to be altered or stored in memory. From 188b5311a7ff22356ca3f0c49f3fc52625fdd719 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Mon, 4 Sep 2023 10:17:22 +0200 Subject: [PATCH 13/17] cleanup(perception): trying to fix spotbugs warning in perception objects MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Moritz Schweppenhäuser --- .../index/objects/PointBoundingBox.java | 35 +++++++++++++++++- .../objects/SpatialObjectBoundingBox.java | 5 ++- .../index/objects/TrafficLightObject.java | 6 ++- .../index/objects/VehicleBoundingBox.java | 37 +++++++++++++++++++ .../index/objects/VehicleObject.java | 5 +++ 5 files changed, 84 insertions(+), 4 deletions(-) diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/PointBoundingBox.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/PointBoundingBox.java index a5d2028e3..2a0a04ce6 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/PointBoundingBox.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/PointBoundingBox.java @@ -18,7 +18,9 @@ import org.eclipse.mosaic.lib.math.Vector3d; import org.eclipse.mosaic.lib.spatial.Edge; -import java.util.ArrayList; +import org.apache.commons.lang3.builder.EqualsBuilder; +import org.apache.commons.lang3.builder.HashCodeBuilder; + import java.util.Collections; import java.util.List; @@ -27,6 +29,8 @@ */ public class PointBoundingBox implements SpatialObjectBoundingBox { + private static final long serialVersionUID = 1L; + /** * A singleton list, defining the position of a {@link SpatialObject} as the only corner. * This point is in global coordinates. @@ -51,4 +55,33 @@ public List getAllCorners() { public List> getAllEdges() { return allEdges; } + + @Override + public boolean equals(Object o) { + if (this == o) { + return true; + } + + if (o == null || getClass() != o.getClass()) { + return false; + } + + PointBoundingBox that = (PointBoundingBox) o; + + return new EqualsBuilder() + .appendSuper(super.equals(o)) + .append(allCorners, that.allCorners) + .append(allEdges, that.allEdges) + .isEquals(); + } + + @Override + public int hashCode() { + return new HashCodeBuilder(5, 11) + .appendSuper(super.hashCode()) + .append(allCorners) + .append(allEdges) + .toHashCode(); + } + } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObjectBoundingBox.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObjectBoundingBox.java index b0cc8c4e6..64d9f4c7b 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObjectBoundingBox.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObjectBoundingBox.java @@ -18,13 +18,14 @@ import org.eclipse.mosaic.lib.math.Vector3d; import org.eclipse.mosaic.lib.spatial.Edge; +import java.io.Serializable; import java.util.List; /** * This interface shall be implemented by all classes representing the bounding box of a {@link SpatialObject}. * All points are returned in the global coordinate system as {@link Vector3d}. */ -public interface SpatialObjectBoundingBox { +public interface SpatialObjectBoundingBox extends Serializable { /** * Returns all corners spanning the 2D bounding box of a {@link SpatialObject} as global coordinates. @@ -32,7 +33,7 @@ public interface SpatialObjectBoundingBox { List getAllCorners(); /** - * Returns all sides spanning the 2D bounding box of a {@link SpatialObject} as {@link Edge edges} in the global + * Returns all sides spanning the 2D bounding box of a {@link SpatialObject} as {@link Edge edges} in the global * coordinate system. */ List> getAllEdges(); diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java index 025ac4292..3ea429800 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/TrafficLightObject.java @@ -22,6 +22,9 @@ import org.apache.commons.lang3.builder.HashCodeBuilder; public class TrafficLightObject extends SpatialObject { + + private static final long serialVersionUID = 1L; + /** * Id of the group the individual traffic light belongs to. */ @@ -41,7 +44,7 @@ public class TrafficLightObject extends SpatialObject { /** * The bounding box of a traffic light is represented by a single point. */ - private PointBoundingBox boundingBox; + private transient PointBoundingBox boundingBox; public TrafficLightObject(String id) { super(id); @@ -116,6 +119,7 @@ public boolean equals(Object o) { .append(trafficLightState, that.trafficLightState) .append(incomingLane, that.incomingLane) .append(outgoingLane, that.outgoingLane) + .append(boundingBox, that.boundingBox) .isEquals(); } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleBoundingBox.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleBoundingBox.java index 44cb2359c..dfa488712 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleBoundingBox.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleBoundingBox.java @@ -20,6 +20,8 @@ import org.eclipse.mosaic.lib.spatial.Edge; import com.google.common.collect.Lists; +import org.apache.commons.lang3.builder.EqualsBuilder; +import org.apache.commons.lang3.builder.HashCodeBuilder; import java.util.List; @@ -35,6 +37,9 @@ * All points are in global coordinates. */ public class VehicleBoundingBox implements SpatialObjectBoundingBox { + + private static final long serialVersionUID = 1L; + private final List allCorners; public final Vector3d frontRightCorner; public final Vector3d backRightCorner; @@ -105,4 +110,36 @@ public List getAllCorners() { public List> getAllEdges() { return allEdges; } + + @Override + public boolean equals(Object o) { + if (this == o) { + return true; + } + + if (o == null || getClass() != o.getClass()) { + return false; + } + + VehicleBoundingBox that = (VehicleBoundingBox) o; + + return new EqualsBuilder() + .appendSuper(super.equals(o)) + .append(frontRightCorner, that.frontRightCorner) + .append(backRightCorner, that.backRightCorner) + .append(backLeftCorner, that.backLeftCorner) + .append(frontLeftCorner, that.frontLeftCorner) + .append(allCorners, that.allCorners) + .append(allEdges, that.allEdges) + .isEquals(); + } + + @Override + public int hashCode() { + return new HashCodeBuilder(5, 11) + .appendSuper(super.hashCode()) + .append(allCorners) + .append(allEdges) + .toHashCode(); + } } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java index 18173b06c..5fe6bb869 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleObject.java @@ -24,6 +24,9 @@ public class VehicleObject extends SpatialObject { + private static final long serialVersionUID = 1L; + + /** * The current speed of the vehicle. [m/s] */ @@ -156,6 +159,7 @@ public boolean equals(Object o) { .append(length, that.length) .append(width, that.width) .append(height, that.height) + .append(boundingBox, that.boundingBox) .isEquals(); } @@ -170,6 +174,7 @@ public int hashCode() { .append(length) .append(width) .append(height) + .append(boundingBox) .toHashCode(); } From b4e93a1319cf31b4098ae7c324825000c0fb2b13 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Mon, 4 Sep 2023 10:23:20 +0200 Subject: [PATCH 14/17] cleanup(perception): trying to fix spotbugs warning in perception objects -> made VehicleIndex no longer implement Serializable MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Moritz Schweppenhäuser --- .../simulation/perception/index/providers/VehicleIndex.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/providers/VehicleIndex.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/providers/VehicleIndex.java index b4fb34f6f..ee5281a7f 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/providers/VehicleIndex.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/providers/VehicleIndex.java @@ -31,13 +31,12 @@ import com.google.gson.annotations.JsonAdapter; import org.slf4j.Logger; -import java.io.Serializable; import java.util.HashMap; import java.util.List; import java.util.Map; @JsonAdapter(VehicleIndexTypeAdapterFactory.class) -public abstract class VehicleIndex implements Serializable { +public abstract class VehicleIndex { /** * Stores {@link VehicleObject}s for fast removal and position update. From a39a6e7413ac9f3e2b97cbb886edab1ac5e2825f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Mon, 4 Sep 2023 11:33:39 +0200 Subject: [PATCH 15/17] cleanup(example-apps): fixing spotbugs warning example app MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Moritz Schweppenhäuser --- .../mosaic/app/examples/commuter/SimpleCommuterApp.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/app/tutorials/example-applications/src/main/java/org/eclipse/mosaic/app/examples/commuter/SimpleCommuterApp.java b/app/tutorials/example-applications/src/main/java/org/eclipse/mosaic/app/examples/commuter/SimpleCommuterApp.java index e6bddae3b..3050fad55 100644 --- a/app/tutorials/example-applications/src/main/java/org/eclipse/mosaic/app/examples/commuter/SimpleCommuterApp.java +++ b/app/tutorials/example-applications/src/main/java/org/eclipse/mosaic/app/examples/commuter/SimpleCommuterApp.java @@ -34,6 +34,7 @@ import org.eclipse.mosaic.rti.TIME; import org.apache.commons.lang3.builder.EqualsBuilder; +import org.apache.commons.lang3.builder.HashCodeBuilder; import javax.annotation.Nonnull; import javax.annotation.Nullable; @@ -186,6 +187,14 @@ public boolean equals(Object obj) { .append(this.homePosition, event.homePosition) .isEquals(); } + + @Override + public int hashCode() { + return new HashCodeBuilder(17, 37) + .append(currentPosition) + .append(homePosition) + .toHashCode(); + } } /** From 9c5ce92d0a9b73c59530427f7d1cd018771ad7d4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Mon, 4 Sep 2023 11:54:46 +0200 Subject: [PATCH 16/17] cleanup(perception): tyring to spotbugs warnings in perception MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * removed unnecessary method getNumberOfTrafficLights in TrafficObjectIndex Signed-off-by: Moritz Schweppenhäuser --- .../CentralPerceptionComponent.java | 30 ++++++------------- .../perception/index/TrafficObjectIndex.java | 14 --------- 2 files changed, 9 insertions(+), 35 deletions(-) diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/CentralPerceptionComponent.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/CentralPerceptionComponent.java index d07fc47a5..e7fd84d28 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/CentralPerceptionComponent.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/CentralPerceptionComponent.java @@ -225,63 +225,51 @@ static class MonitoringTrafficObjectIndexProvider extends TrafficObjectIndex { @Override public List getVehiclesInRange(PerceptionModel searchRange) { + int numberOfVehicles = getNumberOfVehicles(); try (PerformanceMonitor.Measurement m = monitor.start("search-vehicle")) { - m.setProperties(getNumberOfVehicles(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); + m.setProperties(numberOfVehicles, SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); return super.getVehiclesInRange(searchRange); } } @Override public void removeVehicles(Iterable vehiclesToRemove) { + int numberOfVehicles = getNumberOfVehicles(); try (PerformanceMonitor.Measurement m = monitor.start("remove-vehicle")) { - m.setProperties(getNumberOfVehicles(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); + m.setProperties(numberOfVehicles, SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); super.removeVehicles(vehiclesToRemove); } } @Override public void updateVehicles(Iterable vehiclesToUpdate) { + int numberOfVehicles = getNumberOfVehicles(); try (PerformanceMonitor.Measurement m = monitor.start("update-vehicle")) { - m.setProperties(getNumberOfVehicles(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); + m.setProperties(numberOfVehicles, SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); super.updateVehicles(vehiclesToUpdate); } } - @Override - public int getNumberOfVehicles() { - return super.getNumberOfVehicles(); - } - @Override public List getTrafficLightsInRange(PerceptionModel perceptionModel) { try (PerformanceMonitor.Measurement m = monitor.start("search-traffic-light")) { - m.setProperties(getNumberOfTrafficLights(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); + m.setProperties(SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); return super.getTrafficLightsInRange(perceptionModel); } } - @Override - public void addTrafficLightGroup(TrafficLightGroup trafficLightGroup) { - super.addTrafficLightGroup(trafficLightGroup); - } - @Override public void updateTrafficLights(Map trafficLightsToUpdate) { try (PerformanceMonitor.Measurement m = monitor.start("update-traffic-light")) { - m.setProperties(getNumberOfTrafficLights(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); + m.setProperties(SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); super.updateTrafficLights(trafficLightsToUpdate); } } - @Override - public int getNumberOfTrafficLights() { - return super.getNumberOfTrafficLights(); - } - @Override public Collection> getSurroundingWalls(PerceptionModel perceptionModel) { try (PerformanceMonitor.Measurement m = monitor.start("search-walls")) { - m.setProperties(getNumberOfTrafficLights(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); + m.setProperties(SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); return super.getSurroundingWalls(perceptionModel); } } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/TrafficObjectIndex.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/TrafficObjectIndex.java index 13cce80c2..f4f500d5e 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/TrafficObjectIndex.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/TrafficObjectIndex.java @@ -181,20 +181,6 @@ public void updateTrafficLights(Map trafficLights } } - /** - * Returns the number of TLs in the simulation. - * - * @return the number of TLs - */ - public int getNumberOfTrafficLights() { - if (trafficLightIndexProviderConfigured()) { - return trafficLightIndex.getNumberOfTrafficLights(); - } else { - log.debug("No Traffic Light Index Provider configured. There are no indexed Traffic Lights."); - return 0; - } - } - public Collection> getSurroundingWalls(PerceptionModel perceptionModel) { if (wallIndex == null) { log.debug("No Wall Index defined."); From af515e4c479454b92b3f7a4a9ccce2ee5a91f796 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Mon, 11 Sep 2023 13:07:30 +0200 Subject: [PATCH 17/17] cleanup(perception): reverted additional variable MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Moritz Schweppenhäuser --- .../perception/CentralPerceptionComponent.java | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/CentralPerceptionComponent.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/CentralPerceptionComponent.java index e7fd84d28..43749923c 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/CentralPerceptionComponent.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/CentralPerceptionComponent.java @@ -225,27 +225,24 @@ static class MonitoringTrafficObjectIndexProvider extends TrafficObjectIndex { @Override public List getVehiclesInRange(PerceptionModel searchRange) { - int numberOfVehicles = getNumberOfVehicles(); try (PerformanceMonitor.Measurement m = monitor.start("search-vehicle")) { - m.setProperties(numberOfVehicles, SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); + m.setProperties(getNumberOfVehicles(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); return super.getVehiclesInRange(searchRange); } } @Override public void removeVehicles(Iterable vehiclesToRemove) { - int numberOfVehicles = getNumberOfVehicles(); try (PerformanceMonitor.Measurement m = monitor.start("remove-vehicle")) { - m.setProperties(numberOfVehicles, SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); + m.setProperties(getNumberOfVehicles(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); super.removeVehicles(vehiclesToRemove); } } @Override public void updateVehicles(Iterable vehiclesToUpdate) { - int numberOfVehicles = getNumberOfVehicles(); try (PerformanceMonitor.Measurement m = monitor.start("update-vehicle")) { - m.setProperties(numberOfVehicles, SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); + m.setProperties(getNumberOfVehicles(), SimulationKernel.SimulationKernel.getCurrentSimulationTime()).restart(); super.updateVehicles(vehiclesToUpdate); } }