Created
March 29, 2018 06:28
-
-
Save niklas152/c67da42a3880193d02de411f8ef43720 to your computer and use it in GitHub Desktop.
this is the code used for generating the scan point cloud currently in use on our open bots
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
package com.niklashalle; | |
import edu.princeton.cs.introcs.Draw; | |
import java.awt.*; | |
public class GenerateScanCloud { | |
private static class Point { | |
double x; | |
double y; | |
Point(double x0, double y0) { | |
x = x0; | |
y = y0; | |
} | |
double distanceTo(Point oth) { | |
return Math.sqrt((this.x - oth.x) * (this.x - oth.x) + (this.y - oth.y) * (this.y - oth.y)); | |
} | |
} | |
private static Draw drawTest; | |
private static void drawPoint(double x, double y) { | |
drawTest.point(x, y); | |
System.out.println("_points->push_back(Pixel(" + x/972.0 + " * CAMERA_FRAME_WIDTH, " + y/972.0 + " * CAMERA_FRAME_WIDTH));"); | |
} | |
public static void main(String[] args) { | |
double frameSize = 972; | |
double netDensity = Math.ceil(frameSize / 100.0); | |
double frameMiddle = frameSize/2d; | |
drawTest = new Draw(); | |
drawTest.setXscale(0, frameSize); | |
drawTest.setYscale(0, frameSize); | |
drawTest.filledRectangle(frameMiddle, frameMiddle, frameMiddle, frameMiddle); | |
drawTest.setPenColor(Color.BLUE); | |
drawTest.enableDoubleBuffering(); | |
// not quite sure how large it really needs to be, so better overestimate | |
Point[] neighbors = new Point[(int) frameSize]; | |
for (int i = 0; i < frameSize; ++i) { | |
neighbors[i] = new Point(0, 0); | |
} | |
// this account for most of the points very well | |
for (int angle = 0; angle < 360; ++angle) { | |
for (double radius = 10 * netDensity; radius <= frameSize/2d; radius += (1/3d) * (netDensity * frameSize / radius)) { | |
Point current = new Point(Math.sin(Math.toRadians(angle)) * radius + frameMiddle, | |
Math.cos(Math.toRadians(angle)) * radius + frameMiddle); | |
if (neighbors[(int) Math.round(radius)].distanceTo(current) >= netDensity * frameSize / (2*radius)) { | |
drawPoint(current.x, current.y); | |
neighbors[(int) Math.round(radius)] = current; | |
} | |
} | |
} | |
// adding some point very close to the bot manually | |
for (double angle = 0; angle < 2 * Math.PI; angle += Math.PI / 4.5) { | |
drawPoint(Math.sin(angle) * netDensity * 5 + frameMiddle, | |
Math.cos(angle) * netDensity * 5 + frameMiddle); | |
} | |
drawTest.show(); | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment