Commit 16c26f18 authored by Benedikt Zoennchen's avatar Benedikt Zoennchen
Browse files

bug fix in the EikonalSovers: FIM and FSM

parent c66d77d7
......@@ -2,7 +2,6 @@
"name" : "rimea_07_speeds_discrete_ca",
"description" : "",
"release" : "0.5",
"commithash" : "warning: no commit hash",
"processWriters" : {
"files" : [ {
"type" : "org.vadere.simulator.projects.dataprocessing.outputfile.PedestrianIdOutputFile",
......@@ -157,6 +156,7 @@
"groupSizeDistribution" : [ 1.0 ],
"dynamicElementType" : "PEDESTRIAN"
} ],
"dynamicElements" : [ ],
"attributesPedestrian" : {
"radius" : 0.195,
"densityDependentSpeed" : false,
......@@ -166,8 +166,7 @@
"maximumSpeed" : 2.2,
"acceleration" : 2.0
},
"attributesCar" : null,
"dynamicElements" : [ ]
"attributesCar" : null
}
}
}
\ No newline at end of file
......@@ -2,7 +2,6 @@
"name" : "rimea_07_speeds_nelder_mead",
"description" : "",
"release" : "0.5",
"commithash" : "warning: no commit hash",
"processWriters" : {
"files" : [ {
"type" : "org.vadere.simulator.projects.dataprocessing.outputfile.PedestrianIdOutputFile",
......@@ -157,6 +156,7 @@
"groupSizeDistribution" : [ 1.0 ],
"dynamicElementType" : "PEDESTRIAN"
} ],
"dynamicElements" : [ ],
"attributesPedestrian" : {
"radius" : 0.195,
"densityDependentSpeed" : false,
......@@ -166,8 +166,7 @@
"maximumSpeed" : 2.0,
"acceleration" : 2.0
},
"attributesCar" : null,
"dynamicElements" : [ ]
"attributesCar" : null
}
}
}
\ No newline at end of file
......@@ -2,7 +2,6 @@
"name" : "rimea_07_speeds_pso",
"description" : "",
"release" : "0.5",
"commithash" : "warning: no commit hash",
"processWriters" : {
"files" : [ {
"type" : "org.vadere.simulator.projects.dataprocessing.outputfile.PedestrianIdOutputFile",
......@@ -445,6 +444,7 @@
"groupSizeDistribution" : [ 1.0 ],
"dynamicElementType" : "PEDESTRIAN"
} ],
"dynamicElements" : [ ],
"attributesPedestrian" : {
"radius" : 0.195,
"densityDependentSpeed" : false,
......@@ -454,8 +454,7 @@
"maximumSpeed" : 2.0,
"acceleration" : 2.0
},
"attributesCar" : null,
"dynamicElements" : [ ]
"attributesCar" : null
}
}
}
\ No newline at end of file
......@@ -2,7 +2,6 @@
"name" : "rimea_09_public_room_2_exits_nelder_mead",
"description" : "",
"release" : "0.5",
"commithash" : "72af1aed04182a19d4297fdf053337b4acdce871",
"processWriters" : {
"files" : [ {
"type" : "org.vadere.simulator.projects.dataprocessing.outputfile.PedestrianIdOutputFile",
......
......@@ -2,7 +2,6 @@
"name" : "rimea_09_public_room_2_exits_pso",
"description" : "",
"release" : "0.5",
"commithash" : "72af1aed04182a19d4297fdf053337b4acdce871",
"processWriters" : {
"files" : [ {
"type" : "org.vadere.simulator.projects.dataprocessing.outputfile.PedestrianIdOutputFile",
......
......@@ -37,7 +37,7 @@ public class ObstacleDistancePotential implements IPotentialField {
CellGrid cellGrid = new CellGrid(bounds.getWidth(), bounds.getHeight(), attributesFloorField.getPotentialFieldResolution(), new CellState());
for (VShape shape : obstacles) {
FloorDiscretizer.setGridValuesForShapeCentered(cellGrid, shape,
FloorDiscretizer.setGridValuesForShape(cellGrid, shape,
new CellState(0.0, PathFindingTag.Target));
}
......@@ -49,10 +49,10 @@ public class ObstacleDistancePotential implements IPotentialField {
eikonalSolver = new PotentialFieldCalculatorNone();
break;
case FAST_ITERATIVE_METHOD:
eikonalSolver = new EikonalSolverFIM(cellGrid, obstacles, isHighAccuracyFM, new UnitTimeCostFunction());
eikonalSolver = new EikonalSolverFIM(cellGrid, obstacles, new UnitTimeCostFunction());
break;
case FAST_SWEEPING_METHOD:
eikonalSolver = new EikonalSolverFSM(cellGrid, obstacles, isHighAccuracyFM, new UnitTimeCostFunction());
eikonalSolver = new EikonalSolverFSM(cellGrid, obstacles, new UnitTimeCostFunction());
break;
default:
eikonalSolver = new EikonalSolverSFMM(cellGrid, obstacles, isHighAccuracyFM, new UnitTimeCostFunction());
......
......@@ -53,12 +53,12 @@ class PotentialFieldAndInitializer {
if (createMethod != EikonalSolverType.NONE) {
for (VShape shape : targetShapes) {
FloorDiscretizer.setGridValuesForShapeCentered(cellGrid, shape,
FloorDiscretizer.setGridValuesForShape(cellGrid, shape,
new CellState(0.0, PathFindingTag.Target));
}
for (Obstacle obstacle : topography.getObstacles()) {
FloorDiscretizer.setGridValuesForShapeCentered(cellGrid, obstacle.getShape(),
FloorDiscretizer.setGridValuesForShape(cellGrid, obstacle.getShape(),
new CellState(Double.MAX_VALUE, PathFindingTag.Obstacle));
}
}
......@@ -78,10 +78,10 @@ class PotentialFieldAndInitializer {
eikonalSolver = new PotentialFieldCalculatorNone();
break;
case FAST_ITERATIVE_METHOD:
eikonalSolver = new EikonalSolverFIM(cellGrid, targetShapes, isHighAccuracyFM, timeCost);
eikonalSolver = new EikonalSolverFIM(cellGrid, targetShapes, timeCost);
break;
case FAST_SWEEPING_METHOD:
eikonalSolver = new EikonalSolverFSM(cellGrid, targetShapes, isHighAccuracyFM, timeCost);
eikonalSolver = new EikonalSolverFSM(cellGrid, targetShapes, timeCost);
break;
default:
eikonalSolver = new EikonalSolverSFMM(cellGrid, targetShapes, isHighAccuracyFM, timeCost);
......
......@@ -51,12 +51,12 @@ public class QueueDetector extends EikonalSolverFMM {
this.polytope = null;
for (VShape shape : targetShapes) {
FloorDiscretizer.setGridValuesForShapeCentered(cellGrid, shape,
FloorDiscretizer.setGridValuesForShape(cellGrid, shape,
new CellState(0.0, PathFindingTag.Target));
}
for (Obstacle obstacle : topography.getObstacles()) {
FloorDiscretizer.setGridValuesForShapeCentered(
FloorDiscretizer.setGridValuesForShape(
cellGrid, obstacle.getShape(),
new CellState(Double.MAX_VALUE, PathFindingTag.Obstacle));
}
......
......@@ -278,6 +278,8 @@ public interface EikonalSolver {
c += factor * Math.pow(tp, 2);
}
}
return MathUtil.solveQuadraticMax(a, b, c);
}
......
......@@ -32,19 +32,14 @@ public class EikonalSolverFIM implements EikonalSolver {
private List<Point> targetPoints;
private static Logger logger = LogManager.getLogger(EikonalSolverFIM.class);
private ITimeCostFunction timeCostFunction;
private boolean isHighAccuracy;
private boolean isActiveList[][];
private final double epsilon;
private LinkedList<Point> activeList;
public EikonalSolverFIM(final CellGrid cellGrid,
final Collection<VShape> targetShapes,
final boolean isHighAccuracy,
final ITimeCostFunction timeCostFunction) {
this.timeCostFunction = timeCostFunction;
this.isHighAccuracy = isHighAccuracy;
this.targetShapes = targetShapes;
this.targetPoints = cellGrid.pointStream().filter(p -> cellGrid.getValue(p).tag == PathFindingTag.Target)
.collect(Collectors.toList());
......@@ -103,7 +98,7 @@ public class EikonalSolverFIM implements EikonalSolver {
double qq = computeGodunovDifference(neighbour, cellGrid, Direction.ANY);
// converged
if (qq < (pp - epsilon)) {
if (pp > qq) {
cellGrid.setValue(neighbour, new CellState(qq, PathFindingTag.NARROW));
newActiveList.add(neighbour);
}
......@@ -112,6 +107,8 @@ public class EikonalSolverFIM implements EikonalSolver {
cellGrid.setValue(activePoint,
new CellState(cellGrid.getValue(activePoint).potential, PathFindingTag.Reached));
activeListIterator.remove();
} else {
newActiveList.add(activePoint);
}
}
activeList.addAll(newActiveList);
......@@ -160,6 +157,6 @@ public class EikonalSolverFIM implements EikonalSolver {
@Override
public boolean isHighAccuracy() {
return true;
return false;
}
}
package org.vadere.util.potential.calculators;
import java.awt.Point;
import java.util.ArrayList;
import java.util.Collection;
import java.util.List;
import java.util.PriorityQueue;
......@@ -182,13 +181,9 @@ public class EikonalSolverFMM implements EikonalSolver {
}
}
private double minDistanceToTarget(final VPoint point) {
private double minDistanceToTarget(final VPoint point, VPoint dp) {
double minDistance = Double.MAX_VALUE;
double tmp;
// create point that lies in the center of the grid cells so that the distances can be
// computed starting there.
VPoint dp = new VPoint(cellGrid.getWidth() / (cellGrid.getNumPointsX() - 1) / 2.0,
cellGrid.getHeight() / (cellGrid.getNumPointsY() - 1) / 2.0);
for (VShape targetShape : targetShapes) {
// negative distances are possible when point is inside the target
tmp = Math.max(0, targetShape.distance(point.add(dp)));
......@@ -198,4 +193,13 @@ public class EikonalSolverFMM implements EikonalSolver {
}
return minDistance;
}
private double minDistanceToTargetCentered(final VPoint point) {
return minDistanceToTarget(point, new VPoint(cellGrid.getWidth() / (cellGrid.getNumPointsX() - 1) / 2.0,
cellGrid.getHeight() / (cellGrid.getNumPointsY() - 1) / 2.0));
}
private double minDistanceToTarget(final VPoint point) {
return minDistanceToTarget(point, new VPoint(0,0));
}
}
......@@ -3,16 +3,13 @@ package org.vadere.util.potential.calculators;
import org.apache.log4j.LogManager;
import org.apache.log4j.Logger;
import org.vadere.util.geometry.shapes.VShape;
import org.vadere.util.math.MathUtil;
import org.vadere.util.potential.CellGrid;
import org.vadere.util.potential.CellState;
import org.vadere.util.potential.PathFindingTag;
import org.vadere.util.potential.timecost.ITimeCostFunction;
import java.awt.*;
import java.io.BufferedWriter;
import java.io.File;
import java.io.FileWriter;
import java.io.IOException;
import java.util.Collection;
import java.util.List;
import java.util.stream.Collectors;
......@@ -35,18 +32,15 @@ public class EikonalSolverFSM implements EikonalSolver {
private CellGrid cellGrid;
private static Logger logger = LogManager.getLogger(EikonalSolverFSM.class);
private ITimeCostFunction timeCostFunction;
private boolean isHighAccuracy;
private List<Point> targetPoints;
private Collection<VShape> targetShapes;
private static final double EPSILON = 0.001;
private static final double epsilon = 0.001;
public EikonalSolverFSM(final CellGrid cellGrid,
final Collection<VShape> targetShapes,
final boolean isHighAccuracy,
final ITimeCostFunction timeCostFunction) {
this.timeCostFunction = timeCostFunction;
this.isHighAccuracy = isHighAccuracy;
this.targetShapes = targetShapes;
this.targetPoints = cellGrid.pointStream().filter(p -> cellGrid.getValue(p).tag == PathFindingTag.Target)
.collect(Collectors.toList());
......@@ -100,6 +94,7 @@ public class EikonalSolverFSM implements EikonalSolver {
int iterations = 0;
boolean allFrozen = false;
int itNumb = 1;
while (!allFrozen) {
itNumb--;
......@@ -111,13 +106,12 @@ public class EikonalSolverFSM implements EikonalSolver {
for (int x = 0; x < cellGrid.getNumPointsX(); x++) {
for (int y = 0; y < cellGrid.getNumPointsY(); y++) {
Point point = new Point(x, y);
if (cellGrid.getValue(point).tag != PathFindingTag.Target
&& cellGrid.getValue(point).tag != PathFindingTag.Obstacle) {
if (isRelevant(point)) {
double p = cellGrid.getValue(point).potential;
double q = Math.min(computeGodunovDifference(point, cellGrid, Direction.ANY), p);
cellGrid.getValue(point).potential = q;
if (Math.abs(q - p) > EPSILON) {
if (Math.abs(q - p) > epsilon) {
allFrozen = false;
}
}
......@@ -128,13 +122,12 @@ public class EikonalSolverFSM implements EikonalSolver {
for (int x = cellGrid.getNumPointsX() - 1; x >= 0; x--) {
for (int y = 0; y < cellGrid.getNumPointsY(); y++) {
Point point = new Point(x, y);
if (cellGrid.getValue(point).tag != PathFindingTag.Target
&& cellGrid.getValue(point).tag != PathFindingTag.Obstacle) {
if (isRelevant(point)) {
double p = cellGrid.getValue(point).potential;
double q = Math.min(computeGodunovDifference(point, cellGrid, Direction.ANY), p);
cellGrid.getValue(point).potential = q;
if (Math.abs(q - p) > EPSILON) {
if (Math.abs(q - p) > epsilon) {
allFrozen = false;
}
}
......@@ -142,16 +135,15 @@ public class EikonalSolverFSM implements EikonalSolver {
}
// third sweep
for (int x = cellGrid.getNumPointsX() - 1; x >= 0; x--) {
for (int y = cellGrid.getNumPointsY() - 1; y >= 0; y--) {
for (int y = cellGrid.getNumPointsY() - 1; y >= 0; y--) {
for (int x = cellGrid.getNumPointsX() - 1; x >= 0; x--) {
Point point = new Point(x, y);
if (cellGrid.getValue(point).tag != PathFindingTag.Target
&& cellGrid.getValue(point).tag != PathFindingTag.Obstacle) {
if (isRelevant(point)) {
double p = cellGrid.getValue(point).potential;
double q = Math.min(computeGodunovDifference(point, cellGrid, Direction.ANY), p);
cellGrid.getValue(point).potential = q;
if (Math.abs(q - p) > EPSILON) {
if (Math.abs(q - p) > epsilon) {
allFrozen = false;
}
}
......@@ -159,16 +151,15 @@ public class EikonalSolverFSM implements EikonalSolver {
}
// fourth sweep
for (int x = 0; x < cellGrid.getNumPointsX(); x++) {
for (int y = cellGrid.getNumPointsY() - 1; y >= 0; y--) {
for (int y = cellGrid.getNumPointsY() - 1; y >= 0; y--) {
for (int x = 0; x < cellGrid.getNumPointsX(); x++) {
Point point = new Point(x, y);
if (cellGrid.getValue(point).tag != PathFindingTag.Target
&& cellGrid.getValue(point).tag != PathFindingTag.Obstacle) {
if (isRelevant(point)) {
double p = cellGrid.getValue(point).potential;
double q = Math.min(computeGodunovDifference(point, cellGrid, Direction.ANY), p);
cellGrid.getValue(point).potential = q;
if (Math.abs(q - p) > EPSILON) {
if (Math.abs(q - p) > epsilon) {
allFrozen = false;
}
}
......@@ -177,6 +168,15 @@ public class EikonalSolverFSM implements EikonalSolver {
}
}
private boolean converged(final double oldValue, final double newValue) {
double diff = Math.abs(oldValue-newValue);
return diff < oldValue * epsilon;
}
private boolean isRelevant(final Point point) {
return cellGrid.getValue(point).tag != PathFindingTag.Target && cellGrid.getValue(point).tag != PathFindingTag.Obstacle;
}
@Override
public ITimeCostFunction getTimeCostFunction() {
return timeCostFunction;
......@@ -184,6 +184,6 @@ public class EikonalSolverFSM implements EikonalSolver {
@Override
public boolean isHighAccuracy() {
return true;
return false;
}
}
......@@ -196,13 +196,9 @@ public class EikonalSolverSFMM implements EikonalSolver {
}
}
private double minDistanceToTarget(final VPoint point) {
private double minDistanceToTarget(final VPoint point, VPoint dp) {
double minDistance = Double.MAX_VALUE;
double tmp;
// create point that lies in the center of the grid cells so that the distances can be
// computed starting there.
VPoint dp = new VPoint(cellGrid.getWidth() / (cellGrid.getNumPointsX() - 1) / 2.0,
cellGrid.getHeight() / (cellGrid.getNumPointsY() - 1) / 2.0);
for (VShape targetShape : targetShapes) {
// negative distances are possible when point is inside the target
tmp = Math.max(0, targetShape.distance(point.add(dp)));
......@@ -213,6 +209,15 @@ public class EikonalSolverSFMM implements EikonalSolver {
return minDistance;
}
private double minDistanceToTargetCentered(final VPoint point) {
return minDistanceToTarget(point, new VPoint(cellGrid.getWidth() / (cellGrid.getNumPointsX() - 1) / 2.0,
cellGrid.getHeight() / (cellGrid.getNumPointsY() - 1) / 2.0));
}
private double minDistanceToTarget(final VPoint point) {
return minDistanceToTarget(point, new VPoint(0,0));
}
private class ComparatorPotentialFieldValueSFMM implements Comparator<Pair<Point, Double>> {
@Override
public int compare(final Pair<Point, Double> o1, final Pair<Point, Double> o2) {
......
......@@ -45,11 +45,11 @@ public class PerformanceSFMM {
cellGrid = new CellGrid(bounds.getWidth(), bounds.getHeight(), resolution, new CellState());
for (VShape shape : targets) {
FloorDiscretizer.setGridValuesForShapeCentered(cellGrid, shape, new CellState(0.0, PathFindingTag.Target));
FloorDiscretizer.setGridValuesForShape(cellGrid, shape, new CellState(0.0, PathFindingTag.Target));
}
for (VShape obstacle : obstacles) {
FloorDiscretizer.setGridValuesForShapeCentered(cellGrid, obstacle, new CellState(Double.MAX_VALUE, PathFindingTag.Obstacle));
FloorDiscretizer.setGridValuesForShape(cellGrid, obstacle, new CellState(Double.MAX_VALUE, PathFindingTag.Obstacle));
}
}
}
......
package org.vadere.util.potential;
import org.apache.log4j.LogManager;
import org.apache.log4j.Logger;
import org.junit.Before;
import org.junit.Test;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.geometry.shapes.VRectangle;
import org.vadere.util.geometry.shapes.VShape;
import org.vadere.util.potential.calculators.EikonalSolver;
import org.vadere.util.potential.calculators.EikonalSolverFIM;
import org.vadere.util.potential.calculators.EikonalSolverFMM;
import org.vadere.util.potential.calculators.EikonalSolverFSM;
import org.vadere.util.potential.timecost.UnitTimeCostFunction;
import java.util.ArrayList;
import java.util.List;
import static org.junit.Assert.assertTrue;
/**
* @author Benedikt Zoennchen
*/
public class TestEikonalSolver {
private Logger logger = LogManager.getLogger(TestEikonalSolver.class);
private double gridWidth = 20;
private double gridHeight = 20;
private double potentialFieldResolution = 0.1;
private double obstacleGridPenalty = 0.1;
private double targetAttractionStrength = 1.0;
private double dx = 0.05;
private double dy = 0.05;
private CellGrid cellGrid;
private double maxError = 0.5;
private VRectangle targetShape = new VRectangle(7.5, 7.5, 5, 5);
private VPoint[] targetPoints = new VPoint[]{ new VPoint(10,10)};
private List<VShape> targetShapes = new ArrayList<>();
@Before
public void setUp() throws Exception {
cellGrid = new CellGrid(gridWidth, gridHeight, potentialFieldResolution, new CellState(Double.MAX_VALUE, PathFindingTag.Undefined));
FloorDiscretizer.setGridValuesForShape(cellGrid, targetShape, new CellState(0.0, PathFindingTag.Target));
targetShapes.add(targetShape);
}
@Test
public void TestFMM() {
double ms = System.currentTimeMillis();
EikonalSolver eikonalSolverFMM = new EikonalSolverFMM(cellGrid, targetShapes, false, new UnitTimeCostFunction());
eikonalSolverFMM.initialize();
testMaxError(eikonalSolverFMM);
double runtimeInMs = System.currentTimeMillis() - ms;
logger.info("FMM (not high accuracy) required " + runtimeInMs + "[ms]");
}
@Test
public void TestFIM() {
double ms = System.currentTimeMillis();
EikonalSolver eikonalSolverFIM = new EikonalSolverFIM(cellGrid, targetShapes, new UnitTimeCostFunction());
eikonalSolverFIM.initialize();
testMaxError(eikonalSolverFIM);
double runtimeInMs = System.currentTimeMillis() - ms;
logger.info("FIM (not high accuracy) required " + runtimeInMs + "[ms]");
}
@Test
public void TestFSM() {
double ms = System.currentTimeMillis();
EikonalSolver eikonalSolverFSM = new EikonalSolverFSM(cellGrid, targetShapes, new UnitTimeCostFunction());
eikonalSolverFSM.initialize();
testMaxError(eikonalSolverFSM);
double runtimeInMs = System.currentTimeMillis() - ms;
logger.info("FSM (not high accuracy) required " + runtimeInMs + "[ms]");
}
private void testMaxError(EikonalSolver eikonalSolver) {
double max = Double.MIN_VALUE;
for(double x = 0; x < gridWidth; x += dx) {
for(double y = 0; y < gridHeight; y += dy) {
double distance = Math.max(0, targetShape.distance(new VPoint(x, y)));
double travellingTime = eikonalSolver.getPotential(new VPoint(x, y), obstacleGridPenalty, targetAttractionStrength);
max = Math.max(max, Math.abs(travellingTime-distance));
}
}
logger.info(eikonalSolver + " max error = " + max);
assertTrue(max <= maxError);
}
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment