feat: add perception modifier for heading and dimensions

Signed-off-by: Karl Schrab <[email protected]>

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

@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2022 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.VehicleObject;
+import org.eclipse.mosaic.lib.math.RandomNumberGenerator;
+import org.eclipse.mosaic.lib.math.Vector3d;
+
+import java.util.List;
+
+public class DimensionsModifier implements PerceptionModifier {
+
+    private static final double SIGMA_WIDTH_OFFSET = 0.2; // given in m
+    private static final double SIGMA_HEIGHT_OFFSET = 0.2; // given in m
+    private static final double SIGMA_LENGTH_OFFSET = 0.5; // given in m
+
+    private final double heightOffset;
+    private final double widthOffset;
+    private final double lengthOffset;
+
+    private final RandomNumberGenerator rng;
+
+
+    public DimensionsModifier(RandomNumberGenerator rng, double heightOffset, double widthOffset, double lengthOffset) {
+        this.rng = rng;
+        this.heightOffset = heightOffset;
+        this.widthOffset = widthOffset;
+        this.lengthOffset = lengthOffset;
+    }
+
+    public DimensionsModifier(RandomNumberGenerator rng) {
+        this.rng = rng;
+        this.heightOffset = SIGMA_HEIGHT_OFFSET;
+        this.widthOffset = SIGMA_WIDTH_OFFSET;
+        this.lengthOffset = SIGMA_LENGTH_OFFSET;
+    }
+
+    @Override
+    public <T extends SpatialObject> List<T> apply(PerceptionModuleOwner owner, List<T> spatialObjects) {
+        final Vector3d ownerPosition = owner.getVehicleData().getProjectedPosition().toVector3d();
+
+        spatialObjects.stream()
+                .filter(o -> o instanceof VehicleObject)
+                .forEach(o -> adjustDimensionsOfVehicle((VehicleObject) o));
+
+        return spatialObjects;
+    }
+
+    private void adjustDimensionsOfVehicle(VehicleObject vehicleObject) {
+        vehicleObject.setDimensions(
+                Math.abs(rng.nextGaussian(vehicleObject.getLength(), lengthOffset)),
+                Math.abs(rng.nextGaussian(vehicleObject.getHeight(), heightOffset)),
+                Math.abs(rng.nextGaussian(vehicleObject.getWidth(), widthOffset))
+        );
+    }
+
+}
+

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

@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2022 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.VehicleObject;
+import org.eclipse.mosaic.lib.math.RandomNumberGenerator;
+import org.eclipse.mosaic.lib.math.Vector3d;
+import org.eclipse.mosaic.lib.math.VectorUtils;
+
+import java.util.List;
+
+
+public class HeadingModifier implements PerceptionModifier {
+
+    /**
+     * Default standard deviation for heading error
+     */
+    private static final double SIGMA_HEADING_OFFSET = 4; // given in degree
+
+    /**
+     * Default chance of the car being perceived heading in the complete other direction (180° error)
+     */
+    private static final double DEFAULT_CHANCE_WRONG_DIR = 0.01;
+
+    private final RandomNumberGenerator rng;
+
+    /**
+     * Standard deviation for heading error.
+     */
+    private final double headingStandardDeviation;
+
+    private final double chanceOfWrongDirection;
+
+    public HeadingModifier(RandomNumberGenerator rng) {
+        this.rng = rng;
+        this.headingStandardDeviation = SIGMA_HEADING_OFFSET;
+        this.chanceOfWrongDirection = DEFAULT_CHANCE_WRONG_DIR;
+    }
+
+    public HeadingModifier(RandomNumberGenerator rng, double headingStandardDeviation, double chanceOfWrongDirection) {
+        this.rng = rng;
+        this.headingStandardDeviation = headingStandardDeviation;
+        this.chanceOfWrongDirection = chanceOfWrongDirection;
+    }
+
+    @Override
+    public <T extends SpatialObject> List<T> apply(PerceptionModuleOwner owner, List<T> spatialObjects) {
+        final Vector3d ownerPosition = owner.getVehicleData().getProjectedPosition().toVector3d();
+
+        spatialObjects.stream()
+                .filter(o -> o instanceof VehicleObject)
+                .forEach(o -> adjustHeadingOfVehicle(ownerPosition, (VehicleObject) o));
+
+        return spatialObjects;
+    }
+
+    private void adjustHeadingOfVehicle(Vector3d ownerPosition, VehicleObject vehicleObject) {
+
+        double oldHeading = vehicleObject.getHeading();
+
+        if (rng.nextDouble() < chanceOfWrongDirection) {
+            vehicleObject.setHeading(rotatedVehicleHeading(vehicleObject.getHeading()));
+        }
+        vehicleObject.setHeading(rng.nextGaussian(vehicleObject.getHeading(), headingStandardDeviation) % 360);
+
+        double newHeading = vehicleObject.getHeading();
+
+        Vector3d oldHeadingVector = VectorUtils.getDirectionVectorFromHeading(oldHeading, new Vector3d());
+        Vector3d newHeadingVector = VectorUtils.getDirectionVectorFromHeading(newHeading, new Vector3d());
+
+        Vector3d newPosition = new Vector3d(vehicleObject.getPosition())
+                .subtract(oldHeadingVector.multiply(vehicleObject.getLength() / 2))
+                .add(newHeadingVector.multiply(vehicleObject.getLength() / 2));
+        vehicleObject.setPosition(newPosition.x, newPosition.y, newPosition.z);
+    }
+
+    private static double rotatedVehicleHeading(double heading) {
+        return (heading + 180.0) % 360.0;
+    }
+}

fed/mosaic-application/src/test/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java

@@ -17,6 +17,7 @@
 
 import static org.junit.Assert.assertEquals;
 import static org.junit.Assert.assertFalse;
+import static org.junit.Assert.assertNotEquals;
 import static org.junit.Assert.assertTrue;
 import static org.mockito.Mockito.doReturn;
 import static org.mockito.Mockito.mock;
@@ -28,7 +29,9 @@
 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.DimensionsModifier;
 import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.DistanceFilter;
+import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.errormodels.HeadingModifier;
 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;
@@ -173,6 +176,55 @@ public void testPositionErrorModifier() {
         assertEquals("The position error filter shouldn't remove vehicles", VEHICLE_AMOUNT, perceivedVehicles.size());
     }
 
+    @Test
+    public void testHeadingModifier() {
+        HeadingModifier headingModifier = new HeadingModifier(rng, 10, 0);
+        simplePerceptionModule.enable(
+                new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(headingModifier).build()
+        );
+
+        // RUN
+        List<VehicleObject> perceivedVehicles = simplePerceptionModule.getPerceivedVehicles();
+        if (PRINT_POSITIONS) {
+            printBoundingBoxes(perceivedVehicles);
+        }
+        assertEquals("The position error filter shouldn't remove vehicles", VEHICLE_AMOUNT, perceivedVehicles.size());
+
+        // ASSERT if headings differ from ground truth
+        for (VehicleObject realVehicle : getAllVehicles()) {
+            for (VehicleObject perceivedVehicle: perceivedVehicles) {
+                if (realVehicle.getId().equals(perceivedVehicle.getId())) {
+                    assertNotEquals(realVehicle.getHeading(), perceivedVehicle.getHeading(), 0.0001);
+                    // when adjusting heading the position should change too, since it currently points to the front bumper of the vehicle
+                    assertNotEquals(realVehicle.getPosition(), perceivedVehicle.getPosition());
+                }
+            }
+        }
+    }
+
+    @Test
+    public void testDimensionsModifier() {
+        DimensionsModifier dimensionsModifier = new DimensionsModifier(rng, 0.0, 0.0, 1.0);
+        simplePerceptionModule.enable(
+                new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(dimensionsModifier).build()
+        );
+
+        List<VehicleObject> perceivedVehicles = simplePerceptionModule.getPerceivedVehicles();
+        if (PRINT_POSITIONS) {
+            printBoundingBoxes(perceivedVehicles);
+        }
+        assertEquals("The position error filter shouldn't remove vehicles", VEHICLE_AMOUNT, perceivedVehicles.size());
+
+        // ASSERT if headings differ from ground truth
+        for (VehicleObject realVehicle : getAllVehicles()) {
+            for (VehicleObject perceivedVehicle: perceivedVehicles) {
+                if (realVehicle.getId().equals(perceivedVehicle.getId())) {
+                    assertNotEquals(realVehicle.getLength(), perceivedVehicle.getLength(), 0.0001);;
+                }
+            }
+        }
+    }
+
     @Test
     public void testWallOcclusionModifier() {
         List<Edge<Vector3d>> surroundingWalls = Lists.newArrayList(