Commit e38b3d59 authored by Benedikt Zoennchen's avatar Benedikt Zoennchen
Browse files

issue #146 should be fixed, but has to be tested with each model.

parent e9c6aea5
......@@ -10,119 +10,152 @@ import java.util.stream.IntStream;
abstract class GaussianFilter implements IGaussianFilter {
/**
* the scale of the images respect to the scenario width and heigt and based on the
* gridresolution of the potential field grid.
*/
protected final double scale;
/** the width of the scenario. */
protected final int scenarioWidth;
/** the height of the scenario. */
protected final int scenarioHeight;
protected float[] inputMatrix;
protected float[] outputMatrix;
protected float[] kernel;
protected int kernelWidth;
protected int kernelHeight;
protected int matrixWidth;
protected int matrixHeight;
private static Logger logger = LogManager.getLogger(GaussianFilter.class);
GaussianFilter(final Rectangle2D scenarioBounds, final double scale, final BiFunction<Integer, Integer, Float> f,
final boolean noramized) {
this.scale = scale;
this.scenarioWidth = (int) (Math.ceil(scenarioBounds.getWidth())) + 1;
this.scenarioHeight = (int) (Math.ceil(scenarioBounds.getHeight())) + 1;
this.matrixWidth = (int) (Math.ceil(scenarioWidth * scale));
this.matrixHeight = (int) (Math.ceil(scenarioHeight * scale));
this.inputMatrix = new float[matrixWidth * matrixHeight];
kernelWidth = (int) (9 * scale) + 1;
// kernelWidth = 31;
kernelWidth = kernelWidth % 2 == 0 ? kernelWidth + 1 : kernelWidth;
kernelHeight = kernelWidth;
this.kernel = Convolution.floatGaussian1DKernel(kernelWidth, f, noramized);
//this.kernel = Convolution.generateFloatGaussianKernel(kernelWidth, 0.1f);
}
@Override
public void clear() {
this.inputMatrix = new float[matrixWidth * matrixHeight];
}
@Override
public double getFilteredValue(final double x, final double y) {
if (x < 0 || y < 0) {
throw new IllegalArgumentException("x(" + x + ") or y(" + y + ") < 0.");
}
return getFilteredValue((int) Math.round(x * scale), (int) Math.round(y * scale));
}
public double getFilteredValue(final int x, final int y) {
if (x < 0 || y < 0) {
throw new IllegalArgumentException("x(" + x + ") or y(" + y + ") < 0.");
}
int index = matrixWidth * y + x;
if (index >= outputMatrix.length) {
logger.warn("index(" + index + ") is out matrix range");
return 0.0;
// throw new IllegalArgumentException("index("+index+") is out matrix range");
}
/**
* the scale of the images respect to the scenario width and heigt and based on the
* gridresolution of the potential field grid.
*/
protected final double scale;
/** the width of the scenario. */
protected final int scenarioWidth;
/** the height of the scenario. */
protected final int scenarioHeight;
protected float[] inputMatrix;
protected float[] outputMatrix;
protected float[] kernel;
protected int kernelWidth;
protected int kernelHeight;
protected int matrixWidth;
protected int matrixHeight;
private Rectangle2D bound;
private static Logger logger = LogManager.getLogger(GaussianFilter.class);
GaussianFilter(final Rectangle2D scenarioBounds, final double scale, final BiFunction<Integer, Integer, Float> f,
final boolean noramized) {
this.scale = scale;
this.bound = scenarioBounds;
this.scenarioWidth = (int) (Math.ceil(scenarioBounds.getWidth())) + 1;
this.scenarioHeight = (int) (Math.ceil(scenarioBounds.getHeight())) + 1;
this.matrixWidth = (int) (Math.ceil(scenarioWidth * scale));
this.matrixHeight = (int) (Math.ceil(scenarioHeight * scale));
this.inputMatrix = new float[matrixWidth * matrixHeight];
kernelWidth = (int) (9 * scale) + 1;
// kernelWidth = 31;
kernelWidth = kernelWidth % 2 == 0 ? kernelWidth + 1 : kernelWidth;
kernelHeight = kernelWidth;
this.kernel = Convolution.floatGaussian1DKernel(kernelWidth, f, noramized);
//this.kernel = Convolution.generateFloatGaussianKernel(kernelWidth, 0.1f);
}
@Override
public void clear() {
this.inputMatrix = new float[matrixWidth * matrixHeight];
}
@Override
public double getFilteredValue(final double x, final double y) {
if (x < bound.getMinX() || y < bound.getMinY()) {
throw new IllegalArgumentException("x(" + x + ") or y(" + y + ") < 0.");
}
return getFilteredValue(toXIndex(x), toYIndex(y));
}
public double getFilteredValue(final int x, final int y) {
if (x < 0 || y < 0) {
throw new IllegalArgumentException("x(" + x + ") or y(" + y + ") < 0.");
}
int index = matrixWidth * y + x;
if (index >= outputMatrix.length) {
logger.warn("index(" + index + ") is out matrix range");
return 0.0;
// throw new IllegalArgumentException("index("+index+") is out matrix range");
}
/*
* if(outputMatrix[index] > 0) {
* logger.info("index(" + index + "): " + outputMatrix[index]);
* }
*/
return outputMatrix[index];
}
@Override
public void setInputValue(final int x, final int y, double value) {
inputMatrix[matrixWidth * y + x] = (float) value;
}
@Override
public void setInputValue(final double x, final double y, double value) {
inputMatrix[matrixWidth * (int) Math.round(y * scale) + (int) Math.round(x * scale)] = (float) value;
}
@Override
public double getScale() {
return scale;
}
@Override
public int getMatrixWidth() {
return matrixWidth;
}
@Override
public int getMatrixHeight() {
return matrixHeight;
}
@Override
public double getMaxFilteredValue() {
return IntStream.range(0, outputMatrix.length).mapToDouble(i -> outputMatrix[i]).max().orElse(0.0);
}
@Override
public double getMinFilteredValue() {
return IntStream.range(0, outputMatrix.length).mapToDouble(i -> outputMatrix[i]).min().orElse(0.0);
}
@Override
public double getInputValue(int x, int y) {
return inputMatrix[matrixWidth * y + x];
}
return outputMatrix[index];
}
@Override
public void setInputValue(final int x, final int y, double value) {
inputMatrix[matrixWidth * y + x] = (float) value;
}
@Override
public void setInputValue(final double x, final double y, double value) {
inputMatrix[matrixWidth * toYIndex(y) + toXIndex(x)] = (float) value;
}
@Override
public double toXCoord(int xIndex) {
return bound.getMinX() + xIndex / scale;
}
@Override
public double toYCoord(int yIndex) {
return bound.getMinY() + yIndex / scale;
}
@Override
public int toXIndex(final double x) {
return (int) Math.round((x - bound.getMinX()) * scale);
}
@Override
public int toYIndex(final double y) {
return (int) Math.round((y - bound.getMinY()) * scale);
}
@Override
public int toFloorXIndex(double x) {
return (int) Math.floor((x - bound.getMinX()) * scale);
}
@Override
public int toFloorYIndex(double y) {
return (int) Math.floor((y - bound.getMinY()) * scale);
}
@Override
public double getScale() {
return scale;
}
@Override
public int getMatrixWidth() {
return matrixWidth;
}
@Override
public int getMatrixHeight() {
return matrixHeight;
}
@Override
public double getMaxFilteredValue() {
return IntStream.range(0, outputMatrix.length).mapToDouble(i -> outputMatrix[i]).max().orElse(0.0);
}
@Override
public double getMinFilteredValue() {
return IntStream.range(0, outputMatrix.length).mapToDouble(i -> outputMatrix[i]).min().orElse(0.0);
}
@Override
public double getInputValue(int x, int y) {
return inputMatrix[matrixWidth * y + x];
}
}
......@@ -70,10 +70,15 @@ public interface IGaussianFilter {
int toYIndex(final double y);
int toFloorXIndex(final double x);
int toFloorYIndex(final double y);
double toXCoord(int xIndex);
double toYCoord(int yIndex);
/**
* This method has to be called if the Filter will no longer called!
*/
......
......@@ -78,6 +78,16 @@ public class ObstacleGaussianFilter implements IGaussianFilter {
return filter.toYIndex(y);
}
@Override
public int toFloorXIndex(double x) {
return filter.toFloorXIndex(x);
}
@Override
public int toFloorYIndex(double y) {
return filter.toFloorYIndex(y);
}
@Override
public double toXCoord(int xIndex) {
return filter.toXCoord(xIndex);
......
......@@ -58,7 +58,7 @@ public class PedestrianGaussianFilter<E extends Pedestrian> implements IGaussian
}
@Override
public void setInputValue(double x, double y, double value) {
public void setInputValue(double x, double y, double value) {
filter.setInputValue(x, y, value);
}
......@@ -108,6 +108,16 @@ public class PedestrianGaussianFilter<E extends Pedestrian> implements IGaussian
return filter.toYIndex(y);
}
@Override
public int toFloorXIndex(double x) {
return filter.toFloorXIndex(x);
}
@Override
public int toFloorYIndex(double y) {
return filter.toFloorYIndex(y);
}
@Override
public double toXCoord(int xIndex) {
return filter.toXCoord(xIndex);
......@@ -127,75 +137,33 @@ public class PedestrianGaussianFilter<E extends Pedestrian> implements IGaussian
VPoint filteredPosition = pedestrian.getPosition();
//VPoint filteredPosition = new VPoint(Math.max(0, position.x), Math.max(0, position.y));
// better approximation
int indexX = toXIndex(filteredPosition.x);
int indexY = toYIndex(filteredPosition.y);
// maybe find a better approximation. Here we extrapolate by the area of 4 rectangles.
int lowerLeftX = toFloorXIndex(filteredPosition.x);
int lowerLeftY = toFloorYIndex(filteredPosition.y);
double coordX = toXCoord(indexX);
double coordY = toYCoord(indexY);
double coordX = toXCoord(lowerLeftX+1);
double coordY = toYCoord(lowerLeftY+1);
int dx = toXCoord(indexX) > filteredPosition.x ? 1 : -1;
int dy = toYCoord(indexY) > filteredPosition.y ? 1 : -1;
double dx = Math.abs(coordX - filteredPosition.x) * getScale();
double dy = Math.abs(coordY - filteredPosition.y) * getScale();
//TODO: reactivate the split but refactor the code.
/* if (Double.compare(coordX, filteredPosition.x) == 0
&& Double.compare(coordY, filteredPosition.y) == 0) {
setInputValue(filteredPosition.x, filteredPosition.y,
getInputValue(indexX, indexY) + pedestrianLoadingStrategy.calculateLoading(pedestrian));
} else if (Double.compare(coordX, filteredPosition.x) == 0 && Double.compare(coordY, filteredPosition.y) != 0) {
splitY(filteredPosition, indexX, indexY, pedestrian);
splitY(filteredPosition, indexX, indexY + dy, pedestrian);
} else if (Double.compare(coordX, filteredPosition.x) != 0 && Double.compare(coordY, filteredPosition.y) == 0) {
splitX(filteredPosition, indexX, indexY, pedestrian);
splitX(filteredPosition, indexX + dx, indexY, pedestrian);
} else {
double max = getScale() * getScale();
splitXY(filteredPosition, indexX, indexY, pedestrian);
splitXY(filteredPosition, indexX, indexY + dy, pedestrian);
splitXY(filteredPosition, indexX + dx, indexY, pedestrian);
splitXY(filteredPosition, indexX + dx, indexY + dy, pedestrian);
}*/
double w1 = dx * dy;
double w2 = dx * (1.0 - dy);
double w3 = (1.0 - dx) * dy;
double w4 = (1.0 - dx) * (1.0 - dy);
setInputValue(filteredPosition.x, filteredPosition.y, pedestrianLoadingStrategy.calculateLoading(pedestrian));
}
private void splitXY(final VPoint filteredPosition, final int indexX, final int indexY, Pedestrian pedestrian) {
if (checkIndices(indexX, indexY)) {
double dx = Math.abs(filteredPosition.x * getScale() - indexX);
double dy = Math.abs(filteredPosition.y * getScale() - indexY);
assert Math.abs((w1+w2+w3+w4) - 1.0) < 0.00001;
double weight = ((1.0 - dx) + (1.0 - dy)) / 4.0;
// double weight = Math.exp(-(dx * dx + dy * dy) / (2 * 0.7 * 0.7));
setInputValue(indexX, indexY,
getInputValue(indexX, indexY) + pedestrianLoadingStrategy.calculateLoading(pedestrian) * weight);
}
}
double value = pedestrianLoadingStrategy.calculateLoading(pedestrian);
private void splitX(final VPoint filteredPosition, final int indexX, final int indexY, Pedestrian pedestrian) {
if (checkIndices(indexX, indexY)) {
double dx = Math.abs(filteredPosition.x * getScale() - indexX);
double weight = (1.0 - dx);
// double weight = Math.exp(-(dx * dx + dy * dy) / (2 * 0.7 * 0.7));
setInputValue(indexX, indexY,
getInputValue(indexX, indexY) + pedestrianLoadingStrategy.calculateLoading(pedestrian) * weight);
}
setInputValue(lowerLeftX + 1, lowerLeftY + 1, value * w1);
setInputValue(lowerLeftX + 1, lowerLeftY, value * w2);
setInputValue(lowerLeftX, lowerLeftY + 1, value * w3);
setInputValue(lowerLeftX, lowerLeftY, value * w4);
}
private void splitY(final VPoint filteredPosition, final int indexX, final int indexY, Pedestrian pedestrian) {
if (checkIndices(indexX, indexY)) {
double dy = Math.abs(filteredPosition.y * getScale() - indexY);
double weight = (1.0 - dy);
// double weight = Math.exp(-(dx * dx + dy * dy) / (2 * 0.7 * 0.7));
setInputValue(indexX, indexY,
getInputValue(indexX, indexY) + pedestrianLoadingStrategy.calculateLoading(pedestrian) * weight);
}
}
private boolean checkIndices(int x, int y) {
return x >= 0 && y >= 0 && x < filter.getMatrixWidth() && y < filter.getMatrixHeight();
}
private void setValues() {
clear();
......
Supports Markdown
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