Started converting the view class over to ScanPoints.
This commit is contained in:
Quinn
206
map.txt
206
map.txt
@@ -1,112 +1,112 @@
|
||||
numVerts,60
|
||||
numEdges,50
|
||||
vert,206.0,522.0
|
||||
vert,554.0,422.0
|
||||
vert,525.0,281.0
|
||||
vert,404.0,666.0
|
||||
vert,482.0,615.0
|
||||
vert,545.0,326.0
|
||||
vert,520.0,338.0
|
||||
vert,713.0,810.0
|
||||
vert,401.0,616.0
|
||||
vert,520.0,399.0
|
||||
vert,367.0,375.0
|
||||
vert,599.0,418.0
|
||||
vert,591.0,332.0
|
||||
vert,483.0,810.0
|
||||
vert,602.0,652.0
|
||||
vert,711.0,234.0
|
||||
vert,366.0,275.0
|
||||
vert,611.0,359.0
|
||||
vert,570.0,324.0
|
||||
vert,302.0,249.0
|
||||
vert,259.0,476.0
|
||||
vert,411.0,811.0
|
||||
vert,503.0,375.0
|
||||
vert,168.0,471.0
|
||||
vert,603.0,346.0
|
||||
vert,601.0,491.0
|
||||
vert,165.0,235.0
|
||||
vert,405.0,237.0
|
||||
vert,606.0,811.0
|
||||
vert,165.0,809.0
|
||||
vert,213.0,812.0
|
||||
vert,165.0,521.0
|
||||
vert,223.0,273.0
|
||||
vert,294.0,378.0
|
||||
vert,267.0,616.0
|
||||
vert,493.0,238.0
|
||||
vert,403.0,483.0
|
||||
vert,210.0,380.0
|
||||
vert,330.0,376.0
|
||||
vert,714.0,488.0
|
||||
vert,603.0,537.0
|
||||
vert,715.0,655.0
|
||||
vert,224.0,247.0
|
||||
vert,267.0,723.0
|
||||
vert,503.0,357.0
|
||||
vert,618.0,397.0
|
||||
vert,713.0,539.0
|
||||
vert,334.0,727.0
|
||||
vert,485.0,693.0
|
||||
vert,211.0,346.0
|
||||
vert,300.0,277.0
|
||||
vert,294.0,347.0
|
||||
vert,577.0,423.0
|
||||
vert,531.0,414.0
|
||||
vert,520.0,399.0
|
||||
vert,329.0,345.0
|
||||
vert,327.0,615.0
|
||||
vert,403.0,483.0
|
||||
vert,503.0,375.0
|
||||
vert,367.0,347.0
|
||||
vert,715.0,655.0
|
||||
vert,300.0,277.0
|
||||
vert,165.0,809.0
|
||||
vert,713.0,810.0
|
||||
vert,294.0,378.0
|
||||
vert,165.0,521.0
|
||||
vert,165.0,235.0
|
||||
vert,602.0,652.0
|
||||
vert,327.0,615.0
|
||||
vert,599.0,418.0
|
||||
vert,211.0,346.0
|
||||
vert,482.0,615.0
|
||||
vert,259.0,476.0
|
||||
vert,304.0,479.0
|
||||
vert,621.0,379.0
|
||||
vert,601.0,491.0
|
||||
vert,485.0,693.0
|
||||
vert,714.0,488.0
|
||||
vert,520.0,338.0
|
||||
vert,713.0,539.0
|
||||
vert,577.0,423.0
|
||||
vert,294.0,347.0
|
||||
vert,525.0,281.0
|
||||
vert,168.0,471.0
|
||||
vert,213.0,812.0
|
||||
vert,405.0,237.0
|
||||
vert,367.0,375.0
|
||||
vert,366.0,275.0
|
||||
vert,411.0,811.0
|
||||
vert,711.0,234.0
|
||||
vert,401.0,616.0
|
||||
vert,223.0,273.0
|
||||
vert,330.0,376.0
|
||||
vert,267.0,723.0
|
||||
vert,487.0,485.0
|
||||
edge,start,1,end,53
|
||||
edge,start,2,end,16
|
||||
edge,start,3,end,21
|
||||
edge,start,5,end,18
|
||||
edge,end,5,start,6
|
||||
edge,start,7,end,15
|
||||
edge,start,9,end,22
|
||||
edge,start,11,end,52
|
||||
edge,start,12,end,24
|
||||
edge,start,14,end,41
|
||||
edge,start,14,end,28
|
||||
edge,start,15,end,35
|
||||
edge,start,17,end,58
|
||||
edge,end,12,start,18
|
||||
edge,start,19,end,42
|
||||
edge,start,19,end,50
|
||||
edge,start,22,end,44
|
||||
edge,end,20,start,23
|
||||
edge,end,17,start,24
|
||||
edge,start,26,end,29
|
||||
edge,end,26,start,27
|
||||
edge,end,7,start,29
|
||||
edge,end,0,start,30
|
||||
edge,end,0,start,31
|
||||
edge,start,32,end,42
|
||||
edge,start,32,end,50
|
||||
edge,end,8,start,36
|
||||
edge,start,36,end,57
|
||||
edge,start,37,end,49
|
||||
edge,end,33,start,37
|
||||
edge,start,38,end,54
|
||||
edge,end,10,start,38
|
||||
edge,end,25,start,40
|
||||
edge,start,40,end,46
|
||||
edge,end,34,start,43
|
||||
edge,start,43,end,47
|
||||
edge,end,6,start,44
|
||||
edge,end,11,start,45
|
||||
edge,end,13,start,48
|
||||
vert,483.0,810.0
|
||||
vert,621.0,379.0
|
||||
vert,603.0,537.0
|
||||
vert,606.0,811.0
|
||||
vert,503.0,357.0
|
||||
vert,210.0,380.0
|
||||
vert,591.0,332.0
|
||||
vert,334.0,727.0
|
||||
vert,570.0,324.0
|
||||
vert,618.0,397.0
|
||||
vert,545.0,326.0
|
||||
vert,611.0,359.0
|
||||
vert,404.0,666.0
|
||||
vert,302.0,249.0
|
||||
vert,267.0,616.0
|
||||
vert,554.0,422.0
|
||||
vert,603.0,346.0
|
||||
vert,493.0,238.0
|
||||
vert,206.0,522.0
|
||||
edge,start,1,end,2
|
||||
edge,start,2,end,5
|
||||
edge,start,4,end,36
|
||||
edge,start,4,end,20
|
||||
edge,start,5,end,45
|
||||
edge,end,3,start,6
|
||||
edge,start,6,end,32
|
||||
edge,start,9,end,10
|
||||
edge,start,10,end,35
|
||||
edge,start,12,end,59
|
||||
edge,end,9,start,13
|
||||
edge,end,7,start,14
|
||||
edge,start,14,end,44
|
||||
edge,start,15,end,55
|
||||
edge,start,15,end,48
|
||||
edge,start,16,end,26
|
||||
edge,start,22,end,41
|
||||
edge,start,24,end,51
|
||||
edge,start,26,end,56
|
||||
edge,end,17,start,27
|
||||
edge,end,11,start,27
|
||||
edge,start,28,end,33
|
||||
edge,end,19,start,29
|
||||
edge,start,30,end,59
|
||||
edge,end,13,start,31
|
||||
edge,start,35,end,58
|
||||
edge,end,0,start,37
|
||||
edge,end,8,start,37
|
||||
edge,end,3,start,38
|
||||
edge,end,32,start,38
|
||||
edge,start,39,end,55
|
||||
edge,start,39,end,48
|
||||
edge,end,18,start,40
|
||||
edge,end,23,start,40
|
||||
edge,start,42,end,50
|
||||
edge,end,21,start,43
|
||||
edge,end,25,start,43
|
||||
edge,end,24,start,45
|
||||
edge,end,17,start,46
|
||||
edge,end,11,start,46
|
||||
edge,start,47,end,57
|
||||
edge,end,47,start,49
|
||||
edge,end,16,start,50
|
||||
edge,end,49,start,51
|
||||
edge,end,33,start,51
|
||||
edge,end,1,start,52
|
||||
edge,end,9,start,53
|
||||
edge,end,34,start,55
|
||||
edge,end,47,start,55
|
||||
edge,end,54,start,56
|
||||
edge,end,10,start,56
|
||||
edge,end,45,start,58
|
||||
edge,end,4,start,59
|
||||
edge,end,39,start,59
|
||||
edge,end,42,start,52
|
||||
edge,end,34,start,53
|
||||
edge,end,0,start,54
|
||||
edge,end,8,start,54
|
||||
edge,end,1,start,56
|
||||
edge,end,52,start,57
|
||||
@@ -1,7 +1,6 @@
|
||||
import Vector.*;
|
||||
import processing.core.*;
|
||||
|
||||
import java.lang.reflect.Array;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Collections;
|
||||
import java.util.List;
|
||||
@@ -96,7 +95,7 @@ public class SLAM{
|
||||
* @param view a laser scan view
|
||||
*/
|
||||
public void RANSAC(View view){
|
||||
unassociatedPoints.addScan(view.getPos(), view.getPoints());
|
||||
unassociatedPoints.addScan(view.getPos(), view.getScan().getPoints());
|
||||
|
||||
float degreeRange = radians(5); // range to randomly sample readings within
|
||||
int numSampleReadings = 15; // number of readings to randomly sample
|
||||
|
||||
@@ -100,8 +100,8 @@ class CorrespondenceMatrix{
|
||||
correspondenceMatrix.add(new ArrayList<Float>());
|
||||
|
||||
// go through all of the points in the new scan and find the closest point in the old scan
|
||||
for (int newPointIndex = 0; newPointIndex < newScan.getScan().size(); newPointIndex++) {
|
||||
Vector newPoint = newScan.getScan().get(newPointIndex);
|
||||
for (int newPointIndex = 0; newPointIndex < newScan.getPoints().size(); newPointIndex++) {
|
||||
Vector newPoint = newScan.getPoints().get(newPointIndex);
|
||||
// if the new point is null, then skip it
|
||||
if (newPoint == null) {
|
||||
continue;
|
||||
@@ -109,8 +109,8 @@ class CorrespondenceMatrix{
|
||||
// find the closest point in the old scan
|
||||
float closestDistance = Float.MAX_VALUE;
|
||||
int closestIndex = -1;
|
||||
for (int j = 0; j < referenceScan.getScan().size(); j++) {
|
||||
Vector oldPoint = referenceScan.getScan().get(j);
|
||||
for (int j = 0; j < referenceScan.getPoints().size(); j++) {
|
||||
Vector oldPoint = referenceScan.getPoints().get(j);
|
||||
// if the old point is null, then skip it
|
||||
if (oldPoint == null) {
|
||||
continue;
|
||||
@@ -161,7 +161,7 @@ class ScanMatcher{
|
||||
private SimpleMatrix averageScanPosition(ScanPoint scan){
|
||||
Vector averagePosition = new Vector(0, 0);
|
||||
int invalidPoints = 0;
|
||||
for (Vector point : scan.getScan()) {
|
||||
for (Vector point : scan.getPoints()) {
|
||||
if (point != null) {
|
||||
averagePosition = averagePosition.add(point);
|
||||
}
|
||||
@@ -169,7 +169,7 @@ class ScanMatcher{
|
||||
invalidPoints++;
|
||||
}
|
||||
}
|
||||
return new SimpleMatrix(averagePosition.div(scan.getScan().size() - invalidPoints).toArray());
|
||||
return new SimpleMatrix(averagePosition.div(scan.getPoints().size() - invalidPoints).toArray());
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -190,8 +190,8 @@ class ScanMatcher{
|
||||
for (int i = 0; i < correspondenceMatrix.getOldPointIndices().size(); i++) {
|
||||
int oldIndex = correspondenceMatrix.getOldPointIndices().get(i);
|
||||
int newIndex = correspondenceMatrix.getNewPointIndices().get(i);
|
||||
Vector oldPoint = referenceScan.getScan().get(oldIndex);
|
||||
Vector newPoint = newScan.getScan().get(newIndex);
|
||||
Vector oldPoint = referenceScan.getPoints().get(oldIndex);
|
||||
Vector newPoint = newScan.getPoints().get(newIndex);
|
||||
if (oldPoint != null && newPoint != null) {
|
||||
Vector oldPointCentered = oldPoint.sub(referenceScanAveragePosition);
|
||||
Vector newPointCentered = newPoint.sub(newScanAveragePosition);
|
||||
@@ -232,12 +232,12 @@ class ScanMatcher{
|
||||
|
||||
public ScanPoint applyRotationAndTranslationMatrices(ScanPoint newScan){
|
||||
// apply the rotation matrix and translation vector to the new scan
|
||||
for (int i = 0; i < newScan.getScan().size(); i++) {
|
||||
Vector point = newScan.getScan().get(i);
|
||||
for (int i = 0; i < newScan.getPoints().size(); i++) {
|
||||
Vector point = newScan.getPoints().get(i);
|
||||
if (point != null) {
|
||||
SimpleMatrix pointMatrix = new SimpleMatrix(point.toArray());
|
||||
SimpleMatrix newPointMatrix = rotationMatrix.mult(pointMatrix).plus(translationVector);
|
||||
newScan.getScan().set(i, new Vector((float) newPointMatrix.get(0), (float) newPointMatrix.get(1)));
|
||||
newScan.getPoints().set(i, new Vector((float) newPointMatrix.get(0), (float) newPointMatrix.get(1)));
|
||||
}
|
||||
}
|
||||
return newScan;
|
||||
|
||||
@@ -8,10 +8,10 @@ import java.util.ArrayList;
|
||||
public class ScanPoint extends Vertex{
|
||||
|
||||
private Vector position;
|
||||
private Vector orientation;
|
||||
private float orientation;
|
||||
private ArrayList<Vector> scan;
|
||||
|
||||
ScanPoint(Vector scanPosition, Vector orientation, ArrayList<Vector> scan) {
|
||||
public ScanPoint(Vector scanPosition, float orientation, ArrayList<Vector> scan) {
|
||||
super();
|
||||
this.position = scanPosition;
|
||||
this.orientation = orientation;
|
||||
@@ -25,11 +25,11 @@ public class ScanPoint extends Vertex{
|
||||
return position;
|
||||
}
|
||||
|
||||
public Vector getOrientation(){
|
||||
public float getOrientation(){
|
||||
return this.orientation;
|
||||
}
|
||||
|
||||
public ArrayList<Vector> getScan(){
|
||||
public ArrayList<Vector> getPoints(){
|
||||
return this.scan;
|
||||
}
|
||||
|
||||
|
||||
@@ -2,6 +2,7 @@ import Graph.PointGraph;
|
||||
import Vector.Vector;
|
||||
import processing.core.*;
|
||||
import java.util.ArrayList;
|
||||
import ScanGraph.ScanPoint;
|
||||
|
||||
public class View {
|
||||
Vector pose;
|
||||
@@ -81,7 +82,7 @@ public class View {
|
||||
}
|
||||
|
||||
//gets the point that each ray has collided with
|
||||
public ArrayList<Vector> getPoints() {
|
||||
public ScanPoint getScan() {
|
||||
ArrayList<Vector> points = new ArrayList<>();
|
||||
|
||||
for (Ray ray : rays) {
|
||||
@@ -92,7 +93,7 @@ public class View {
|
||||
points.add(point);
|
||||
}
|
||||
}
|
||||
return points;
|
||||
return new ScanPoint(this.pose,this.angle, points);
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
Reference in New Issue
Block a user