Esempio n. 1
0
  public AngleLocalization(
      List<PolygonObstacle> cSpaceObstacles,
      Rectangle2D.Double cSpaceWorld,
      Point2D.Double robotStart,
      Fiducial[] fiducialPairs) {

    this.cSpaceObstacles = cSpaceObstacles;
    this.cSpaceWorld = cSpaceWorld;
    this.robotStart = robotStart;
    this.fiducialPairs = fiducialPairs;

    particles = new ArrayList<OdoPoint>();
    probabilities = new ArrayList<Double>();

    double delta_x = 0;
    double delta_y = 0;
    double delta_theta = 0;
    for (int i = 0; i < NUM_PARTICLES; i++) {
      delta_x = cSpaceWorld.getWidth() * INITIAL_NOISE * (1 - 2 * Math.random());
      delta_y = cSpaceWorld.getHeight() * INITIAL_NOISE * (1 - 2 * Math.random());
      delta_theta = 2 * Math.PI * INITIAL_NOISE * (1 - 2 * Math.random());
      particles.add(new OdoPoint(robotStart.x + delta_x, robotStart.y + delta_y, delta_theta));
      probabilities.add(1.0 / NUM_PARTICLES);
    }

    prev_odo_x = robotStart.x;
    prev_odo_y = robotStart.y;
    prev_odo_theta = 0;
    localPoint = new OdoPoint(robotStart.x, robotStart.y, 0);
  }
Esempio n. 2
0
 @Override
 public void updateOverlay(final Figure figure, final OverlayView overlay) {
   super.updateOverlay(figure, overlay);
   final RectangleOverlay rOverlay = downcastOverlay(overlay.getData());
   final RectangleRegionOfInterest roi = rOverlay.getRegionOfInterest();
   final Rectangle2D.Double bounds = figure.getBounds();
   roi.setOrigin(bounds.getMinX(), 0);
   roi.setOrigin(bounds.getMinY(), 1);
   roi.setExtent(bounds.getWidth(), 0);
   roi.setExtent(bounds.getHeight(), 1);
 }
Esempio n. 3
0
  public static void loadingProgress(int prog) {
    if (loadingScreen != null) {
      // Color for the background of progressArea
      loadingGraphics.setPaint(new Color(0, 0, 0, 0));
      loadingGraphics.fill(loadingProgressArea);
      // Color for the border of progressArea
      loadingGraphics.setPaint(Color.WHITE);
      loadingGraphics.draw(loadingProgressArea);

      int x = (int) loadingProgressArea.getMinX();
      int y = (int) loadingProgressArea.getMinY();

      int width = (int) loadingProgressArea.getWidth();
      int height = (int) loadingProgressArea.getHeight();

      loadingGraphics.setPaint(
          new GradientPaint(325, 0, new Color(0, 2, 76), width + 325, 0, new Color(58, 174, 249)));
      loadingGraphics.fillRect(x + 4, y + 4, prog - 8, height - 6);

      loadingScreen.update();
    }
  }
Esempio n. 4
0
  public Localization(
      List<PolygonObstacle> cSpaceObstacles,
      Rectangle2D.Double cSpaceWorld,
      Point2D.Double robotStart,
      Map<int[], Point2D.Double> fiducialPairs) {

    this.cSpaceObstacles = cSpaceObstacles;
    this.cSpaceWorld = cSpaceWorld;
    this.robotStart = robotStart;
    this.fiducialPairs = fiducialPairs;

    particles = new ArrayList<Point2D.Double>();
    probabilities = new ArrayList<Double>();

    double delta_x = 0;
    double delta_y = 0;
    for (int i = 0; i < NUM_PARTICLES; i++) {
      delta_x = cSpaceWorld.getWidth() * INITIAL_NOISE * (2 * Math.random() - 1);
      delta_y = cSpaceWorld.getHeight() * INITIAL_NOISE * (2 * Math.random() - 1);
      particles.add(new Point2D.Double(robotStart.x + delta_x, robotStart.y + delta_y));
      probabilities.add(1.0 / NUM_PARTICLES);
    }
  }
Esempio n. 5
0
  public void render(Graphics2D g2d, IProgressMonitor monitor) throws RenderException {
    try {
      final IRenderContext currentContext = getContext();
      currentContext.setStatus(ILayer.WAIT);
      CoordinateReferenceSystem destinationCRS = currentContext.getCRS();

      // the bounds of the visible area in world coordinates
      // get the envelope and the screen extent
      Envelope envelope = getRenderBounds();
      if (envelope == null || envelope.isNull()) {
        envelope = context.getImageBounds();
      }

      Point upperLeft =
          currentContext.worldToPixel(new Coordinate(envelope.getMinX(), envelope.getMinY()));
      Point bottomRight =
          currentContext.worldToPixel(new Coordinate(envelope.getMaxX(), envelope.getMaxY()));
      Rectangle screenSize = new Rectangle(upperLeft);
      screenSize.add(bottomRight);

      final IGeoResource resource = getContext().getGeoResource();
      if (resource == null || !resource.canResolve(JGrassMapGeoResource.class)) {
        return;
      }
      JGrassMapGeoResource grassMapGeoResource =
          resource.resolve(JGrassMapGeoResource.class, monitor);

      JGrassRegion fileWindow = new JGrassRegion(grassMapGeoResource.getFileWindow());
      JGrassMapsetGeoResource parent =
          (JGrassMapsetGeoResource) grassMapGeoResource.parent(new NullProgressMonitor());
      CoordinateReferenceSystem grassCrs = parent.getLocationCrs();
      JGrassRegion screenDrawWindow =
          new JGrassRegion(
              envelope.getMinX(),
              envelope.getMaxX(),
              envelope.getMinY(),
              envelope.getMaxY(),
              fileWindow.getRows(),
              fileWindow.getCols());

      // to intersect with the data window, we transform the screen window
      JGrassRegion reprojectedScreenDrawWindow = screenDrawWindow;
      if (!CRS.equalsIgnoreMetadata(destinationCRS, grassCrs)) {
        reprojectedScreenDrawWindow = screenDrawWindow.reproject(destinationCRS, grassCrs, true);
      }

      /*
       * if the map is not visible, do not render it
       */
      // JGrassRegion fileWindow = grassMapGeoResource.getFileWindow();
      Rectangle2D.Double fileRectDouble = fileWindow.getRectangle();
      Double reprojScreenRectangle = reprojectedScreenDrawWindow.getRectangle();
      if (!reprojScreenRectangle.intersects(fileRectDouble)) {
        getContext().setStatus(ILayer.DONE);
        getContext().setStatusMessage(THE_MAP_IS_OUTSIDE_OF_THE_VISIBLE_REGION);
        System.out.println(THE_MAP_IS_OUTSIDE_OF_THE_VISIBLE_REGION);
        return;
      }
      /*
       * we will draw only the intersection of the map in the display system = part of visible map
       */
      Rectangle2D drawMapRectangle =
          reprojectedScreenDrawWindow.getRectangle().createIntersection(fileRectDouble);
      // Rectangle2D drawMapRectangle = fileRectDouble.getBounds2D();
      // resolution is that of the file window
      double ewRes = fileWindow.getWEResolution();
      double nsRes = fileWindow.getNSResolution();
      if (fileRectDouble.getWidth() < ewRes || fileRectDouble.getHeight() < nsRes) {
        getContext().setStatus(ILayer.DONE);
        getContext().setStatusMessage(THE_MAP_IS_OUTSIDE_OF_THE_VISIBLE_REGION);
        System.out.println(THE_MAP_IS_OUTSIDE_OF_THE_VISIBLE_REGION);
        return;
      }
      MathTransform transform = CRS.findMathTransform(destinationCRS, grassCrs, true);
      Coordinate pixelSize = getContext().getPixelSize();

      Coordinate c1 = new Coordinate(envelope.getMinX(), envelope.getMinY());
      Coordinate c2 =
          new Coordinate(envelope.getMinX() + pixelSize.x, envelope.getMinY() + pixelSize.y);
      Envelope envy = new Envelope(c1, c2);
      Envelope envyTrans = JTS.transform(envy, transform);

      pixelSize = new Coordinate(envyTrans.getWidth(), envyTrans.getHeight());
      /*
       * if the resolution is higher of that of the screen, it doesn't make much sense to draw it
       * all. So for visualization we just use the screen resolution to do things faster.
       */
      if (ewRes < pixelSize.x) {
        ewRes = pixelSize.x;
      }
      if (nsRes < pixelSize.y) {
        nsRes = pixelSize.y;
      }
      fileWindow.setNSResolution(nsRes);
      fileWindow.setWEResolution(ewRes);
      nsRes = fileWindow.getNSResolution();
      ewRes = fileWindow.getWEResolution();
      /*
       * redefine the region of the map to be drawn
       */
      /*
       * snap the screen to fit into the active region grid. This is mandatory for the exactness
       * of the query of the pixels (ex. d.what.rast).
       */
      JGrassRegion activeWindow = grassMapGeoResource.getActiveWindow();
      Coordinate minXY =
          JGrassRegion.snapToNextHigherInRegionResolution(
              drawMapRectangle.getMinX(), drawMapRectangle.getMinY(), activeWindow);
      Coordinate maxXY =
          JGrassRegion.snapToNextHigherInRegionResolution(
              drawMapRectangle.getMaxX(), drawMapRectangle.getMaxY(), activeWindow);

      JGrassRegion drawMapRegion =
          new JGrassRegion(minXY.x, maxXY.x, minXY.y, maxXY.y, ewRes, nsRes);
      // JGrassRegion drawMapRegion = new JGrassRegion(drawMapRectangle.getMinX(),
      // drawMapRectangle.getMaxX(), drawMapRectangle.getMinY(), drawMapRectangle
      // .getMaxY(), ewRes, nsRes);
      JGrassMapEnvironment grassMapEnvironment = grassMapGeoResource.getjGrassMapEnvironment();
      GridCoverage2D coverage =
          JGrassCatalogUtilities.getGridcoverageFromGrassraster(grassMapEnvironment, drawMapRegion);
      coverage = coverage.view(ViewType.RENDERED);
      if (coverage != null) {

        // setting rendering hints
        RenderingHints hints = new RenderingHints(Collections.EMPTY_MAP);
        hints.add(
            new RenderingHints(RenderingHints.KEY_RENDERING, RenderingHints.VALUE_RENDER_SPEED));
        hints.add(
            new RenderingHints(RenderingHints.KEY_DITHERING, RenderingHints.VALUE_DITHER_DISABLE));
        hints.add(
            new RenderingHints(
                RenderingHints.KEY_ALPHA_INTERPOLATION,
                RenderingHints.VALUE_ALPHA_INTERPOLATION_SPEED));
        hints.add(
            new RenderingHints(
                RenderingHints.KEY_COLOR_RENDERING, RenderingHints.VALUE_COLOR_RENDER_SPEED));
        hints.add(
            new RenderingHints(
                RenderingHints.KEY_INTERPOLATION,
                RenderingHints.VALUE_INTERPOLATION_NEAREST_NEIGHBOR));
        hints.add(
            new RenderingHints(
                RenderingHints.KEY_STROKE_CONTROL, RenderingHints.VALUE_STROKE_PURE));
        hints.add(
            new RenderingHints(
                RenderingHints.KEY_FRACTIONALMETRICS, RenderingHints.VALUE_FRACTIONALMETRICS_OFF));
        hints.add(new RenderingHints(JAI.KEY_INTERPOLATION, new InterpolationNearest()));
        g2d.addRenderingHints(hints);
        final TileCache tempCache = JAI.createTileCache();
        tempCache.setMemoryCapacity(16 * 1024 * 1024);
        tempCache.setMemoryThreshold(1.0f);
        hints.add(new RenderingHints(JAI.KEY_TILE_CACHE, tempCache));

        // draw

        AffineTransform worldToScreen =
            RendererUtilities.worldToScreenTransform(envelope, screenSize, destinationCRS);
        final GridCoverageRenderer paint =
            new GridCoverageRenderer(destinationCRS, envelope, screenSize, worldToScreen, hints);
        RasterSymbolizer rasterSymbolizer =
            CommonFactoryFinder.getStyleFactory(null).createRasterSymbolizer();

        paint.paint(g2d, coverage, rasterSymbolizer);

        tempCache.flush();

        // IBlackboard blackboard = context.getMap().getBlackboard();
        // String legendString = coverageReader.getLegendString();
        // String name = grassMapGeoResource.getTitle();
        // blackboard.putString(JGrassMapGeoResource.READERID + "#" + name, legendString);

      }

    } catch (Exception e) {
      e.printStackTrace();
    } finally {
      getContext().setStatus(ILayer.DONE);
      getContext().setStatusMessage(null);
    }
  }
Esempio n. 6
0
  public double getTerminalAttachPoint(
      double terminalPositionAngle,
      Rectangle2D.Double globalBounds,
      AffineTransform localToGlobalTx,
      Point2D.Double arcAttachPoint) {
    double nw = globalBounds.getWidth(); // node width
    double nh = globalBounds.getHeight(); // node height
    double nhalfw = nw / 2.0; // half node width
    double nhalfh = nh / 2.0; // half node height
    double tanTheta = Math.tan(terminalPositionAngle);

    /*
     * Idea: A node has four sides. Firstly, see which side intersects a line with
     * an angle of "theta", originating at the centre of the node.
     * Secondly, see where exactly on the side this intersection happens.
     */
    // angle in radians between horizontal line through centre of node and top left corner
    double alpha = Math.atan2(nh, nw);
    double beta = DisplayConstants.HALF_PI - alpha; // in radians
    double x = 0.0;
    double y = 0.0;
    double tmpAngle = alpha + 2 * beta;
    if (terminalPositionAngle >= alpha && terminalPositionAngle <= tmpAngle) {
      // intersects with bottom side of node
      y = nhalfh;
      x = y / tanTheta;
    } else if ((terminalPositionAngle >= tmpAngle)
        || (terminalPositionAngle <= -DisplayConstants.HALF_PI)) {
      // intersects with left side of node
      double x1 = 0; // (x1,y1) is the top point of the triangle
      double y1 = -nhalfh;
      double x2 = -nhalfw; // (x2,y2) is the bottom left point)
      double y2 = nhalfh;
      double x3 = 0; // (x3,y3) is the center point
      double y3 = 0;
      double x4 =
          -nhalfw; // (x4,y4) is the left edge of the box at a y position relative to the angle
      double y4 = x4 * tanTheta;
      double u1 =
          ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3))
              / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));
      x = x1 + u1 * (x2 - x1);
      y = y1 + u1 * (y2 - y1);
    } else {
      // intersects with right side of node
      double x1 = 0; // (x1,y1) is the top of triangle
      double y1 = -nhalfh;
      double x2 = nhalfw; // (x2,y2) is the bottom right corner
      double y2 = nhalfh;
      double x3 = 0; // (x3,y3) is the center of triangle
      double y3 = 0;
      double x4 = nhalfw; // (x4,y4) is the right edge relative of the box to the angle
      double y4 = x4 * tanTheta;
      double u1 =
          ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3))
              / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));
      x = x1 + u1 * (x2 - x1);
      y = y1 + u1 * (y2 - y1);
    }

    arcAttachPoint.setLocation(x, y);
    return terminalPositionAngle;
  }
Esempio n. 7
0
 @Override
 public double getMillingLength() {
   return rec.getHeight();
 }