From a0173b1053e9986d9bf29366b077be2510fcde57 Mon Sep 17 00:00:00 2001 From: Quinn Date: Fri, 24 Nov 2023 18:06:35 -0500 Subject: [PATCH] Started converting the view class over to ScanPoints. --- map.txt | 206 +++++++++++++++++------------------ src/SLAM.java | 3 +- src/ScanGraph/ScanGraph.java | 22 ++-- src/ScanGraph/ScanPoint.java | 8 +- src/View.java | 5 +- 5 files changed, 122 insertions(+), 122 deletions(-) diff --git a/map.txt b/map.txt index 7a823b0..1edddbe 100644 --- a/map.txt +++ b/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 \ No newline at end of file +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 \ No newline at end of file diff --git a/src/SLAM.java b/src/SLAM.java index 74fedeb..d722f2d 100644 --- a/src/SLAM.java +++ b/src/SLAM.java @@ -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 diff --git a/src/ScanGraph/ScanGraph.java b/src/ScanGraph/ScanGraph.java index bb719b9..fb4c3e5 100644 --- a/src/ScanGraph/ScanGraph.java +++ b/src/ScanGraph/ScanGraph.java @@ -100,8 +100,8 @@ class CorrespondenceMatrix{ correspondenceMatrix.add(new ArrayList()); // 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; diff --git a/src/ScanGraph/ScanPoint.java b/src/ScanGraph/ScanPoint.java index 303756b..59e915a 100644 --- a/src/ScanGraph/ScanPoint.java +++ b/src/ScanGraph/ScanPoint.java @@ -8,10 +8,10 @@ import java.util.ArrayList; public class ScanPoint extends Vertex{ private Vector position; - private Vector orientation; + private float orientation; private ArrayList scan; - ScanPoint(Vector scanPosition, Vector orientation, ArrayList scan) { + public ScanPoint(Vector scanPosition, float orientation, ArrayList 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 getScan(){ + public ArrayList getPoints(){ return this.scan; } diff --git a/src/View.java b/src/View.java index 3cc745e..71e20aa 100644 --- a/src/View.java +++ b/src/View.java @@ -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 getPoints() { + public ScanPoint getScan() { ArrayList 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); } /**