From 0b1074daba5c6423691e46fe802c392f94a065f2 Mon Sep 17 00:00:00 2001 From: schwepmo <39306374+schwepmo@users.noreply.github.com> Date: Tue, 12 Sep 2023 10:05:27 +0200 Subject: [PATCH] feat(perception): Use 2D bounding boxes for Occlusion Modifiers (#343) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat(perception): added bounding box occlusion modifier * feat(perception): added bounding box attribute to SpatialObject and properly implemented * BoundingBoxOcclusionModifier now evaluates if points are hidden behind other bounding boxes * WallOcclusionModifier now uses bounding boxes to see if objects are hidden * feat(perception): added additional check in BoundingBoxOcclusionModifier constructor * feat(perception): extended bounding box occlusion test * renaming of occlusion modifiers * cleanup(perception): trying to fix spotbugs warning in perception objects -> made VehicleIndex no longer implement Serializable * removed unnecessary method getNumberOfTrafficLights in TrafficObjectIndex Signed-off-by: Moritz Schweppenhäuser --- .../examples/commuter/SimpleCommuterApp.java | 29 +++ .../app/tutorial/vehicle/PerceptionApp.java | 14 +- .../perception/AbstractPerceptionModule.java | 2 +- .../CentralPerceptionComponent.java | 33 +--- .../errormodels/BoundingBoxOcclusion.java | 186 ++++++++++++++++++ ...tanceModifier.java => DistanceFilter.java} | 6 +- ...rorModifier.java => PositionModifier.java} | 8 +- ...sionModifier.java => SimpleOcclusion.java} | 8 +- ...lusionModifier.java => WallOcclusion.java} | 41 ++-- .../perception/index/TrafficObjectIndex.java | 14 -- .../index/objects/PointBoundingBox.java | 87 ++++++++ .../index/objects/SpatialObject.java | 4 +- .../objects/SpatialObjectBoundingBox.java | 40 ++++ .../index/objects/TrafficLightObject.java | 30 ++- .../index/objects/VehicleBoundingBox.java | 145 ++++++++++++++ .../index/objects/VehicleObject.java | 42 +++- .../index/providers/VehicleIndex.java | 5 +- .../app/api/perception/PerceptionModule.java | 2 +- .../perception/PerceptionModifierTest.java | 104 +++++++--- .../weighting/FixedOrderSelector.java | 5 +- 20 files changed, 693 insertions(+), 112 deletions(-) create mode 100644 fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusion.java rename fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/{DistanceModifier.java => DistanceFilter.java} (94%) rename fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/{PositionErrorModifier.java => PositionModifier.java} (93%) rename fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/{SimpleOcclusionModifier.java => SimpleOcclusion.java} (95%) rename fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/{WallOcclusionModifier.java => WallOcclusion.java} (58%) 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/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..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 @@ -33,6 +33,9 @@ import org.eclipse.mosaic.lib.util.scheduling.EventProcessor; 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; @@ -166,6 +169,32 @@ 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(); + } + + @Override + public int hashCode() { + return new HashCodeBuilder(17, 37) + .append(currentPosition) + .append(homePosition) + .toHashCode(); + } } /** 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..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,9 +16,9 @@ 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.SimpleOcclusionModifier; +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; import org.eclipse.mosaic.fed.application.app.api.VehicleApplication; @@ -66,15 +66,15 @@ 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); + 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(simpleOcclusionModifier, 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/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/CentralPerceptionComponent.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/CentralPerceptionComponent.java index 047133224..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 @@ -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,50 +242,31 @@ 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); } } - @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/errormodels/BoundingBoxOcclusion.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusion.java new file mode 100644 index 000000000..1043bc5d9 --- /dev/null +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/BoundingBoxOcclusion.java @@ -0,0 +1,186 @@ +/* + * 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.SpatialObjectBoundingBox; +import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.TrafficLightObject; +import org.eclipse.mosaic.lib.math.Vector3d; +import org.eclipse.mosaic.lib.math.VectorUtils; +import org.eclipse.mosaic.lib.spatial.Edge; + +import java.util.ArrayList; +import java.util.List; +import java.util.stream.Collectors; + +public class BoundingBoxOcclusion 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. + * Default: 2 + */ + private final int pointsPerSide; + + /** + * 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; + + /** + * Default constructor for the {@link BoundingBoxOcclusion}. + * Uses {@link #pointsPerSide} = 2 and {@link #detectionThreshold} = 2 as default values. + */ + public BoundingBoxOcclusion() { + this.pointsPerSide = 2; + this.detectionThreshold = 2; + } + + /** + * Constructor for {@link BoundingBoxOcclusion}, validates and sets + * the parameters {@link #pointsPerSide} and {@link #detectionThreshold}. + * + * @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 BoundingBoxOcclusion(int pointsPerSide, int detectionThreshold) { + if (pointsPerSide < 2) { + throw new IllegalArgumentException("Need at least 2 points per edge, meaning every corner will be checked for occlusion."); + } + if (detectionThreshold < 1) { + 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 IllegalArgumentException("The detection threshold exceeds the number of points evaluated per object"); + } + this.pointsPerSide = pointsPerSide; + this.detectionThreshold = detectionThreshold; + } + + @Override + public List apply(PerceptionModuleOwner owner, List spatialObjects) { + List newObjects = new ArrayList<>(); + // the ego object cannot occlude vision + List occludingObjects = spatialObjects.stream() + .filter(object -> !object.getId().equals(owner.getId())) + .collect(Collectors.toList()); + 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(objectToEvaluate); + final int requiredVisiblePoints = pointsToEvaluate.size() == 1 ? 1 : detectionThreshold; + int numberOfPointsVisible = 0; + for (Vector3d point : pointsToEvaluate) { + 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(objectToEvaluate); + break; + } + } + } + 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. + * 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<>(); + 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; + } + + 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/DistanceModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/DistanceFilter.java similarity index 94% 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 b50ed5f7a..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; @@ -52,7 +52,7 @@ public DistanceModifier(RandomNumberGenerator rng, double offset) { @Override public List apply(PerceptionModuleOwner owner, List spatialObjects) { - if (spatialObjects.size() == 0) { + 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/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 41146508c..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 @@ -32,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) */ @@ -58,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; @@ -71,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; @@ -85,7 +85,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/SimpleOcclusionModifier.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/SimpleOcclusion.java similarity index 95% 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 a39778903..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); @@ -63,7 +63,7 @@ public SimpleOcclusionModifier(double minDetectionAngle, double maxDetectionAngl @Override public List apply(PerceptionModuleOwner owner, List spatialObjects) { - if (spatialObjects.size() == 0) { + 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/WallOcclusion.java similarity index 58% 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 6ae19d765..1d1c9f661 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,13 +31,13 @@ * 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 Vector3d intersectionResult = new Vector3d(); @Override public List apply(PerceptionModuleOwner owner, List spatialObjects) { - if (spatialObjects.size() == 0) { + if (spatialObjects.isEmpty()) { return spatialObjects; } @@ -45,25 +45,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/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."); 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..2a0a04ce6 --- /dev/null +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/PointBoundingBox.java @@ -0,0 +1,87 @@ +/* + * 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 org.apache.commons.lang3.builder.EqualsBuilder; +import org.apache.commons.lang3.builder.HashCodeBuilder; + +import java.util.Collections; +import java.util.List; + +/** + * 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 { + + 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. + */ + private final List allCorners; + /** + * An empty list as a no bounding box is spanned by a single point. + */ + private final List> allEdges; + + public PointBoundingBox(Vector3d position) { + allCorners = Collections.singletonList(position); + allEdges = Collections.emptyList(); + } + + @Override + public List getAllCorners() { + return allCorners; + } + + @Override + 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/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..64d9f4c7b --- /dev/null +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/SpatialObjectBoundingBox.java @@ -0,0 +1,40 @@ +/* + * 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.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 extends Serializable { + + /** + * 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} 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 026ac0727..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 @@ -19,8 +19,12 @@ 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 { + + private static final long serialVersionUID = 1L; + /** * Id of the group the individual traffic light belongs to. */ @@ -37,6 +41,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 transient PointBoundingBox boundingBox; public TrafficLightObject(String id) { super(id); @@ -49,6 +57,14 @@ public TrafficLightObject setPosition(CartesianPoint position) { return this; } + @Override + public SpatialObjectBoundingBox getBoundingBox() { + if (boundingBox == null) { + boundingBox = new PointBoundingBox(this); + } + return boundingBox; + } + public String getTrafficLightGroupId() { return trafficLightGroupId; } @@ -103,9 +119,21 @@ public boolean equals(Object o) { .append(trafficLightState, that.trafficLightState) .append(incomingLane, that.incomingLane) .append(outgoingLane, that.outgoingLane) + .append(boundingBox, that.boundingBox) .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. @@ -114,7 +142,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/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..dfa488712 --- /dev/null +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/index/objects/VehicleBoundingBox.java @@ -0,0 +1,145 @@ +/* + * 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 org.apache.commons.lang3.builder.EqualsBuilder; +import org.apache.commons.lang3.builder.HashCodeBuilder; + +import java.util.List; + +/** + * An object representing a vehicles' 2D bounding box. + * 
+ *   frontRightCorner----------rightEdge----------backRightCorner
+ *                  |                             |
+ *   Heading <== frontEdge                     backEdge
+ *                  |                             |
+ *    frontLeftCorner----------leftEdge-----------backLeftCorner
+ *
+ * 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; + public final Vector3d backLeftCorner; + public final Vector3d frontLeftCorner; + + private final List> allEdges; + 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; + 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); + } + + /** + * 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; + // 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); + } + + @Override + public List getAllCorners() { + return allCorners; + } + + @Override + 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 08e56723a..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 @@ -18,11 +18,15 @@ 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; public class VehicleObject extends SpatialObject { + private static final long serialVersionUID = 1L; + + /** * The current speed of the vehicle. [m/s] */ @@ -51,6 +55,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); @@ -60,6 +68,7 @@ public VehicleObject(String id) { public VehicleObject setPosition(CartesianPoint position) { cartesianPosition.set(position); position.toVector3d(this); + this.boundingBox = null; return this; } @@ -89,6 +98,7 @@ public double getSpeed() { public VehicleObject setHeading(double heading) { this.heading = heading; + this.boundingBox = null; return this; } @@ -100,6 +110,7 @@ public VehicleObject setDimensions(double length, double width, double height) { this.length = length; this.width = width; this.height = height; + this.boundingBox = null; return this; } @@ -115,6 +126,18 @@ public double getHeight() { return height; } + /** + * 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) { @@ -136,9 +159,25 @@ public boolean equals(Object o) { .append(length, that.length) .append(width, that.width) .append(height, that.height) + .append(boundingBox, that.boundingBox) .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) + .append(boundingBox) + .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. @@ -147,12 +186,11 @@ 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()) .setDimensions(getLength(), getWidth(), getHeight()) .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 a551d014b..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. @@ -77,7 +76,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..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 @@ -28,10 +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.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.BoundingBoxOcclusion; +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; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.SpatialObject; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.index.objects.VehicleObject; @@ -46,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; @@ -123,53 +125,51 @@ 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); + 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 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() ); List perceivedVehicles = simplePerceptionModule.getPerceivedVehicles(); if (PRINT_POSITIONS) { - printPerceivedPositions(perceivedVehicles); + printBoundingBoxes(perceivedVehicles); } assertTrue("The occlusion filter should remove vehicles", VEHICLE_AMOUNT > perceivedVehicles.size()); } @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(); if (PRINT_POSITIONS) { - printPerceivedPositions(perceivedVehicles); + printBoundingBoxes(perceivedVehicles); } assertTrue("The distance filter should remove vehicles", VEHICLE_AMOUNT > perceivedVehicles.size()); } @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(); if (PRINT_POSITIONS) { - printPerceivedPositions(perceivedVehicles); + printBoundingBoxes(perceivedVehicles); } assertEquals("The position error filter shouldn't remove vehicles", VEHICLE_AMOUNT, perceivedVehicles.size()); } @@ -181,13 +181,13 @@ 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() ); 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 @@ -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 @@ -224,12 +224,41 @@ 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() { + BoundingBoxOcclusion boundingBoxOcclusion = new BoundingBoxOcclusion(); + simplePerceptionModule.enable( + 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 + BoundingBoxOcclusion boundingBoxOcclusionCustomParams = new BoundingBoxOcclusion(5, 2); + simplePerceptionModule.enable( + new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE) + .addModifier(boundingBoxOcclusionCustomParams).build() + ); + List perceivedVehiclesCustomModifier = simplePerceptionModule.getPerceivedVehicles(); + + if (PRINT_POSITIONS) { + printBoundingBoxes(perceivedVehicles); + System.out.println(); + 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++) { points.add(getRandomPointInRange(origin, range)); @@ -261,16 +290,39 @@ 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); } 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; + } + }); + } } 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()