diff --git a/map.txt b/map.txt index 42be74f..7a823b0 100644 --- a/map.txt +++ b/map.txt @@ -1,109 +1,112 @@ -numVerts,58 -numEdges,49 -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 +numVerts,60 +numEdges,50 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,554.0,422.0 +vert,525.0,281.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,482.0,615.0 +vert,545.0,326.0 +vert,520.0,338.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,520.0,399.0 +vert,367.0,375.0 +vert,599.0,418.0 +vert,591.0,332.0 +vert,483.0,810.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 -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 +vert,601.0,491.0 +vert,165.0,235.0 +vert,405.0,237.0 +vert,606.0,811.0 +vert,165.0,809.0 +vert,213.0,812.0 +vert,165.0,521.0 +vert,223.0,273.0 +vert,294.0,378.0 +vert,267.0,616.0 +vert,493.0,238.0 +vert,403.0,483.0 +vert,210.0,380.0 +vert,330.0,376.0 +vert,714.0,488.0 +vert,603.0,537.0 +vert,715.0,655.0 +vert,224.0,247.0 +vert,267.0,723.0 +vert,503.0,357.0 +vert,618.0,397.0 +vert,713.0,539.0 +vert,334.0,727.0 +vert,485.0,693.0 +vert,211.0,346.0 +vert,300.0,277.0 +vert,294.0,347.0 +vert,577.0,423.0 +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,33,end,48 -edge,end,22,start,33 -edge,start,35,end,51 -edge,end,13,start,36 +edge,start,32,end,42 +edge,start,32,end,50 +edge,end,8,start,36 +edge,start,36,end,57 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 +edge,end,33,start,37 +edge,start,38,end,54 +edge,end,10,start,38 +edge,end,25,start,40 +edge,start,40,end,46 +edge,end,34,start,43 +edge,start,43,end,47 +edge,end,6,start,44 +edge,end,11,start,45 +edge,end,13,start,48 +edge,end,49,start,51 +edge,end,33,start,51 +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 \ No newline at end of file diff --git a/src/Car.java b/src/Car.java index 1333991..6a0f716 100644 --- a/src/Car.java +++ b/src/Car.java @@ -38,11 +38,13 @@ 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.slam.drawFeatures(proc); + this.updateScan(map); + if(!SLAMIsHidden){ + this.slam.drawFeatures(proc); + } } //With all the views that the car has, get their point list diff --git a/src/Processing.java b/src/Processing.java index 1d88ec2..139721c 100644 --- a/src/Processing.java +++ b/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(){ diff --git a/src/SLAM.java b/src/SLAM.java index 1096159..74fedeb 100644 --- a/src/SLAM.java +++ b/src/SLAM.java @@ -11,7 +11,7 @@ import static processing.core.PApplet.*; public class SLAM{ ArrayList lines = new ArrayList<>(); - ArrayList unassociatedPoints = new ArrayList<>(); + ShortTermMem unassociatedPoints = new ShortTermMem(); private static PApplet proc; SLAM(PApplet processing){ @@ -27,7 +27,7 @@ public class SLAM{ */ private List randomSampleInAngleRange(ArrayList 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 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 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 newRandomSample = new ArrayList<>(); for (Vector v : randomSample) { @@ -73,17 +74,35 @@ public class SLAM{ } } + private void fitToPreviousReadings(List sample, float maxRange){ + // keep track of points that were succesffully associated so they can be removed from the sample at the end + ArrayList 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 - int numSampleReadings = 10; // number of readings to randomly sample + float degreeRange = radians(5); // range to randomly sample readings within + 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. - float maxRange = 5; // the maximum distance a point can be away from the line for it to count as a consensus + int consensus = 10; // 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 // 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 randomSample = this.randomSampleInAngleRange(this.unassociatedPoints, numSampleReadings, randomAngle-degreeRange, randomAngle+degreeRange); + List 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); } diff --git a/src/ShortTermMem.java b/src/ShortTermMem.java new file mode 100644 index 0000000..5124ad5 --- /dev/null +++ b/src/ShortTermMem.java @@ -0,0 +1,59 @@ +import java.util.ArrayList; +import Vector.Vector; +public class ShortTermMem { + ArrayList> scans = new ArrayList<>(); + ArrayList scanPositions = new ArrayList<>(); + + ArrayList scanTimes = new ArrayList<>(); + private int size = 0; + + public void addScan(Vector scanPosition, ArrayList scan){ + size += scan.size(); + scans.add(scan); + scanPositions.add(scanPosition); + scanTimes.add(System.currentTimeMillis()); + purgeScans(); + } + + public ArrayList getPoints(){ + ArrayList points = new ArrayList<>(); + for(ArrayList pointList : this.scans){ + points.addAll(pointList); + } + return points; + } + + public void remove(Vector point){ + for(ArrayList 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; + } +} diff --git a/src/Vector/Line.java b/src/Vector/Line.java index 78f910e..aa346b2 100644 --- a/src/Vector/Line.java +++ b/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 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 points){ + public void bestFit(List 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){ diff --git a/src/Vector/Vector.java b/src/Vector/Vector.java index 8bdf9e8..4887ef2 100644 --- a/src/Vector/Vector.java +++ b/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); + } } diff --git a/src/View.java b/src/View.java index e36141b..3cc745e 100644 --- a/src/View.java +++ b/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); } } }