From 62e72324350076bdaa1378047c043e1b5b3980dd Mon Sep 17 00:00:00 2001 From: Quinn Date: Wed, 3 May 2023 00:12:17 -0500 Subject: [PATCH] Tweaking SLAM parameters to get better accuracy --- map.txt | 208 +++++++++++++++++++++---------------------- src/Processing.java | 10 ++- src/SLAM.java | 8 +- src/Vector/Line.java | 12 +-- 4 files changed, 124 insertions(+), 114 deletions(-) diff --git a/map.txt b/map.txt index 3f28bc0..42be74f 100644 --- a/map.txt +++ b/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,601.0,491.0 -vert,267.0,723.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,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,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 +edge,start,3,end,56 +edge,start,3,end,34 +edge,start,4,end,38 +edge,start,5,end,50 +edge,start,5,end,16 +edge,start,6,end,15 +edge,start,6,end,18 +edge,start,7,end,55 +edge,start,8,end,19 +edge,start,9,end,32 +edge,start,10,end,56 +edge,start,10,end,34 +edge,start,12,end,41 +edge,end,9,start,13 +edge,start,14,end,20 +edge,end,14,start,19 +edge,end,12,start,20 +edge,start,21,end,52 +edge,start,21,end,53 +edge,end,1,start,23 +edge,start,25,end,38 +edge,end,24,start,26 +edge,start,26,end,45 +edge,end,17,start,27 +edge,end,2,start,28 +edge,end,0,start,28 +edge,start,29,end,30 +edge,start,30,end,57 +edge,end,2,start,31 +edge,end,0,start,31 +edge,end,8,start,32 +edge,start,33,end,48 +edge,end,22,start,33 +edge,start,35,end,51 +edge,end,13,start,36 +edge,start,37,end,49 +edge,start,40,end,48 +edge,end,22,start,40 +edge,end,7,start,41 +edge,end,11,start,43 +edge,end,42,start,44 +edge,end,39,start,44 +edge,start,46,end,54 +edge,start,47,end,52 +edge,start,47,end,53 +edge,end,35,start,49 +edge,end,37,start,54 +edge,end,29,start,55 +edge,end,36,start,57 \ No newline at end of file diff --git a/src/Processing.java b/src/Processing.java index 16e187f..1d88ec2 100644 --- a/src/Processing.java +++ b/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(){ diff --git a/src/SLAM.java b/src/SLAM.java index b945b4a..1096159 100644 --- a/src/SLAM.java +++ b/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 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); + } } diff --git a/src/Vector/Line.java b/src/Vector/Line.java index d5b2815..78f910e 100644 --- a/src/Vector/Line.java +++ b/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)); }