Skip to content

Commit

Permalink
improvement(perception-module): dramatically improved speed of wall o…
Browse files Browse the repository at this point in the history 
…cclusion eval

* achieved by a) using a more sensible pre-selection of walls
* b not using the 3D intersections eval from VectorUtils

Signed-off-by: Moritz Schweppenhäuser <[email protected]>
  • Loading branch information
@schwepmo
schwepmo committed Dec 5, 2024
1 parent ef7ba5b commit fb9b932
Show file tree
Hide file tree
Showing 3 changed files with 61 additions and 16 deletions.
54 changes: 46 additions & 8 deletions ...se/mosaic/fed/application/ambassador/simulation/perception/errormodels/WallOcclusion.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@

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.lib.math.MathUtils;
import org.eclipse.mosaic.lib.math.Vector3d;
import org.eclipse.mosaic.lib.math.VectorUtils;
import org.eclipse.mosaic.lib.spatial.Edge;

import java.util.ArrayList;
Expand All @@ -34,8 +34,6 @@
*/
public class WallOcclusion implements PerceptionModifier {

private final Vector3d intersectionResult = new Vector3d();

@Override
public <T extends SpatialObject> List<T> apply(PerceptionModuleOwner owner, List<T> spatialObjects) {
if (spatialObjects.isEmpty()) {
Expand All @@ -46,6 +44,7 @@ public <T extends SpatialObject> List<T> apply(PerceptionModuleOwner owner, List
if (walls.isEmpty()) {
return spatialObjects;
}
Vector3d ownerPosition = owner.getVehicleData().getProjectedPosition().toVector3d();
final List<T> result = new ArrayList<>();
for (T spatialObject : spatialObjects) {
List<Vector3d> pointsToEvaluate = spatialObject.getBoundingBox().getAllCorners();
Expand All @@ -56,11 +55,7 @@ public <T extends SpatialObject> List<T> apply(PerceptionModuleOwner owner, List
boolean pointOccluded = false;
for (Edge<Vector3d> 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) {
if (doIntersect(ownerPosition.x, ownerPosition.z, point.x, point.z, wall.a.x, wall.a.z, wall.b.x, wall.b.z)) {
pointOccluded = true;
break;
}
Expand All @@ -77,4 +72,47 @@ public <T extends SpatialObject> List<T> apply(PerceptionModuleOwner owner, List
return result;
}

private enum Orientation {
COLLINEAR, CLOCKWISE, COUNTERCLOCKWISE
}

// Function to calculate the orientation of the triplet (px, py), (qx, qy), (rx, ry).
static Orientation orientation(double px, double py, double qx, double qy, double rx, double ry) {
double val = (qy - py) * (rx - qx) - (qx - px) * (ry - qy);
if (MathUtils.isFuzzyEqual(val, 0d)) { // Collinear
return Orientation.COLLINEAR;
}
return (val > 0) ? Orientation.CLOCKWISE : Orientation.COUNTERCLOCKWISE; // Clockwise or Counterclockwise
}

// Function to check if point (qx, qy) lies on segment (px, py) to (rx, ry).
static boolean onSegment(double px, double py, double qx, double qy, double rx, double ry) {
return qx <= Math.max(px, rx)
&& qx >= Math.min(px, rx)
&& qy <= Math.max(py, ry)
&& qy >= Math.min(py, ry);
}

// Function to check if two lines (p1, q1) and (p2, q2) intersect.
static boolean doIntersect(double p1x, double p1y, double q1x, double q1y,
double p2x, double p2y, double q2x, double q2y) {
Orientation o1 = orientation(p1x, p1y, q1x, q1y, p2x, p2y);
Orientation o2 = orientation(p1x, p1y, q1x, q1y, q2x, q2y);
Orientation o3 = orientation(p2x, p2y, q2x, q2y, p1x, p1y);
Orientation o4 = orientation(p2x, p2y, q2x, q2y, q1x, q1y);

// General case
if (o1 != o2 && o3 != o4) {
return true;
}

// Special cases
if (o1 == Orientation.COLLINEAR && onSegment(p1x, p1y, p2x, p2y, q1x, q1y)) return true;
if (o2 == Orientation.COLLINEAR && onSegment(p1x, p1y, q2x, q2y, q1x, q1y)) return true;
if (o3 == Orientation.COLLINEAR && onSegment(p2x, p2y, p1x, p1y, q2x, q2y)) return true;
if (o4 == Orientation.COLLINEAR && onSegment(p2x, p2y, q1x, q1y, q2x, q2y)) return true;

return false; // Doesn't fall in any of the above cases
}

}
13 changes: 11 additions & 2 deletions ...pse/mosaic/fed/application/ambassador/simulation/perception/index/providers/WallTree.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,10 @@
import java.util.List;

public class WallTree extends WallIndex {

/**
* The longest wall, relevant for tree setup, so that all walls will be included. [m]
*/
private double maxWallLength = 50;
private final int bucketSize;

private KdTree<Edge<Vector3d>> wallTree;
Expand All @@ -42,22 +45,28 @@ public WallTree(int bucketSize) {
@Override
public void initialize() {
List<Edge<Vector3d>> walls = new ArrayList<>();
double maxWallLength = 0;
for (Building building : super.getDatabase().getBuildings()) {
for (Wall wall : building.getWalls()) {
walls.add(new Edge<>(
wall.getFromCorner().getCartesianPosition().toVector3d(),
wall.getToCorner().getCartesianPosition().toVector3d()
));
if (wall.length > maxWallLength) {
maxWallLength = wall.length;
}
}
}
this.maxWallLength = maxWallLength;
wallTree = new KdTree<>(new SpatialItemAdapter.EdgeAdapter<>(), walls, bucketSize);
wallTraverser = new org.eclipse.mosaic.lib.database.spatial.Edge.InRadius<>();
}

@Override
public Collection<Edge<Vector3d>> getSurroundingWalls(PerceptionModel perceptionModel) {
// overestimating the initial list of walls by extending max bounding box radius with maximal wall length
wallTraverser.setup(perceptionModel.getBoundingBox().center,
perceptionModel.getBoundingBox().center.distanceSqrTo(perceptionModel.getBoundingBox().min)); // overestimating distance
perceptionModel.getBoundingBox().center.distanceTo(perceptionModel.getBoundingBox().min) + maxWallLength);
wallTraverser.traverse(wallTree);
return wallTraverser.getResult();
}
Expand Down
10 changes: 4 additions & 6 deletions ...lipse/mosaic/fed/application/ambassador/simulation/perception/PerceptionModifierTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -192,7 +192,7 @@ public void testHeadingModifier() {

// ASSERT if headings differ from ground truth
for (VehicleObject realVehicle : getAllVehicles()) {
for (VehicleObject perceivedVehicle: perceivedVehicles) {
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
Expand All @@ -204,7 +204,7 @@ public void testHeadingModifier() {

@Test
public void testDimensionsModifier() {
DimensionsModifier dimensionsModifier = new DimensionsModifier(rng, 1.0, 0.0, 0.0);
DimensionsModifier dimensionsModifier = new DimensionsModifier(rng, 1.0, 0.0, 0.0);
simplePerceptionModule.enable(
new SimplePerceptionConfiguration.Builder(VIEWING_ANGLE, VIEWING_RANGE).addModifier(dimensionsModifier).build()
);
Expand All @@ -217,7 +217,7 @@ public void testDimensionsModifier() {

// ASSERT if headings differ from ground truth
for (VehicleObject realVehicle : getAllVehicles()) {
for (VehicleObject perceivedVehicle: perceivedVehicles) {
for (VehicleObject perceivedVehicle : perceivedVehicles) {
if (realVehicle.getId().equals(perceivedVehicle.getId())) {
assertNotEquals(realVehicle.getLength(), perceivedVehicle.getLength(), 0.0001);
// when adjusting length the position should change too, since it currently points to the front bumper of the vehicle
Expand Down Expand Up @@ -270,9 +270,7 @@ public void testIndexedObjectsNotChanged() {
.stream().collect(Collectors.toMap(VehicleObject::getId, v -> new Vector3d(v.getPosition())));

// ASSERT that all positions in the index are still the same as before applying modifier
allVehiclesInIndexPre.forEach((id, pos) -> {
assertTrue(pos.isFuzzyEqual(allVehiclesInIndexPost.get(id)));
});
allVehiclesInIndexPre.forEach((id, pos) -> assertTrue(pos.isFuzzyEqual(allVehiclesInIndexPost.get(id))));

// ASSERT that all modified positions differ from the positions before (or after) applying the modifier
for (VehicleObject object : perceivedAndAlteredObjects) {
Expand Down

0 comments on commit fb9b932

Please sign in to comment.