feat(perception): Use 2D bounding boxes for Occlusion Modifiers

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();
+        }
     }
 
     /**

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);
     }

fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/AbstractPerceptionModule.java

@@ -112,7 +112,7 @@ public List<SpatialObject> getPerceivedObjects() {
 
     private <T extends SpatialObject<T>> List<T> applyPerceptionModifiers(List<T> objectsInRange) {
         List<T> 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

fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/CentralPerceptionComponent.java

@@ -226,68 +226,47 @@ static class MonitoringTrafficObjectIndexProvider extends TrafficObjectIndex {
         @Override
         public List<VehicleObject> 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);
             }
         }
 
         @Override
         public void removeVehicles(Iterable<String> 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);
             }
         }
 
         @Override
         public void updateVehicles(Iterable<VehicleData> 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<TrafficLightObject> 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<String, TrafficLightGroupInfo> 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<Edge<Vector3d>> 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);
             }
         }

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: [email protected]
+ */
+
+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 <T extends SpatialObject> List<T> apply(PerceptionModuleOwner owner, List<T> spatialObjects) {
+        List<T> newObjects = new ArrayList<>();
+        // the ego object cannot occlude vision
+        List<T> 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<Vector3d> 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 <T extends SpatialObject> boolean isVisible(Vector3d egoPosition, Vector3d pointToEvaluate, String objectId, List<T> 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<Vector3d> 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):
+     * <pre>
+     *     x-----o-----x
+     *     |           |
+     *     |           |
+     *     o           o
+     *     |           |
+     *     |           |
+     *     x-----o-----y
+     * </pre>
+     *
+     * @param spatialObject a {@link SpatialObject} for which the occlusion should be evaluated
+     */
+    private <T extends SpatialObject> List<Vector3d> createPointsToEvaluate(T spatialObject) {
+        List<Vector3d> 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<Vector3d> 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<Vector3d> points, Vector3d newPoint) {
+        return points.stream().noneMatch(vector3d -> vector3d.isFuzzyEqual(newPoint));
+    }
+}

fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/errormodels/DistanceModifier.java → 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,15 +44,15 @@ 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;
     }
 
     @Override
     public <T extends SpatialObject> List<T> apply(PerceptionModuleOwner owner, List<T> spatialObjects) {
-        if (spatialObjects.size() == 0) {
+        if (spatialObjects.isEmpty()) {
             return spatialObjects;
         }
         Vector3d ownerPosition = owner.getVehicleData().getProjectedPosition().toVector3d();