Tweaking SLAM parameters to get better accuracy

src/Processing.java

@@ -12,6 +12,7 @@ public class Processing extends PApplet {
     public static PApplet processing;
 
     PointGraph map = new PointGraph();
+    boolean mapIsHidden = false;
 
     public static void main(String[] args) {
         PApplet.main("Processing");
@@ -21,7 +22,7 @@ public class Processing extends PApplet {
         processing = this;
         car = new Car(processing, 100,100,50,40);
         size(1000, 1000);
-        car.addView(360,180);
+        car.addView(360,360);
 
 //        for(int i = 0; i < 10; i++){
 //            PointVertex vStart = new PointVertex(random(50, 950), random(50, 950));
@@ -32,7 +33,9 @@ public class Processing extends PApplet {
     }
     public void draw(){
         background(0);
-        map.draw(processing);
+        if(!mapIsHidden){
+            map.draw(processing);
+        }
         car.drawCar(map);
         strokeWeight(2);
         stroke(255);
@@ -89,6 +92,9 @@ public class Processing extends PApplet {
                 e.printStackTrace();
             }
         }
+        if(key == 'h'){
+            mapIsHidden = !mapIsHidden;
+        }
     }
 
     public void mousePressed(){

src/SLAM.java

@@ -79,7 +79,7 @@ public class SLAM{
     public void RANSAC(View view){
         unassociatedPoints.addAll(view.getPoints());
 
-        float degreeRange = radians(25/2); // range to randomly sample readings within
+        float degreeRange = radians(10); // range to randomly sample readings within
         int numSampleReadings = 10; // 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.
@@ -98,8 +98,10 @@ public class SLAM{
             // 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);
 
-            // check if the sub sample forms a valid line and remove the randomSample points if it does.
-            extractFeature(randomSample, maxRange, consensus);
+            if(randomSample.size() >= numSampleReadings){
+                // check if the sub sample forms a valid line and remove the randomSample points if it does.
+                extractFeature(randomSample, maxRange, consensus);
+            }
 
         }
 

src/Vector/Line.java

@@ -37,26 +37,28 @@ public class Line implements LineInterface{
 
         // this section calculates the direction vector of the line of best fit
         Vector direction = new Vector();
-        float length = 1;
+        float length = 0;
         // get the rise and run of the line of best fit
         for(Vector point : points){
             direction.y += (point.x - mean.x)*(point.y - mean.y); // rise
             direction.x += pow((point.x - mean.x),2);
 
-            // find the point that's furthest from the mean and use it to set the line length.
+            // get the average distance avery point is away from the mean.
             float dist = abs(point.sub(mean).mag());
-            if(dist > length){
-                length = 2*dist;
-            }
+            length += dist;
 
         }
+        length = 2.5f*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);
         }
         else{
             this.direction = new Vector(1, direction.y/direction.x);
         }
+        // scale the direction vector to be the correct length of the line.
         this.direction = this.direction.normalize().mul(length);
+
         this.position = mean.sub(this.direction.div(2));
     }