Further tweaks to RANSAC algorithm and line of best fit

map.txt

@@ -1,109 +1,112 @@
-numVerts,58
+numVerts,60
-numEdges,49
+numEdges,50
-vert,367.0,375.0
-vert,483.0,810.0
-vert,329.0,345.0
-vert,223.0,273.0
-vert,165.0,521.0
-vert,403.0,483.0
-vert,603.0,537.0
-vert,520.0,338.0
-vert,577.0,423.0
-vert,618.0,397.0
-vert,302.0,249.0
-vert,411.0,811.0
-vert,503.0,375.0
-vert,621.0,379.0
-vert,531.0,414.0
-vert,601.0,491.0
-vert,304.0,479.0
-vert,259.0,476.0
-vert,713.0,539.0
-vert,554.0,422.0
-vert,520.0,399.0
-vert,267.0,723.0
-vert,294.0,378.0
-vert,485.0,693.0
-vert,482.0,615.0
-vert,213.0,812.0
-vert,487.0,485.0
-vert,168.0,471.0
-vert,330.0,376.0
-vert,570.0,324.0
-vert,591.0,332.0
-vert,367.0,347.0
-vert,599.0,418.0
-vert,294.0,347.0
-vert,300.0,277.0
-vert,711.0,234.0
-vert,611.0,359.0
-vert,165.0,809.0
 vert,206.0,522.0
-vert,606.0,811.0
+vert,554.0,422.0
-vert,210.0,380.0
+vert,525.0,281.0
-vert,503.0,357.0
-vert,715.0,655.0
 vert,404.0,666.0
-vert,602.0,652.0
+vert,482.0,615.0
-vert,714.0,488.0
+vert,545.0,326.0
-vert,405.0,237.0
+vert,520.0,338.0
-vert,327.0,615.0
-vert,211.0,346.0
 vert,713.0,810.0
 vert,401.0,616.0
-vert,493.0,238.0
+vert,520.0,399.0
-vert,267.0,616.0
+vert,367.0,375.0
-vert,334.0,727.0
+vert,599.0,418.0
-vert,165.0,235.0
+vert,591.0,332.0
-vert,545.0,326.0
+vert,483.0,810.0
-vert,224.0,247.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
-edge,start,3,end,56
+vert,601.0,491.0
-edge,start,3,end,34
+vert,165.0,235.0
-edge,start,4,end,38
+vert,405.0,237.0
-edge,start,5,end,50
+vert,606.0,811.0
-edge,start,5,end,16
+vert,165.0,809.0
-edge,start,6,end,15
+vert,213.0,812.0
-edge,start,6,end,18
+vert,165.0,521.0
-edge,start,7,end,55
+vert,223.0,273.0
-edge,start,8,end,19
+vert,294.0,378.0
-edge,start,9,end,32
+vert,267.0,616.0
-edge,start,10,end,56
+vert,493.0,238.0
-edge,start,10,end,34
+vert,403.0,483.0
-edge,start,12,end,41
+vert,210.0,380.0
-edge,end,9,start,13
+vert,330.0,376.0
-edge,start,14,end,20
+vert,714.0,488.0
-edge,end,14,start,19
+vert,603.0,537.0
-edge,end,12,start,20
+vert,715.0,655.0
-edge,start,21,end,52
+vert,224.0,247.0
-edge,start,21,end,53
+vert,267.0,723.0
-edge,end,1,start,23
+vert,503.0,357.0
-edge,start,25,end,38
+vert,618.0,397.0
-edge,end,24,start,26
+vert,713.0,539.0
-edge,start,26,end,45
+vert,334.0,727.0
-edge,end,17,start,27
+vert,485.0,693.0
-edge,end,2,start,28
+vert,211.0,346.0
-edge,end,0,start,28
+vert,300.0,277.0
-edge,start,29,end,30
+vert,294.0,347.0
-edge,start,30,end,57
+vert,577.0,423.0
-edge,end,2,start,31
+vert,531.0,414.0
+vert,329.0,345.0
+vert,327.0,615.0
+vert,367.0,347.0
+vert,304.0,479.0
+vert,621.0,379.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,end,8,start,32
+edge,start,32,end,42
-edge,start,33,end,48
+edge,start,32,end,50
-edge,end,22,start,33
+edge,end,8,start,36
-edge,start,35,end,51
+edge,start,36,end,57
-edge,end,13,start,36
 edge,start,37,end,49
-edge,start,40,end,48
+edge,end,33,start,37
-edge,end,22,start,40
+edge,start,38,end,54
-edge,end,7,start,41
+edge,end,10,start,38
-edge,end,11,start,43
+edge,end,25,start,40
-edge,end,42,start,44
+edge,start,40,end,46
-edge,end,39,start,44
+edge,end,34,start,43
-edge,start,46,end,54
+edge,start,43,end,47
-edge,start,47,end,52
+edge,end,6,start,44
-edge,start,47,end,53
+edge,end,11,start,45
-edge,end,35,start,49
+edge,end,13,start,48
-edge,end,37,start,54
+edge,end,49,start,51
-edge,end,29,start,55
+edge,end,33,start,51
-edge,end,36,start,57
+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

src/Car.java

@@ -38,12 +38,14 @@ public class Car{
     }
 
     //draw the car and its views
-    public void drawCar(PointGraph g){
+    public void drawCar(PointGraph map, boolean SLAMIsHidden){
         proc.stroke(255);
         proc.ellipse(pose.x, pose.y, carWidth, carLength);
-        this.updateScan(g);
+        this.updateScan(map);
+        if(!SLAMIsHidden){
             this.slam.drawFeatures(proc);
         }
+    }
 
     //With all the views that the car has, get their point list
     void updateScan(PointGraph map){

src/Processing.java

@@ -13,6 +13,7 @@ public class Processing extends PApplet {
 
     PointGraph map = new PointGraph();
     boolean mapIsHidden = false;
+    boolean SLAMIsHidden = false;
 
     public static void main(String[] args) {
         PApplet.main("Processing");
@@ -36,7 +37,7 @@ public class Processing extends PApplet {
         if(!mapIsHidden){
             map.draw(processing);
         }
-        car.drawCar(map);
+        car.drawCar(map, SLAMIsHidden);
         strokeWeight(2);
         stroke(255);
         //car.drive(new int[] {0, 0});
@@ -95,6 +96,9 @@ public class Processing extends PApplet {
         if(key == 'h'){
             mapIsHidden = !mapIsHidden;
         }
+        if(key == 'j'){
+            SLAMIsHidden = !SLAMIsHidden;
+        }
     }
 
     public void mousePressed(){

src/SLAM.java

@@ -11,7 +11,7 @@ import static processing.core.PApplet.*;
 
 public class SLAM{
     ArrayList<Line> lines = new ArrayList<>();
-    ArrayList<Vector> unassociatedPoints = new ArrayList<>();
+    ShortTermMem unassociatedPoints = new ShortTermMem();
     private static PApplet proc;
 
     SLAM(PApplet processing){
@@ -27,7 +27,7 @@ public class SLAM{
      */
     private List<Vector> randomSampleInAngleRange(ArrayList<Vector> set, int subSampleSize, float minAngle, float maxAngle){
 
-        // create an arraylist with all points within the angle range fro mthe given set
+        // create an arraylist with all points within the angle range from the given set
         ArrayList<Vector> pointsInAngleRange = new ArrayList<>();
         for(Vector point : set){
             if(minAngle <= point.z && point.z <= maxAngle){
@@ -54,6 +54,7 @@ public class SLAM{
     private void extractFeature(List<Vector> randomSample, float maxRange, int consensus){
         // get a line of best fit for this list.
         Line bestFit = new Line(randomSample);
+        // check that there are enough points in the sample that are less than the maxRange away to form a consensus
         int count = 0;
         ArrayList<Vector> newRandomSample = new ArrayList<>();
         for (Vector v : randomSample) {
@@ -73,17 +74,35 @@ public class SLAM{
         }
     }
 
+    private void fitToPreviousReadings(List<Vector> sample, float maxRange){
+        // keep track of points that were succesffully associated so they can be removed from the sample at the end
+        ArrayList<Vector> pointsToRemove = new ArrayList<>();
+        // try to associate points from the smaple with pre-existing lines
+        for(Vector v: sample){
+            for(Line l : lines){
+                if(l.getDistance(v) < maxRange){
+                    l.refitLine(v);
+                    pointsToRemove.add(v);
+                }
+            }
+        }
+
+        for(Vector v : pointsToRemove){
+            sample.remove(v);
+        }
+    }
+
     /**
      * @param view a laser scan view
      */
     public void RANSAC(View view){
-        unassociatedPoints.addAll(view.getPoints());
+        unassociatedPoints.addScan(view.getPos(), view.getPoints());
 
-        float degreeRange = radians(10); // range to randomly sample readings within
+        float degreeRange = radians(5); // range to randomly sample readings within
-        int numSampleReadings = 10; // number of readings to randomly sample
+        int numSampleReadings = 15; // number of readings to randomly sample
 
-        int consensus = 7; // the number of points that need to lie near a line for it to be considered valid.
+        int consensus = 10; // 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
+        float maxRange = 10; // 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++) {
@@ -96,9 +115,11 @@ public class SLAM{
             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.randomSampleInAngleRange(this.unassociatedPoints, numSampleReadings, randomAngle-degreeRange, randomAngle+degreeRange);
+            List<Vector> randomSample = this.randomSampleInAngleRange(this.unassociatedPoints.getPoints(), numSampleReadings, randomAngle-degreeRange, randomAngle+degreeRange);
 
             if(randomSample.size() >= numSampleReadings){
+                // try to associate points from the sample with previously made lines
+                fitToPreviousReadings(randomSample, maxRange);
                 // check if the sub sample forms a valid line and remove the randomSample points if it does.
                 extractFeature(randomSample, maxRange, consensus);
             }

src/ShortTermMem.java

@@ -0,0 +1,59 @@
+import java.util.ArrayList;
+import Vector.Vector;
+public class ShortTermMem {
+    ArrayList<ArrayList<Vector>> scans = new ArrayList<>();
+    ArrayList<Vector> scanPositions = new ArrayList<>();
+
+    ArrayList<Long> scanTimes = new ArrayList<>();
+    private int size = 0;
+
+    public void addScan(Vector scanPosition, ArrayList<Vector> scan){
+        size += scan.size();
+        scans.add(scan);
+        scanPositions.add(scanPosition);
+        scanTimes.add(System.currentTimeMillis());
+        purgeScans();
+    }
+
+    public ArrayList<Vector> getPoints(){
+        ArrayList<Vector> points = new ArrayList<>();
+        for(ArrayList<Vector> pointList : this.scans){
+            points.addAll(pointList);
+        }
+        return points;
+    }
+
+    public void remove(Vector point){
+        for(ArrayList<Vector> pointList : this.scans){
+            int listSize = pointList.size();
+            pointList.remove(point);
+            if(listSize - pointList.size() != 0){
+                size--;
+                break;
+            }
+        }
+    }
+
+    private void purgeScans(){
+        long currentTime = System.currentTimeMillis();
+        int i = scanTimes.size();
+
+        // loop through the list backwards and remove all scans that are over second old
+        // we loop backwards to avoid removal conflicts
+        while(i > 0){
+            i--;
+            long dt = currentTime - scanTimes.get(i);
+            if(dt < 1000){
+                continue;
+            }
+            size -= scans.get(i).size();
+            scanTimes.remove(i);
+            scanPositions.remove(i);
+            scans.remove(i);
+        }
+    }
+
+    public int size(){
+        return this.size;
+    }
+}

src/Vector/Line.java

@@ -3,6 +3,7 @@ package Vector;
 import Vector.Vector;
 import processing.core.PApplet;
 
+import java.util.ArrayList;
 import java.util.List;
 
 import static processing.core.PApplet.*;
@@ -13,6 +14,8 @@ public class Line implements LineInterface{
     // store the starting position of the line
     protected Vector position = new Vector(0,0);
 
+    protected ArrayList<Vector> points = new ArrayList<>();
+
     public Line(Vector startPosition, Vector endPosition){
         direction = endPosition.sub(startPosition);
         position = startPosition;
@@ -26,8 +29,16 @@ public class Line implements LineInterface{
         bestFit(points);
     }
 
+    public void refitLine(Vector newPoint){
+        // add the new point to our list
+        this.points.add(newPoint);
+        // rerun the bestFit algorithm with the new point
+        bestFit(new ArrayList<>());
+    }
+
     // least squares line of best fit algorithm
-    private void bestFit(List<Vector> points){
+    public void bestFit(List<Vector> fitPoints){
+        this.points.addAll(fitPoints);
         // get the mean of all the points
         Vector mean = new Vector();
         for(Vector point : points){
@@ -48,7 +59,7 @@ public class Line implements LineInterface{
             length += dist;
 
         }
-        length = 2.5f*length/points.size();
+        length = 2f*length/points.size();
         // if the direction is perfectly vertical create a line to represent that.
         if(direction.y == 0){
             this.direction = new Vector(0, 1);
@@ -86,12 +97,15 @@ public class Line implements LineInterface{
         return this.position.add(this.direction);
     }
 
-    /**
-     * @param point
-     * @return the smallest distance from the point to this line
-     */
     public float getDistance(Vector point){
-        return (point.sub(position).cross(direction)).mag() / direction.mag();
+        float line_dist = direction.mag();
+        if(line_dist == 0) return this.position.sub(point).mag();
+
+        Vector l2 = this.endPoint();
+        float t = ((point.x - position.x) * (l2.x - position.x) + (point.y - position.y) * (l2.y - position.y) + (point.z - position.z) * (l2.z - position.z)) / line_dist;
+        t = constrain(t, 0, 1);
+        Vector closestPoint = new Vector(position.x + t * (l2.x - position.x), position.y + t * (l2.y - position.y), position.z + t * (l2.z - position.z));
+        return closestPoint.sub(point).mag();
     }
 
     public void draw(PApplet proc){

src/Vector/Vector.java

@@ -1,5 +1,7 @@
 package Vector;
 
+import processing.core.PApplet;
+
 import static java.lang.Math.*;
 import static processing.core.PApplet.cos;
 import static processing.core.PApplet.sin;
@@ -90,4 +92,8 @@ public class Vector {
         float currentAngle = this.angle();
         return new Vector(cos(currentAngle + angle), sin(currentAngle + angle)).mul(distance);
     }
+
+    public void draw(PApplet proc){
+        proc.circle(this.x, this.y, 8);
+    }
 }

src/View.java

@@ -35,7 +35,8 @@ public class View {
         for (Ray ray : rays) {
             ray.castRay(map);
             if(ray.hasCollided()){
-                ray.drawRay(proc);
+                ray.getPoint().draw(proc);
+//                ray.drawRay(proc);
             }
         }
     }