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.

2023-05-02 23:51:59 -05:00
parent 2f46605837
commit 7298f80d36
4 changed files with 176 additions and 119 deletions
--- 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
+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
--- 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<Vector> pointList = view.getPoints();
-//            slam.RANSAC(pointList, view.getFOV() / view.getRayNum());
        }
    }

--- 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<Line> lines = new ArrayList<>();
+    ArrayList<Vector> 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<Vector> randomSample(ArrayList<Vector> 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<Vector> randomSampleInAngleRange(ArrayList<Vector> set, int subSampleSize, float minAngle, float maxAngle){

-        // get a random sample of size numSampleReadings within degreeRange degrees of this laser reading.
-        List<Vector> 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<Vector> 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<Vector> 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<Vector> originalList, List<Vector> randomSample, float maxRange, int consensus){
+    private void extractFeature(List<Vector> 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<Vector> 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<Vector> randomSample = this.randomSample(newPoints, indexRange, numSampleReadings);
+            List<Vector> 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);
+        }
+    }
+
 }
--- 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<Vector> 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<Float> getAngles(){
+        ArrayList<Float> angles = new ArrayList<>();
+        for (Ray ray : rays) {
+            if (ray.hasCollided()){
+                angles.add(ray.getAngle());
+            }
+        }
+        return angles;
+    }
 }