Exemplo n.º 1
0
  @Test
  public void testPathStateConvert2() {
    final Path startPath =
        TestUtils.makeTmpPath(graph, false, new Coordinate(0, 0), new Coordinate(10, 0));

    final Matrix covar =
        MatrixFactory.getDefault()
            .copyArray(new double[][] {new double[] {126.56, 8.44}, new double[] {8.44, 0.56}});
    final MultivariateGaussian startBelief =
        new AdjMultivariateGaussian(VectorFactory.getDenseDefault().createVector2D(0d, 1d), covar);
    final PathStateDistribution currentBelief = new PathStateDistribution(startPath, startBelief);

    final Vector groundLoc =
        MotionStateEstimatorPredictor.getOg()
            .times(currentBelief.getGroundDistribution().getMean());
    AssertJUnit.assertEquals("initial state x", 0d, groundLoc.getElement(0), 0d);
    AssertJUnit.assertEquals("initial state y", 0d, groundLoc.getElement(1), 0d);

    final Path newPath =
        TestUtils.makeTmpPath(
            graph,
            true,
            new Coordinate(20, -10),
            new Coordinate(10, -10),
            new Coordinate(10, 0),
            new Coordinate(0, 0));

    final PathStateDistribution result = currentBelief.convertToPath(newPath);

    AssertJUnit.assertEquals("distance", -0d, result.getMean().getElement(0), 0d);
    AssertJUnit.assertEquals("velocity", -1d, result.getMean().getElement(1), 0d);
  }
Exemplo n.º 2
0
  /** Path & edge are the same, directions are reverse, neg. to pos. */
  @Test
  public void testPathStateConvert7() {

    final Path startPath =
        TestUtils.makeTmpPath(
            graph,
            true,
            new Coordinate(20, -10),
            new Coordinate(10, -10),
            new Coordinate(10, 0),
            new Coordinate(0, 0));
    final Matrix covar =
        MatrixFactory.getDefault()
            .copyArray(new double[][] {new double[] {126.56, 8.44}, new double[] {8.44, 0.56}});
    final MultivariateGaussian startBelief =
        new AdjMultivariateGaussian(VectorFactory.getDefault().createVector2D(-2.5d, 1d), covar);
    final PathStateDistribution currentBelief = new PathStateDistribution(startPath, startBelief);

    final Path newPath =
        TestUtils.makeTmpPath(graph, false, new Coordinate(10, 0), new Coordinate(0, 0));

    final PathStateDistribution result = currentBelief.convertToPath(newPath);

    AssertJUnit.assertEquals("distance", 7.5d, result.getMean().getElement(0), 0d);
    AssertJUnit.assertEquals("velocity", 1d, result.getMean().getElement(1), 0d);
  }
Exemplo n.º 3
0
  /** Test conversion from a positive state to a negative one with an opposite geom. */
  @Test
  public void testGetStateOnPath() {

    final Path startPath =
        TestUtils.makeTmpPath(graph, false, new Coordinate(0, 0), new Coordinate(0, 60));

    final PathState startState =
        new PathState(startPath, VectorFactory.getDefault().createVector2D(12d, -1));

    final Path newPath =
        TestUtils.makeTmpPath(graph, true, new Coordinate(0, 60), new Coordinate(0, 0));

    final Vector result = startState.convertToPath(newPath);

    AssertJUnit.assertEquals("dist", -12d, result.getElement(0), 0d);
    AssertJUnit.assertEquals("dist", 1d, result.getElement(1), 0d);
  }
Exemplo n.º 4
0
  @Test(enabled = false, expectedExceptions = IllegalStateException.class)
  public void testBadConversion() {
    final Path startPath =
        TestUtils.makeTmpPath(graph, true, new Coordinate(20, -10), new Coordinate(10, -10));

    final Matrix covar =
        MatrixFactory.getDefault()
            .copyArray(new double[][] {new double[] {126.56, 8.44}, new double[] {8.44, 0.56}});
    final MultivariateGaussian startBelief =
        new AdjMultivariateGaussian(
            VectorFactory.getDefault().createVector2D(-0d, -5d / 30d), covar);
    final PathStateDistribution currentBelief = new PathStateDistribution(startPath, startBelief);

    final Path newPath =
        TestUtils.makeTmpPath(
            graph,
            true,
            new Coordinate(20, -10),
            new Coordinate(10, -10),
            new Coordinate(10, 0),
            new Coordinate(0, 0));

    PathUtils.checkAndGetConvertedBelief(currentBelief, newPath);
  }
Exemplo n.º 5
0
  /** Test around the start of the edge, positive. */
  @Test
  public void testPathEdge1() {
    final Path path1 =
        TestUtils.makeTmpPath(graph, false, new Coordinate(0, 10.11d), new Coordinate(10.11d, 20d));

    final PathEdge edge1 = Iterables.getFirst(path1.getPathEdges(), null);

    final Vector testVec1 = VectorFactory.getDenseDefault().createVector2D(-1e-12d, -1d);
    final Vector result1 = edge1.getCheckedStateOnEdge(testVec1, 1e-7d, false);

    AssertJUnit.assertEquals(0d, result1.getElement(0), 0d);

    final Vector testVec2 = VectorFactory.getDenseDefault().createVector2D(1e-12d, -1d);
    final Vector result2 = edge1.getCheckedStateOnEdge(testVec2, 1e-7d, false);

    AssertJUnit.assertEquals(1e-12d, result2.getElement(0), 0d);

    final Vector testVec3 = VectorFactory.getDenseDefault().createVector2D(-1e-6d, -1d);
    final Vector result3 = edge1.getCheckedStateOnEdge(testVec3, 1e-7d, false);

    AssertJUnit.assertEquals(null, result3);
  }