Added Bugfixes and unit tests for incremental MST determination

This commit is contained in:
Vladislav Chumak committed 2019-01-15 11:54:33 +01:00
1 parent 682146dbd3
commit 95aef2c0fe
6 files changed
+270 -67

No files matched your search

@@ -0,0 +1,158 @@
package com.sap.sailing.windestimation.aggregator.advancedhmm;
import static org.junit.Assert.assertEquals;
import java.util.List;
import org.junit.Before;
import org.junit.Test;
import com.sap.sailing.windestimation.aggregator.advancedhmm.AbstractAdvancedGraphGenerator.NodeWithDistance;
import com.sap.sailing.windestimation.aggregator.advancedhmm.AbstractAdvancedGraphGenerator.NodeWithNeighbors;
import com.sap.sse.common.Util.Pair;
public class AbstractAdvancedGraphGeneratorTest {
private static final double DOUBLE_TOLERANCE = 0.0000001;
private int counter;
@Before
public void beforeStart() {
counter = 1;
}
@Test
public void testOrderedInsert() {
MockedAdvancedGraphGenerator generator = new MockedAdvancedGraphGenerator();
generator.addNode(tuple(0, 0));
generator.addNode(tuple(3, 3));
generator.addNode(tuple(5, 5));
assertMstCorrect(generator, 10);
}
@Test
public void testWith3ElementsMiddleInsertedLast() {
MockedAdvancedGraphGenerator generator = new MockedAdvancedGraphGenerator();
generator.addNode(tuple(0, 0));
generator.addNode(tuple(5, 5));
generator.addNode(tuple(3, 3));
assertMstCorrect(generator, 10);
}
@Test
public void testCircle() {
MockedAdvancedGraphGenerator generator = new MockedAdvancedGraphGenerator();
generator.addNode(tuple(5, 0));
generator.addNode(tuple(5, 10));
generator.addNode(tuple(0, 5));
generator.addNode(tuple(10, 5));
assertMstCorrect(generator, 30);
}
@Test
public void testCircleWithMiddle() {
MockedAdvancedGraphGenerator generator = new MockedAdvancedGraphGenerator();
generator.addNode(tuple(5, 0));
generator.addNode(tuple(5, 10));
generator.addNode(tuple(0, 5));
generator.addNode(tuple(10, 5));
generator.addNode(tuple(5, 5));
assertMstCorrect(generator, 20);
}
@Test
public void testAdvancedStuff() {
MockedAdvancedGraphGenerator generator = new MockedAdvancedGraphGenerator();
generator.addNode(tuple(0, 0)); // 1
generator.addNode(tuple(10, 0)); // 2
generator.addNode(tuple(20, 0)); // 3
generator.addNode(tuple(5, 4)); // 4
generator.addNode(tuple(14, 3)); // 5
generator.addNode(tuple(9, 4)); // 6
assertMstCorrect(generator, 33);
// 9 (1->4) + 4 (4->6) + 5 (6->2) + 6 (6->5) + 9 (5->3) = 33
}
@Test
public void testAdvancedStuff2() {
MockedAdvancedGraphGenerator generator = new MockedAdvancedGraphGenerator();
generator.addNode(tuple(5, 1)); // 1
generator.addNode(tuple(4, 0)); // 2
generator.addNode(tuple(12, 3)); // 3
generator.addNode(tuple(10, 4)); // 4
generator.addNode(tuple(8, 4)); // 5
generator.addNode(tuple(6, 6)); // 6
generator.addNode(tuple(2, 6)); // 7
generator.addNode(tuple(3, 4)); // 8
generator.addNode(tuple(5, 4)); // 9
assertMstCorrect(generator, 21);
// 2 (2->1) + 3 (1->9) + 2 (9->8) + 3 (8->7) + 3 (6->9) + 3 (9->5) + 2 (5->4) + 3 (4->3) = 21
}
@Test
public void testCircleReverse() {
MockedAdvancedGraphGenerator generator = new MockedAdvancedGraphGenerator();
generator.addNode(tuple(5, 0)); // 1
generator.addNode(tuple(9, 3)); // 2
generator.addNode(tuple(7, 6)); // 3
generator.addNode(tuple(3, 6)); // 4
generator.addNode(tuple(2, 4)); // 5
generator.addNode(tuple(3, 2)); // 6
assertMstCorrect(generator, 19);
// 4 (1->6) + 3 (6->5) + 3 (5->4) + 4 (4->3) + 5 (3->2) = 19
}
private void assertMstCorrect(MockedAdvancedGraphGenerator generator, double targetEdgesWeightSum) {
double calculatedMstWeightSum = getWeightSumOfEdges(generator);
assertEquals(targetEdgesWeightSum, calculatedMstWeightSum, DOUBLE_TOLERANCE);
}
private double getWeightSumOfEdges(MockedAdvancedGraphGenerator generator) {
List<NodeWithNeighbors<Tuple>> nodes = generator.getNodes();
NodeWithNeighbors<Tuple> firstNode = nodes.get(0);
return getWeightSumOfEdges(firstNode, null);
}
private double getWeightSumOfEdges(NodeWithNeighbors<Tuple> node, NodeWithNeighbors<Tuple> previousNode) {
double weightSum = 0;
for (NodeWithDistance<Tuple> neighbor : node.getNeighbors()) {
NodeWithNeighbors<Tuple> neighborNode = neighbor.getNodeWithNeighbors();
if (neighborNode != previousNode) {
System.out.println(node + " connected to " + neighbor);
weightSum += neighbor.getDistance();
weightSum += getWeightSumOfEdges(neighborNode, node);
}
}
return weightSum;
}
private Tuple tuple(Integer a, Integer b) {
return new Tuple(a, b);
}
private static class MockedAdvancedGraphGenerator extends AbstractAdvancedGraphGenerator<Tuple> {
@Override
protected double getDistanceBetweenObservations(Tuple o1, Tuple o2) {
return Math.abs(o1.getA() - o2.getA()) + Math.abs(o1.getB() - o2.getB());
}
}
private class Tuple extends Pair<Integer, Integer> {
private static final long serialVersionUID = -8583758797034450326L;
private int number = counter++;
public Tuple(int a, int b) {
super(a, b);
}
@Override
public String toString() {
return number + ": " + super.toString();
}
}
}
@@ -1,4 +1,4 @@
package com.sap.sailing.windestimation.maneuvergraph.maneuvernode;
package com.sap.sailing.windestimation.aggregator.hmm;
import static org.junit.Assert.assertEquals;
@@ -6,6 +6,8 @@ import java.util.Iterator;
import java.util.LinkedList;
import java.util.List;
import com.sap.sse.common.Util.Triple;
public abstract class AbstractAdvancedGraphGenerator<T> {
private List<NodeWithNeighbors<T>> nodes = new ArrayList<>();
@@ -14,49 +16,49 @@ public abstract class AbstractAdvancedGraphGenerator<T> {
public void addNode(T observation) {
NodeWithNeighbors<T> newNode = new NodeWithNeighbors<>(observation);
LinkedList<NeighborWithDistance<T>> distancesFromOtherNodes = getDistancesFromOtherNodes(observation);
if (distancesFromOtherNodes.isEmpty()) {
nodes.add(newNode);
} else {
LinkedList<NodeWithDistance<T>> distancesFromOtherNodes = getDistancesFromNearestOtherNodes(observation);
nodes.add(newNode);
if (!distancesFromOtherNodes.isEmpty()) {
Collections.sort(distancesFromOtherNodes);
NeighborWithDistance<T> closestNeighborWithDistance = distancesFromOtherNodes.removeFirst();
nodes.add(newNode);
NodeWithDistance<T> closestNeighborWithDistance = distancesFromOtherNodes.removeFirst();
addEdge(newNode, closestNeighborWithDistance);
if (!distancesFromOtherNodes.isEmpty()) {
optimizeEdges(newNode, distancesFromOtherNodes, closestNeighborWithDistance);
}
optimizeEdges(newNode, distancesFromOtherNodes, closestNeighborWithDistance);
}
}
private void optimizeEdges(NodeWithNeighbors<T> newNode,
LinkedList<NeighborWithDistance<T>> distancesFromOtherNodes,
NeighborWithDistance<T> currentNeighborWithDistance) {
NodeWithNeighbors<T> currentNeighbor = currentNeighborWithDistance.getNeighbor();
for (NeighborWithDistance<T> neighborWithDistance : currentNeighbor.getNeighbors()) {
NodeWithNeighbors<T> nextNeighbor = neighborWithDistance.getNeighbor();
double distanceToImprove = neighborWithDistance.getDistance();
for (Iterator<NeighborWithDistance<T>> iterator = distancesFromOtherNodes.iterator(); iterator.hasNext();) {
NeighborWithDistance<T> candidate = iterator.next();
if (distanceToImprove <= candidate.getDistance()) {
if (canReach(newNode, currentNeighbor, newNode)) {
removeEdge(currentNeighbor, nextNeighbor);
addEdge(newNode, candidate);
LinkedList<NodeWithDistance<T>> distancesFromNewNodeToOtherNodes,
NodeWithDistance<T> currentNeighborWithDistance) {
NodeWithNeighbors<T> currentNeighborNode = currentNeighborWithDistance.getNodeWithNeighbors();
List<NodeWithDistance<T>> neighborsOfCurrentNeighborNode = new ArrayList<>(currentNeighborNode.getNeighbors());
for (NodeWithDistance<T> subNeighborWithDistance : neighborsOfCurrentNeighborNode) {
NodeWithNeighbors<T> subNeighborNode = subNeighborWithDistance.getNodeWithNeighbors();
if (subNeighborNode != newNode) {
for (Iterator<NodeWithDistance<T>> iterator = distancesFromNewNodeToOtherNodes.iterator(); iterator
.hasNext();) {
NodeWithDistance<T> candidate = iterator.next();
Triple<Double, NodeWithNeighbors<T>, NodeWithNeighbors<T>> canReachAndEdgeWithHighestDistance = canReach(
candidate.getNodeWithNeighbors(), currentNeighborNode, newNode);
if (canReachAndEdgeWithHighestDistance != null
&& canReachAndEdgeWithHighestDistance.getA() > candidate.getDistance()) {
removeEdge(canReachAndEdgeWithHighestDistance.getB(),
canReachAndEdgeWithHighestDistance.getC());
iterator.remove();
optimizeEdges(newNode, distancesFromOtherNodes, candidate);
addEdge(newNode, candidate);
optimizeEdges(newNode, distancesFromNewNodeToOtherNodes, candidate);
break;
}
} else {
break;
}
}
}
}
private LinkedList<NeighborWithDistance<T>> getDistancesFromOtherNodes(T observation) {
LinkedList<NeighborWithDistance<T>> result = new LinkedList<>();
// TODO implement with Octree so that the octree returns the nearest x elements to observation
private LinkedList<NodeWithDistance<T>> getDistancesFromNearestOtherNodes(T observation) {
LinkedList<NodeWithDistance<T>> result = new LinkedList<>();
for (NodeWithNeighbors<T> node : nodes) {
double distance = getDistanceBetweenObservations(observation, node.getObservation());
NeighborWithDistance<T> neighborWithDistance = new NeighborWithDistance<>(node, distance);
NodeWithDistance<T> neighborWithDistance = new NodeWithDistance<>(node, distance);
result.add(neighborWithDistance);
}
return result;
@@ -64,9 +66,10 @@ public abstract class AbstractAdvancedGraphGenerator<T> {
protected abstract double getDistanceBetweenObservations(T o1, T o2);
private void addEdge(NodeWithNeighbors<T> newNode, NeighborWithDistance<T> closestNeighborWithDistance) {
private void addEdge(NodeWithNeighbors<T> newNode, NodeWithDistance<T> closestNeighborWithDistance) {
newNode.addNeighbor(closestNeighborWithDistance);
closestNeighborWithDistance.getNeighbor().addNeighbor(newNode, closestNeighborWithDistance.getDistance());
closestNeighborWithDistance.getNodeWithNeighbors().addNeighbor(newNode,
closestNeighborWithDistance.getDistance());
}
private void removeEdge(NodeWithNeighbors<T> a, NodeWithNeighbors<T> b) {
@@ -74,32 +77,35 @@ public abstract class AbstractAdvancedGraphGenerator<T> {
b.removeNeighbor(a);
}
private boolean canReach(NodeWithNeighbors<T> nodeToReach, NodeWithNeighbors<T> nodeToStartFrom,
NodeWithNeighbors<T> firstNodeOfPathToIgnore) {
return canReach(nodeToReach, nodeToStartFrom, firstNodeOfPathToIgnore, 0);
private Triple<Double, NodeWithNeighbors<T>, NodeWithNeighbors<T>> canReach(NodeWithNeighbors<T> nodeToReach,
NodeWithNeighbors<T> nodeToStartFrom, NodeWithNeighbors<T> firstNodeOfPathToIgnore) {
return canReach(nodeToReach, nodeToStartFrom, firstNodeOfPathToIgnore, 0,
new Triple<>(0.0, nodeToReach, nodeToStartFrom));
}
private boolean canReach(NodeWithNeighbors<T> nodeToReach, NodeWithNeighbors<T> nodeToStartFrom,
NodeWithNeighbors<T> firstNodeOfPathToIgnore, int currentDepth) {
private Triple<Double, NodeWithNeighbors<T>, NodeWithNeighbors<T>> canReach(NodeWithNeighbors<T> nodeToReach,
NodeWithNeighbors<T> nodeToStartFrom, NodeWithNeighbors<T> firstNodeOfPathToIgnore, int currentDepth,
Triple<Double, NodeWithNeighbors<T>, NodeWithNeighbors<T>> edgeWithHighestDistance) {
if (nodeToStartFrom == nodeToReach) {
return true;
}
for (NeighborWithDistance<T> neighborWithDistance : nodeToStartFrom.getNeighbors()) {
NodeWithNeighbors<T> node = neighborWithDistance.getNeighbor();
if (nodeToReach == node) {
return true;
}
return edgeWithHighestDistance;
}
if (currentDepth < MAX_DEPTH_FOR_DFS) {
int nextDepth = currentDepth + 1;
for (NeighborWithDistance<T> neighborWithDistance : nodeToStartFrom.getNeighbors()) {
NodeWithNeighbors<T> node = neighborWithDistance.getNeighbor();
if (canReach(nodeToReach, node, nodeToStartFrom, nextDepth)) {
return true;
for (NodeWithDistance<T> neighborWithDistance : nodeToStartFrom.getNeighbors()) {
NodeWithNeighbors<T> node = neighborWithDistance.getNodeWithNeighbors();
if (node != firstNodeOfPathToIgnore) {
double distance = neighborWithDistance.getDistance();
Triple<Double, NodeWithNeighbors<T>, NodeWithNeighbors<T>> newEdgeWithHighestDistance = distance > edgeWithHighestDistance
.getA() ? new Triple<>(distance, nodeToStartFrom, node) : edgeWithHighestDistance;
Triple<Double, NodeWithNeighbors<T>, NodeWithNeighbors<T>> canReachAndEdgeWithHighestDistance = canReach(
nodeToReach, node, nodeToStartFrom, nextDepth, newEdgeWithHighestDistance);
if (canReachAndEdgeWithHighestDistance != null) {
return canReachAndEdgeWithHighestDistance;
}
}
}
}
return false;
return null;
}
public List<NodeWithNeighbors<T>> getNodes() {
@@ -107,7 +113,7 @@ public abstract class AbstractAdvancedGraphGenerator<T> {
}
protected static class NodeWithNeighbors<T> {
private final List<NeighborWithDistance<T>> neighbors = new ArrayList<>(2);
private final List<NodeWithDistance<T>> neighbors = new ArrayList<>(2);
private final T observation;
public NodeWithNeighbors(T observation) {
@@ -115,11 +121,11 @@ public abstract class AbstractAdvancedGraphGenerator<T> {
}
public void addNeighbor(NodeWithNeighbors<T> nodeWithNeighbors, double distance) {
NeighborWithDistance<T> neighborWithDistance = new NeighborWithDistance<>(nodeWithNeighbors, distance);
NodeWithDistance<T> neighborWithDistance = new NodeWithDistance<>(nodeWithNeighbors, distance);
addNeighbor(neighborWithDistance);
}
public void addNeighbor(NeighborWithDistance<T> neighborWithDistance) {
public void addNeighbor(NodeWithDistance<T> neighborWithDistance) {
neighbors.add(neighborWithDistance);
}
@@ -128,25 +134,50 @@ public abstract class AbstractAdvancedGraphGenerator<T> {
neighbors.remove(nodeWithNeighbors);
}
public List<NeighborWithDistance<T>> getNeighbors() {
public List<NodeWithDistance<T>> getNeighbors() {
return neighbors;
}
public T getObservation() {
return observation;
}
@Override
public String toString() {
return "NodeWithNeighbors [observation=" + observation + "]";
}
@Override
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + ((observation == null) ? 0 : observation.hashCode());
return result;
}
@Override
public boolean equals(Object obj) {
if (this == obj)
return true;
if (obj instanceof NodeWithDistance<?>) {
if (((NodeWithDistance<?>) obj).neighbor == this) {
return true;
}
}
return false;
}
}
protected static class NeighborWithDistance<T> implements Comparable<NeighborWithDistance<T>> {
protected static class NodeWithDistance<T> implements Comparable<NodeWithDistance<T>> {
private final NodeWithNeighbors<T> neighbor;
private final double distance;
public NeighborWithDistance(NodeWithNeighbors<T> neighbor, double distance) {
public NodeWithDistance(NodeWithNeighbors<T> neighbor, double distance) {
this.neighbor = neighbor;
this.distance = distance;
}
public NodeWithNeighbors<T> getNeighbor() {
public NodeWithNeighbors<T> getNodeWithNeighbors() {
return neighbor;
}
@@ -155,7 +186,7 @@ public abstract class AbstractAdvancedGraphGenerator<T> {
}
@Override
public int compareTo(NeighborWithDistance<T> o) {
public int compareTo(NodeWithDistance<T> o) {
return Double.compare(distance, o.getDistance());
}
@@ -170,6 +201,12 @@ public abstract class AbstractAdvancedGraphGenerator<T> {
return true;
return false;
}
@Override
public String toString() {
return "NodeWithDistance [neighbor=" + neighbor + ", distance=" + distance + "]";
}
}
}
@@ -32,21 +32,27 @@ public class AdvancedManeuverGraphGenerator
NodeWithNeighbors<ManeuverWithProbabilisticTypeClassification> firstNode = nodes.get(0);
AdvancedGraphLevel firstGraphLevel = new AdvancedGraphLevel(firstNode.getObservation(),
transitionProbabilitiesCalculator);
parseGraphFromNodes(firstNode.getNeighbors(), firstGraphLevel, leafs);
parseGraphFromNodes(firstNode, firstGraphLevel, null, leafs);
AdvancedManeuverGraphComponents graphComponents = new AdvancedManeuverGraphComponents(firstGraphLevel, leafs);
return graphComponents;
}
private void parseGraphFromNodes(List<NeighborWithDistance<ManeuverWithProbabilisticTypeClassification>> nodes,
AdvancedGraphLevel parent, List<AdvancedGraphLevel> leafs) {
if (nodes.isEmpty()) {
leafs.add(parent);
private void parseGraphFromNodes(NodeWithNeighbors<ManeuverWithProbabilisticTypeClassification> previousNode,
AdvancedGraphLevel previousGraphLevel,
NodeWithNeighbors<ManeuverWithProbabilisticTypeClassification> parentOfPreviousNode,
List<AdvancedGraphLevel> leafs) {
List<NodeWithDistance<ManeuverWithProbabilisticTypeClassification>> childNodes = previousNode.getNeighbors();
if (childNodes.size() == 1) {
leafs.add(previousGraphLevel);
} else {
for (NeighborWithDistance<ManeuverWithProbabilisticTypeClassification> nodeWithDistance : nodes) {
NodeWithNeighbors<ManeuverWithProbabilisticTypeClassification> node = nodeWithDistance.getNeighbor();
AdvancedGraphLevel newGraphLevel = parent.addChild(nodeWithDistance.getDistance(),
node.getObservation(), transitionProbabilitiesCalculator);
parseGraphFromNodes(node.getNeighbors(), newGraphLevel, leafs);
for (NodeWithDistance<ManeuverWithProbabilisticTypeClassification> childNodeWithDistance : childNodes) {
NodeWithNeighbors<ManeuverWithProbabilisticTypeClassification> childNode = childNodeWithDistance
.getNodeWithNeighbors();
if (childNode != parentOfPreviousNode) {
AdvancedGraphLevel newGraphLevel = previousGraphLevel.addChild(childNodeWithDistance.getDistance(),
childNode.getObservation(), transitionProbabilitiesCalculator);
parseGraphFromNodes(childNode, newGraphLevel, previousNode, leafs);
}
}
}
}
@@ -12,6 +12,7 @@ public class DistanceAndDurationAwareWindTransitionProbabilitiesCalculator
extends SimpleIntersectedWindRangeBasedTransitionProbabilitiesCalculator
implements AdvancedGraphNodeTransitionProbabilitiesCalculator {
private static final double LA_PLACE_TRANSITION_PROBABILITY = 0.0001;
private final GaussianBasedTwdTransitionDistributionCache gaussianBasedTwdTransitionDistributionCache;
public DistanceAndDurationAwareWindTransitionProbabilitiesCalculator(
@@ -24,7 +25,7 @@ public class DistanceAndDurationAwareWindTransitionProbabilitiesCalculator
@Override
protected double getPenaltyFactorForTransition(TwdTransition twdTransition) {
double penaltyFactor = gaussianBasedTwdTransitionDistributionCache.getP(twdTransition);
return penaltyFactor + 0.05;
return penaltyFactor + LA_PLACE_TRANSITION_PROBABILITY;
}
public double getCompoundDistance(ManeuverForEstimation fromManeuver, ManeuverForEstimation toManeuver) {
@@ -13,6 +13,7 @@ import com.sap.sse.common.impl.DegreeBearingImpl;
public class IntersectedWindRangeBasedTransitionProbabilitiesCalculator
implements GraphNodeTransitionProbabilitiesCalculator {
private static final double LA_PLACE_TRANSITION_PROBABILITY = 0.001;
protected static final int MIN_BEATING_ANGLE_PLUS_MIN_RUNNING_ANGLE = 40;
private static final double MAX_ABS_WIND_COURSE_DEVIATION_TOLERANCE_WITHIN_ANALYSIS_INTERVAL_IN_DEGREES = 40;
private final boolean propagateIntersectedWindRangeOfHeadupAndBearAway;
@@ -69,7 +70,7 @@ public class IntersectedWindRangeBasedTransitionProbabilitiesCalculator
penaltyFactor = 1 / (1 + (Math.pow(violationRange, 2)));
}
}
return penaltyFactor + 0.05;
return penaltyFactor + LA_PLACE_TRANSITION_PROBABILITY;
}
public WindCourseRange getWindCourseRangeForManeuverType(ManeuverForEstimation maneuver,