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);
}
@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);
}
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();
}
}
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);
}
}
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);
}
}
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;
}
@Override
public double getMillingLength() {
return rec.getHeight();
}