Tweaking SLAM parameters to get better accuracy

map.txt

@@ -1,109 +1,109 @@
 numVerts,58
 numEdges,49
-vert,165.0,809.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
-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,483.0,810.0
-vert,601.0,491.0
+vert,329.0,345.0
-vert,267.0,723.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,210.0,380.0
+vert,503.0,357.0
+vert,715.0,655.0
+vert,404.0,666.0
+vert,602.0,652.0
+vert,714.0,488.0
+vert,405.0,237.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,267.0,616.0
+vert,334.0,727.0
+vert,165.0,235.0
+vert,545.0,326.0
+vert,224.0,247.0
 vert,603.0,346.0
-edge,start,0,end,34
+edge,start,3,end,56
-edge,start,3,end,16
+edge,start,3,end,34
-edge,end,0,start,4
+edge,start,4,end,38
-edge,start,6,end,24
+edge,start,5,end,50
-edge,start,6,end,36
+edge,start,5,end,16
-edge,start,11,end,30
+edge,start,6,end,15
-edge,start,12,end,21
+edge,start,6,end,18
-edge,start,14,end,45
+edge,start,7,end,55
-edge,start,14,end,26
+edge,start,8,end,19
-edge,end,10,start,15
+edge,start,9,end,32
-edge,start,16,end,57
+edge,start,10,end,56
-edge,start,17,end,20
+edge,start,10,end,34
-edge,start,17,end,39
+edge,start,12,end,41
-edge,start,19,end,27
+edge,end,9,start,13
-edge,start,21,end,23
+edge,start,14,end,20
-edge,start,22,end,45
+edge,end,14,start,19
-edge,start,22,end,26
+edge,end,12,start,20
-edge,start,23,end,29
+edge,start,21,end,52
-edge,end,1,start,25
+edge,start,21,end,53
-edge,end,18,start,25
+edge,end,1,start,23
-edge,start,27,end,49
+edge,start,25,end,38
-edge,end,4,start,28
+edge,end,24,start,26
-edge,start,29,end,38
+edge,start,26,end,45
-edge,end,19,start,30
+edge,end,17,start,27
-edge,end,3,start,31
+edge,end,2,start,28
-edge,end,10,start,32
+edge,end,0,start,28
-edge,start,33,end,42
+edge,start,29,end,30
-edge,start,34,end,46
+edge,start,30,end,57
-edge,start,37,end,51
+edge,end,2,start,31
-edge,start,37,end,52
+edge,end,0,start,31
-edge,start,38,end,40
+edge,end,8,start,32
-edge,end,11,start,40
+edge,start,33,end,48
-edge,start,41,end,51
+edge,end,22,start,33
-edge,start,41,end,52
+edge,start,35,end,51
-edge,end,2,start,43
+edge,end,13,start,36
-edge,end,9,start,44
+edge,start,37,end,49
-edge,end,7,start,44
+edge,start,40,end,48
-edge,end,8,start,46
+edge,end,22,start,40
-edge,end,35,start,48
+edge,end,7,start,41
-edge,end,5,start,48
+edge,end,11,start,43
-edge,end,31,start,49
+edge,end,42,start,44
-edge,end,13,start,50
+edge,end,39,start,44
-edge,start,53,end,54
+edge,start,46,end,54
-edge,end,47,start,53
+edge,start,47,end,52
-edge,end,20,start,55
+edge,start,47,end,53
-edge,end,39,start,55
+edge,end,35,start,49
-edge,end,35,start,56
+edge,end,37,start,54
-edge,end,5,start,56
+edge,end,29,start,55
-edge,end,12,start,57
+edge,end,36,start,57

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);
+        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);
 
+            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 += dist;
-                length = 2*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));
     }