From 7298f80d36e686507af44458841ee93a60a8aa0e Mon Sep 17 00:00:00 2001 From: Quinn Date: Tue, 2 May 2023 23:51:59 -0500 Subject: [PATCH] First semi-successful test of feature extraction. I think the algorithm for line of best fit needs some work. Specifically, ifnding the starting and end point for the line. --- map.txt | 184 +++++++++++++++++++++++++++++--------------------- src/Car.java | 7 +- src/SLAM.java | 77 ++++++++++++--------- src/View.java | 27 ++++++-- 4 files changed, 176 insertions(+), 119 deletions(-) diff --git a/map.txt b/map.txt index 20eed34..3f28bc0 100644 --- a/map.txt +++ b/map.txt @@ -1,79 +1,109 @@ -numVerts,43 -numEdges,34 -vert,601.0,491.0 -vert,602.0,652.0 -vert,300.0,277.0 -vert,213.0,812.0 -vert,715.0,655.0 -vert,294.0,378.0 -vert,259.0,476.0 -vert,224.0,247.0 -vert,304.0,479.0 -vert,485.0,693.0 -vert,713.0,810.0 -vert,367.0,347.0 -vert,223.0,273.0 -vert,404.0,666.0 -vert,329.0,345.0 -vert,482.0,615.0 -vert,606.0,811.0 -vert,483.0,810.0 -vert,267.0,616.0 -vert,603.0,537.0 -vert,330.0,376.0 -vert,267.0,723.0 -vert,714.0,488.0 -vert,401.0,616.0 -vert,294.0,347.0 -vert,206.0,522.0 -vert,711.0,234.0 -vert,493.0,238.0 -vert,302.0,249.0 -vert,403.0,483.0 -vert,334.0,727.0 -vert,168.0,471.0 -vert,487.0,485.0 -vert,327.0,615.0 -vert,405.0,237.0 -vert,211.0,346.0 -vert,210.0,380.0 -vert,165.0,235.0 -vert,411.0,811.0 +numVerts,58 +numEdges,49 vert,165.0,809.0 -vert,713.0,539.0 -vert,367.0,375.0 +vert,482.0,615.0 +vert,411.0,811.0 +vert,570.0,324.0 +vert,165.0,235.0 +vert,300.0,277.0 +vert,602.0,652.0 +vert,304.0,479.0 +vert,493.0,238.0 +vert,401.0,616.0 +vert,206.0,522.0 +vert,531.0,414.0 +vert,611.0,359.0 +vert,483.0,810.0 +vert,294.0,347.0 vert,165.0,521.0 -edge,start,1,end,4 -edge,start,1,end,16 -edge,start,3,end,25 -edge,start,9,end,17 -edge,start,10,end,26 -edge,start,11,end,14 -edge,start,11,end,41 -edge,end,7,start,12 -edge,end,2,start,12 -edge,start,13,end,38 -edge,end,0,start,19 -edge,start,19,end,40 -edge,end,14,start,20 -edge,start,20,end,41 -edge,end,18,start,21 -edge,start,21,end,30 -edge,start,24,end,35 -edge,end,5,start,24 -edge,start,26,end,27 -edge,end,7,start,28 -edge,end,2,start,28 -edge,end,23,start,29 -edge,end,8,start,29 -edge,end,6,start,31 -edge,end,15,start,32 -edge,end,22,start,32 -edge,end,18,start,33 -edge,end,30,start,33 -edge,start,34,end,37 -edge,end,35,start,36 -edge,end,5,start,36 -edge,start,37,end,39 -edge,end,10,start,39 -edge,end,25,start,42 \ No newline at end of file +vert,591.0,332.0 +vert,327.0,615.0 +vert,714.0,488.0 +vert,503.0,375.0 +vert,267.0,616.0 +vert,621.0,379.0 +vert,210.0,380.0 +vert,618.0,397.0 +vert,715.0,655.0 +vert,487.0,485.0 +vert,294.0,378.0 +vert,503.0,357.0 +vert,405.0,237.0 +vert,599.0,418.0 +vert,520.0,399.0 +vert,545.0,326.0 +vert,213.0,812.0 +vert,168.0,471.0 +vert,713.0,810.0 +vert,224.0,247.0 +vert,606.0,811.0 +vert,367.0,347.0 +vert,577.0,423.0 +vert,334.0,727.0 +vert,554.0,422.0 +vert,330.0,376.0 +vert,259.0,476.0 +vert,404.0,666.0 +vert,403.0,483.0 +vert,211.0,346.0 +vert,711.0,234.0 +vert,713.0,539.0 +vert,302.0,249.0 +vert,520.0,338.0 +vert,485.0,693.0 +vert,329.0,345.0 +vert,367.0,375.0 +vert,603.0,537.0 +vert,601.0,491.0 +vert,267.0,723.0 +vert,223.0,273.0 +vert,603.0,346.0 +edge,start,0,end,34 +edge,start,3,end,16 +edge,end,0,start,4 +edge,start,6,end,24 +edge,start,6,end,36 +edge,start,11,end,30 +edge,start,12,end,21 +edge,start,14,end,45 +edge,start,14,end,26 +edge,end,10,start,15 +edge,start,16,end,57 +edge,start,17,end,20 +edge,start,17,end,39 +edge,start,19,end,27 +edge,start,21,end,23 +edge,start,22,end,45 +edge,start,22,end,26 +edge,start,23,end,29 +edge,end,1,start,25 +edge,end,18,start,25 +edge,start,27,end,49 +edge,end,4,start,28 +edge,start,29,end,38 +edge,end,19,start,30 +edge,end,3,start,31 +edge,end,10,start,32 +edge,start,33,end,42 +edge,start,34,end,46 +edge,start,37,end,51 +edge,start,37,end,52 +edge,start,38,end,40 +edge,end,11,start,40 +edge,start,41,end,51 +edge,start,41,end,52 +edge,end,2,start,43 +edge,end,9,start,44 +edge,end,7,start,44 +edge,end,8,start,46 +edge,end,35,start,48 +edge,end,5,start,48 +edge,end,31,start,49 +edge,end,13,start,50 +edge,start,53,end,54 +edge,end,47,start,53 +edge,end,20,start,55 +edge,end,39,start,55 +edge,end,35,start,56 +edge,end,5,start,56 +edge,end,12,start,57 \ No newline at end of file diff --git a/src/Car.java b/src/Car.java index 3d14a30..1333991 100644 --- a/src/Car.java +++ b/src/Car.java @@ -42,18 +42,15 @@ public class Car{ proc.stroke(255); proc.ellipse(pose.x, pose.y, carWidth, carLength); this.updateScan(g); -// this.slam.drawLines(); + this.slam.drawFeatures(proc); } //With all the views that the car has, get their point list void updateScan(PointGraph map){ for(View view : views){ view.look(map); - } + slam.RANSAC(view); - for(View view : views){ - ArrayList pointList = view.getPoints(); -// slam.RANSAC(pointList, view.getFOV() / view.getRayNum()); } } diff --git a/src/SLAM.java b/src/SLAM.java index 1516a7b..b945b4a 100644 --- a/src/SLAM.java +++ b/src/SLAM.java @@ -1,14 +1,17 @@ import Vector.*; import processing.core.*; +import java.lang.reflect.Array; import java.util.ArrayList; import java.util.Collections; import java.util.List; +import static java.lang.Math.random; import static processing.core.PApplet.*; public class SLAM{ ArrayList lines = new ArrayList<>(); + ArrayList unassociatedPoints = new ArrayList<>(); private static PApplet proc; SLAM(PApplet processing){ @@ -17,36 +20,38 @@ public class SLAM{ /** * @param set the set to take a sub sample of - * @param indexRange the range within to take the sub sample * @param subSampleSize the size of the sub sample - * @return A random subset of the set within an indexRange and of size: subSampleSize + * @param minAngle the minimum angle allowed in the subset + * @param maxAngle the maximum angle allowed in the subset + * @return A random subset of the set within the angle range */ - private List randomSample(ArrayList set, int indexRange, int subSampleSize){ - // select a random laser data reading - int randomIdx = (int) proc.random(set.size() - 1); // index of starter reading - Vector point = set.get(randomIdx); // point of starter reading + private List randomSampleInAngleRange(ArrayList set, int subSampleSize, float minAngle, float maxAngle){ - // get a random sample of size numSampleReadings within degreeRange degrees of this laser reading. - List subSample; - int rangeStart = randomIdx - indexRange >= 0 ? randomIdx - indexRange : 0; - int rangeEnd = randomIdx + indexRange < set.size() ? randomIdx + indexRange : set.size()-1; - subSample = set.subList(rangeStart, rangeEnd); // get the sub-sample - Collections.shuffle(subSample); // shuffle the list - List randomSample = subSample.subList(0, rangeEnd-rangeStart); // get our random sample - if (!randomSample.contains(point)) { - randomSample.add(point); + // create an arraylist with all points within the angle range fro mthe given set + ArrayList pointsInAngleRange = new ArrayList<>(); + for(Vector point : set){ + if(minAngle <= point.z && point.z <= maxAngle){ + pointsInAngleRange.add(point); + } } - return randomSample; + // shuffle the list to randomize it + Collections.shuffle(pointsInAngleRange); + + // if the list is too small, just return the whole list + if(pointsInAngleRange.size() < subSampleSize){ + return pointsInAngleRange; + } + // return a subSample of the list + return pointsInAngleRange.subList(0, subSampleSize); } /** - * @param originalList the list which the randomSample of points originated from * @param randomSample a random subsampling of points from the originalList * @param maxRange the maximum distance away from the line of best fit of the subSample of points for a given point's consensus to count. * @param consensus the number of points that have to give their consensus for the line of best fit to count as a valid feature. */ - private void extractFeature(ArrayList originalList, List randomSample, float maxRange, int consensus){ + private void extractFeature(List randomSample, float maxRange, int consensus){ // get a line of best fit for this list. Line bestFit = new Line(randomSample); int count = 0; @@ -63,41 +68,47 @@ public class SLAM{ lines.add(bestFit); // remove the associated readings from the total available readings. for (Vector v : newRandomSample) { - originalList.remove(v); + this.unassociatedPoints.remove(v); } } } /** - * @param newPoints a new scan of points to perform feature detection on - * @param raysPerDegree How many degrees apart are each ray that was cast + * @param view a laser scan view */ - public void RANSAC(ArrayList newPoints, float raysPerDegree){ - float degreeRange = radians(10/2); // range to randomly sample readings within - int indexRange = (int) (degreeRange / raysPerDegree); + public void RANSAC(View view){ + unassociatedPoints.addAll(view.getPoints()); + + float degreeRange = radians(25/2); // range to randomly sample readings within int numSampleReadings = 10; // number of readings to randomly sample - // constrain numSampleReadings so that it cant be higher than possible - if(numSampleReadings >= 2 * indexRange){ - numSampleReadings = 2 * indexRange; - } - int consensus = 6; // the number of points that need to lie near a line for it to be considered valid. - float maxRange = 10; // the maximum distance a point can be away from the line for it to count as a consensus + + int consensus = 7; // the number of points that need to lie near a line for it to be considered valid. + float maxRange = 5; // the maximum distance a point can be away from the line for it to count as a consensus // this for loop determines the maximum number of trials we're willing to do. for(int j = 0; j < 20; j++) { // if there aren't enough points left in the set to form a consensus, we're done. - if(newPoints.size() < consensus){ + if(this.unassociatedPoints.size() < maxRange){ break; } + // get a random angle between -PI and PI + float randomAngle = (float) (2*PI*(random()) - 0.5); + // get a random sub sample of newPoints within the index range of a given size - List randomSample = this.randomSample(newPoints, indexRange, numSampleReadings); + List randomSample = this.randomSampleInAngleRange(this.unassociatedPoints, numSampleReadings, randomAngle-degreeRange, randomAngle+degreeRange); // check if the sub sample forms a valid line and remove the randomSample points if it does. - extractFeature(newPoints, randomSample, maxRange, consensus); + extractFeature(randomSample, maxRange, consensus); } } + public void drawFeatures(PApplet proc){ + for(Line line : lines){ + line.draw(proc); + } + } + } \ No newline at end of file diff --git a/src/View.java b/src/View.java index 7134083..e36141b 100644 --- a/src/View.java +++ b/src/View.java @@ -22,7 +22,7 @@ public class View { public void setRayNum(int numberOfRays, float FOV, float angleOffset) { float rayStep = FOV / numberOfRays; rays.clear(); - float angle = (float) (0.01 - angleOffset); //the 0.01 fixes some bugs + float angle = (float) (angleOffset); //the 0.01 fixes some bugs for (int i = 0; i < numberOfRays; i++) { Ray ray = new Ray(pose, angle); angle = angle + rayStep; @@ -51,7 +51,10 @@ public class View { //changes the angle of the view public void setAngle(float angle) { this.angle = angle; - this.setRayNum(rays.size(), this.FOV, angle); + for(Ray ray : rays){ + float angleOffset = ray.getAngle() - this.angle; + ray.setAngle(this.angle+angleOffset); + } } //changes the field of view of the view @@ -81,11 +84,27 @@ public class View { ArrayList points = new ArrayList<>(); for (Ray ray : rays) { - if (ray.hasCollided()){ - points.add(ray.getPoint()); + if(ray.hasCollided()){ + Vector point = ray.getPoint(); + // store the angle information for that point in the z coordinate + point.z = ray.getAngle(); + points.add(point); } } return points; } + + /** + * @return A list of the angles where a collision was detected + */ + public ArrayList getAngles(){ + ArrayList angles = new ArrayList<>(); + for (Ray ray : rays) { + if (ray.hasCollided()){ + angles.add(ray.getAngle()); + } + } + return angles; + } }