diff --git a/SLAM-Sim.iml b/SLAM-Sim.iml index 36e1ef0..42bad0a 100644 --- a/SLAM-Sim.iml +++ b/SLAM-Sim.iml @@ -4,6 +4,7 @@ + @@ -16,5 +17,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/lib/apiguardian-api-1.1.2.jar b/lib/apiguardian-api-1.1.2.jar new file mode 100644 index 0000000..2b678e1 Binary files /dev/null and b/lib/apiguardian-api-1.1.2.jar differ diff --git a/lib/hamcrest-core-1.3.jar b/lib/hamcrest-core-1.3.jar new file mode 100644 index 0000000..9d5fe16 Binary files /dev/null and b/lib/hamcrest-core-1.3.jar differ diff --git a/lib/junit-4.13.1.jar b/lib/junit-4.13.1.jar new file mode 100644 index 0000000..b376ffc Binary files /dev/null and b/lib/junit-4.13.1.jar differ diff --git a/lib/junit-jupiter-5.8.1.jar b/lib/junit-jupiter-5.8.1.jar new file mode 100644 index 0000000..730b9ae Binary files /dev/null and b/lib/junit-jupiter-5.8.1.jar differ diff --git a/lib/junit-jupiter-api-5.8.1.jar b/lib/junit-jupiter-api-5.8.1.jar new file mode 100644 index 0000000..8424eca Binary files /dev/null and b/lib/junit-jupiter-api-5.8.1.jar differ diff --git a/lib/junit-jupiter-engine-5.8.1.jar b/lib/junit-jupiter-engine-5.8.1.jar new file mode 100644 index 0000000..cfa38d2 Binary files /dev/null and b/lib/junit-jupiter-engine-5.8.1.jar differ diff --git a/lib/junit-jupiter-params-5.8.1.jar b/lib/junit-jupiter-params-5.8.1.jar new file mode 100644 index 0000000..1e4d0ec Binary files /dev/null and b/lib/junit-jupiter-params-5.8.1.jar differ diff --git a/lib/junit-platform-commons-1.8.1.jar b/lib/junit-platform-commons-1.8.1.jar new file mode 100644 index 0000000..20185cd Binary files /dev/null and b/lib/junit-platform-commons-1.8.1.jar differ diff --git a/lib/junit-platform-engine-1.8.1.jar b/lib/junit-platform-engine-1.8.1.jar new file mode 100644 index 0000000..54ce076 Binary files /dev/null and b/lib/junit-platform-engine-1.8.1.jar differ diff --git a/lib/opentest4j-1.2.0.jar b/lib/opentest4j-1.2.0.jar new file mode 100644 index 0000000..d500636 Binary files /dev/null and b/lib/opentest4j-1.2.0.jar differ diff --git a/src/Car.java b/src/Car.java index 31c602b..852046d 100644 --- a/src/Car.java +++ b/src/Car.java @@ -3,11 +3,10 @@ import java.util.ArrayList; import static java.lang.Math.PI; import static processing.core.PApplet.degrees; import static processing.core.PApplet.radians; -import processing.core.PVector; import processing.core.PApplet; public class Car{ - PVector pose = new PVector(0,0); // the car's x, y position + Vector pose = new Vector(0,0); // the car's x, y position float angle = 0; // the current angle that the car is at. int carLength = 50; int carWidth = 40; @@ -25,7 +24,7 @@ public class Car{ Car(PApplet processing, int xPos, int yPos, int carLength, int carWidth){ proc = processing; slam = new SLAM(proc); - this.pose = new PVector(xPos, yPos); + this.pose = new Vector(xPos, yPos); this.carLength = carLength; this.carWidth = carWidth; } @@ -40,6 +39,7 @@ public class Car{ proc.stroke(255); proc.ellipse(pose.x, pose.y, carWidth, carLength); this.updateScan(walls); + this.slam.drawLines(); } //With all the views that the car has, get their point list @@ -49,12 +49,12 @@ public class Car{ } for(View view : views){ - ArrayList pointList = view.getPoints(); + ArrayList pointList = view.getPoints(); slam.RANSAC(pointList, view.getFOV() / view.getRayNum()); } } - public PVector getPose(){ + public Vector getPose(){ return pose; } @@ -75,7 +75,7 @@ public class Car{ } } - public void setPose(PVector newPose){ + public void setPose(Vector newPose){ pose = newPose; for(View view : views){ view.setPos(pose); diff --git a/src/Line.java b/src/Line.java new file mode 100644 index 0000000..22b7738 --- /dev/null +++ b/src/Line.java @@ -0,0 +1,98 @@ +import processing.core.PApplet; + +import java.util.List; + +import static processing.core.PApplet.abs; +import static processing.core.PApplet.pow; + +public class Line{ + Vector direction = new Vector(0,0); + Vector position = new Vector(0,0); + float length = 0; + private static PApplet proc; + + Line(PApplet processing, Vector startPosition, Vector endPosition){ + this.proc = processing; + this.position = startPosition; + this.direction = endPosition.sub(startPosition).normalize(); + this.length = direction.mag(); + } + Line(PApplet processing, Vector direction, Vector position, float lineLength){ + this.direction = direction.normalize(); + this.position = position; + this.length = lineLength; + proc = processing; + } + + /** + * attempt to find the line of best fit for the given points + * @param points the points to get the line of best for + */ + Line(PApplet processing, List points){ + bestFit(points); + proc = processing; + } + + // least squares line of best fit algorithm + private void bestFit(List points){ + // get the mean of all the points + Vector mean = new Vector(); + for(Vector point : points){ + mean.add(point); + } + mean.div(points.size()); + + // this section calculates the direction vector of the line of best fit + Vector direction = new Vector(); + + // 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. + float dist = abs(point.sub(mean).mag()); + if(dist > this.length){ + this.length = 2*dist; + } + + } + + this.position = mean.sub(direction.div(direction.mag()).mul(this.length / 2)); + this.direction = direction.normalize(); + } + + public Vector getSlopeIntForm(){ + float slope = direction.y / direction.x; + float intercept = position.y - slope * position.x; + return new Vector(slope, intercept); + } + + public Vector getDirection(){ + return direction; + } + + public Vector getPosition(){ + return position; + } + + public float getLength(){ + return length; + } + + /** + * Draw the line on screen + */ + public void draw(){ + Vector endPoint = position.add(direction.mul(length)); + proc.line(position.x, position.y, endPoint.x, endPoint.y); + } + + /** + * @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(); + } +} \ No newline at end of file diff --git a/src/Processing.java b/src/Processing.java index 1f66aae..e3c3492 100644 --- a/src/Processing.java +++ b/src/Processing.java @@ -1,5 +1,4 @@ import processing.core.PApplet; -import processing.core.PVector; import java.util.ArrayList; @@ -20,15 +19,15 @@ public class Processing extends PApplet { size(1000, 1000); car.addView(180,180); for(int i = 0; i < 20; i++){ - Wall wall = new Wall(processing, new PVector((int)random(40, 1840), (int)random(40, 960)), (int)random(360), (int)random(100, 1000)); + Wall wall = new Wall(processing, new Vector((int)random(40, 1840), (int)random(40, 960)), (int)random(360), (int)random(100, 1000)); objects.add(wall); } } public void draw(){ background(0); - for(Wall object : objects){ - object.drawWall(); - } +// for(Wall object : objects){ +// object.drawWall(); +// } car.drawCar(objects); //car.drive(new int[] {0, 0}); } diff --git a/src/SLAM.java b/src/SLAM.java index dadc677..449f137 100644 --- a/src/SLAM.java +++ b/src/SLAM.java @@ -1,11 +1,10 @@ -import static processing.core.PApplet.radians; import processing.core.*; import java.util.ArrayList; import java.util.Collections; import java.util.List; -import static processing.core.PApplet.pow; +import static processing.core.PApplet.*; public class SLAM{ ArrayList lines = new ArrayList<>(); @@ -21,15 +20,18 @@ public class SLAM{ * @param subSampleSize the size of the sub sample * @return A random subset of the set within an indexRange and of size: subSampleSize */ - private List randomSample(ArrayList set, int indexRange, int subSampleSize){ + private List randomSample(ArrayList set, int indexRange, int subSampleSize){ // select a random laser data reading int randomIdx = (int) proc.random(set.size() - 1); // index of starter reading - PVector point = set.get(randomIdx); // point of starter reading + Vector point = set.get(randomIdx); // point of starter reading // get a random sample of size numSampleReadings within degreeRange degrees of this laser reading. - List subSample = set.subList(randomIdx - indexRange, randomIdx + indexRange); // get the sub-sample + List subSample; + int rangeStart = randomIdx - indexRange >= 0 ? randomIdx - indexRange : 0; + int rangeEnd = randomIdx + indexRange < set.size() ? randomIdx + indexRange : set.size()-1; + subSample = set.subList(rangeStart, rangeEnd); // get the sub-sample Collections.shuffle(subSample); // shuffle the list - List randomSample = subSample.subList(0, subSampleSize); // get our random sample + List randomSample = subSample.subList(0, rangeEnd-rangeStart); // get our random sample if (!randomSample.contains(point)) { randomSample.add(point); } @@ -43,12 +45,12 @@ public class SLAM{ * @param maxRange the maximum distance away from the line of best fit of the subSample of points for a given point's consensus to count. * @param consensus the number of points that have to give their consensus for the line of best fit to count as a valid feature. */ - private void extractFeature(ArrayList originalList, List randomSample, float maxRange, int consensus){ + private void extractFeature(ArrayList originalList, List randomSample, float maxRange, int consensus){ // get a line of best fit for this list. Line bestFit = new Line(proc, randomSample); int count = 0; - ArrayList newRandomSample = new ArrayList<>(); - for (PVector v : randomSample) { + ArrayList newRandomSample = new ArrayList<>(); + for (Vector v : randomSample) { if (bestFit.getDistance(v) <= maxRange) { count++; newRandomSample.add(v); @@ -59,12 +61,17 @@ public class SLAM{ bestFit = new Line(proc, newRandomSample.subList(0, newRandomSample.size() - 1)); lines.add(bestFit); // remove the associated readings from the total available readings. - for (PVector v : newRandomSample) { + for (Vector v : newRandomSample) { originalList.remove(v); } } } - public void RANSAC(ArrayList newPoints, float raysPerDegree){ + + /** + * @param newPoints a new scan of points to perform feature detection on + * @param raysPerDegree How many degrees apart are each ray that was cast + */ + public void RANSAC(ArrayList newPoints, float raysPerDegree){ float degreeRange = radians(10/2); // range to randomly sample readings within int indexRange = (int) (degreeRange / raysPerDegree); int numSampleReadings = 10; // number of readings to randomly sample @@ -83,7 +90,7 @@ public class SLAM{ } // get a random sub sample of newPoints within the index range of a given size - List randomSample = this.randomSample(newPoints, indexRange, numSampleReadings); + List randomSample = this.randomSample(newPoints, indexRange, numSampleReadings); // check if the sub sample forms a valid line and remove the randomSample points if it does. extractFeature(newPoints, randomSample, maxRange, consensus); @@ -92,71 +99,10 @@ public class SLAM{ } -} - -class Line{ - PVector direction = new PVector(0,0); - PVector position = new PVector(0,0); - private static PApplet proc; - - Line(PApplet processing){ - proc = processing; - } - Line(PApplet processing, PVector direction, PVector position){ - this.direction = direction; - this.position = position; - proc = processing; - } - - /** - * attempt to find the line of best fit for the given points - * @param points the points to get the line of best for - */ - Line(PApplet processing, List points){ - bestFit(points); - proc = processing; - } - - // least squares line of best fit algorithm - private void bestFit(List points){ - // get the mean of all the points - PVector mean = new PVector(); - for(PVector point : points){ - mean.add(point); + public void drawLines(){ + for(Line line : lines){ + line.draw(); } - mean.div(points.size()); - - // this section calculates the direction vector of the line of best fit - PVector direction = new PVector(); - // get the rise and run of the line of best fit - for(PVector point : points){ - direction.y += (point.x - mean.x)*(point.y - mean.y); // rise - direction.x += pow((point.x - mean.x),2); - } - - this.position = mean; - this.direction = direction; } - public PVector getSlopeIntForm(){ - float slope = direction.y / direction.x; - float intercept = position.y - slope * position.x; - return new PVector(slope, intercept); - } - - public PVector getDirection(){ - return direction; - } - - public PVector getPosition(){ - return position; - } - - /** - * @param point - * @return the smallest distance from the point to this line - */ - public float getDistance(PVector point){ - return (point.sub(position).cross(direction)).mag() / direction.mag(); - } } \ No newline at end of file diff --git a/src/Vector.java b/src/Vector.java new file mode 100644 index 0000000..64e2823 --- /dev/null +++ b/src/Vector.java @@ -0,0 +1,68 @@ +import static java.lang.Math.sqrt; + +public class Vector { + public float x = 0; + public float y = 0; + public float z = 0; + + Vector(){} + Vector(float x, float y){ + this.x = x; + this.y = y; + } + + Vector(float x, float y, float z){ + this.x = x; + this.y = y; + this.z = z; + } + + Vector add(Vector other){ + return new Vector(this.x + other.x, this.y + other.y, this.z + other.z); + } + + Vector add(float x, float y){ + return new Vector(this.x + x, this.y + y); + } + + Vector add(float x, float y, float z){ + return new Vector(this.x + x, this.y + y, this.z + z); + } + + Vector sub(Vector other){ + return new Vector(this.x - other.x, this.y - other.y, this.z - other.z); + } + + Vector sub(float x, float y){ + return new Vector(this.x - x, this.y - y); + } + + Vector sub(float x, float y, float z){ + return new Vector(this.x - x, this.y - y, this.z - z); + } + + Vector mul(float scalar){ + return new Vector(this.x * scalar, this.y * scalar, this.z * scalar); + } + + Vector div(float scalar){ + return mul(1/scalar); + } + + float mag(){ + return (float)sqrt(x*x + y*y + z*z); + } + + float dot(Vector other){ + return x * other.x + y * other.y + z * other.z; + } + + Vector cross(Vector other){ + return new Vector(this.y*other.z - this.z*other.y, this.z*other.x - this.x*other.z, this.x*other.y - this.y*other.x); + } + + Vector normalize(){ + float mag = this.mag(); + return new Vector(x / mag, y / mag, z / mag); + } +} diff --git a/src/View.java b/src/View.java index 32375f8..417968b 100644 --- a/src/View.java +++ b/src/View.java @@ -5,14 +5,14 @@ import java.util.Objects; import static processing.core.PApplet.*; public class View{ - PVector pose; + Vector pose; float angle = 0; float FOV; ArrayList rays = new ArrayList<>(); private static PApplet proc; //the x,y position of the view, what angle it's looking at and its FOV - View(PApplet processing, PVector newPose, int numberOfRays, float FOV){ + View(PApplet processing, Vector newPose, int numberOfRays, float FOV){ proc = processing; this.pose = newPose; this.FOV = FOV; @@ -23,7 +23,7 @@ public class View{ public void setRayNum(int numberOfRays, float FOV, float angleOffset){ float rayStep = FOV/numberOfRays; rays.clear(); - float angle = (float) (0.01-angleOffset); //the 0.01 fixes some bugs + float angle = (float)(0.01-angleOffset); //the 0.01 fixes some bugs for(int i = 0; i < numberOfRays; i++){ Ray ray = new Ray(proc, pose, 100000, angle); angle = angle + rayStep; @@ -40,7 +40,7 @@ public class View{ } //changes the position of the view - public void setPos(PVector newPose){ + public void setPos(Vector newPose){ pose = newPose; for(Ray ray : rays){ray.setPos(pose);} } @@ -57,7 +57,7 @@ public class View{ this.setRayNum(this.rays.size(), this.FOV, this.angle); } - public PVector getPos(){return pose;} + public Vector getPos(){return pose;} public float getAngle(){return this.angle;} @@ -66,11 +66,11 @@ public class View{ public int getRayNum(){return this.rays.size();} //gets the point that each ray has collided with - public ArrayList getPoints(){ - ArrayList points = new ArrayList<>(); + public ArrayList getPoints(){ + ArrayList points = new ArrayList<>(); for(Ray ray : rays){ - if(!Objects.equals(ray.getPoint(), new PVector(0, 0) { + if(!Objects.equals(ray.getPoint(), new Vector(0, 0) { })){ points.add(ray.getPoint()); } @@ -80,14 +80,14 @@ public class View{ } class Ray{ - PVector pose; + Vector pose; int rayLength; int defaultRayLength; float angle; // IN RADIANS private static PApplet proc; //takes the starting position of the ray, the length of the ray, and it's casting angle (radians) - Ray(PApplet processing, PVector position, int defaultRayLength, float angle){ + Ray(PApplet processing, Vector position, int defaultRayLength, float angle){ proc = processing; this.pose = position; this.defaultRayLength = defaultRayLength; @@ -107,7 +107,7 @@ class Ray{ for(Wall object : objects){ float theta1 = angle; float theta2 = radians(object.getAngle()); - PVector wallPos = object.getPos(); + Vector wallPos = object.getPos(); //finds where along the wall the ray collides float b = (pose.x*sin(theta1) + wallPos.y*cos(theta1) - pose.y*cos(theta1) - wallPos.x*sin(theta1)) / (cos(theta2)*sin(theta1) - sin(theta2)*cos(theta1)); @@ -134,7 +134,7 @@ class Ray{ else this.rayLength = defaultRayLength; } - public PVector getPos(){ return pose;} + public Vector getPos(){ return pose;} public int getRayLength(){return this.rayLength;} @@ -145,16 +145,16 @@ class Ray{ } //returns the absolute position of the point - public PVector getPoint(){ + public Vector getPoint(){ if(this.rayLength != this.defaultRayLength){ - return new PVector(rayLength * (int)cos(this.angle) + pose.x, rayLength * (int)sin(this.angle) + pose.y); + return new Vector(rayLength * (int)cos(this.angle) + pose.x, rayLength * (int)sin(this.angle) + pose.y); } else{ - return new PVector(0,0); + return new Vector(0,0); } } - public void setPos(PVector newPose){ + public void setPos(Vector newPose){ pose = newPose; } diff --git a/src/Wall.java b/src/Wall.java index ee2a0bd..711a603 100644 --- a/src/Wall.java +++ b/src/Wall.java @@ -3,7 +3,7 @@ import processing.core.*; import static processing.core.PApplet.*; public class Wall{ - PVector pos; + Vector pos; float angle; int wallLength; private static PApplet proc; @@ -11,7 +11,7 @@ public class Wall{ int g; int b; - Wall(PApplet processing, PVector pos, float angle, int wallLength){ + Wall(PApplet processing, Vector pos, float angle, int wallLength){ proc = processing; this.pos = pos; this.angle = angle; @@ -27,7 +27,7 @@ public class Wall{ //ellipse((xPos + cos(radians(angle))*wallLength), (yPos + sin(radians(angle))*wallLength), 20, 20); } - PVector getPos(){ + Vector getPos(){ return pos; } diff --git a/tests/VectorTest.java b/tests/VectorTest.java new file mode 100644 index 0000000..8382c28 --- /dev/null +++ b/tests/VectorTest.java @@ -0,0 +1,84 @@ +import org.junit.jupiter.api.Test; +import processing.core.PApplet; + +import static java.lang.Math.*; +import static org.junit.jupiter.api.Assertions.*; + +class VectorTest{ + + @Test + public void vector2DOperations(){ + for(int i = 0; i < 20; i++){ + float x1 = (float)(1000*random() - 500); + float y1 = (float)(1000*random() - 500); + Vector v1 = new Vector(x1, y1); + for(int j = 0; j < 20; j++){ + float x2 = (float)(1000*random()); + float y2 = (float)(1000*random()); + Vector v2 = new Vector(x2, y2); + + // test general setters + assertFloatEquals(x1, v1.x); + assertFloatEquals(y1, v1.y); + assertFloatEquals(x2, v2.x); + assertFloatEquals(y2, v2.y); + + // test magnitude + assertFloatEquals((float)sqrt(x1*x1 + y1*y1), v1.mag()); + assertFloatEquals((float)sqrt(x2*x2 + y2*y2), v2.mag()); + + // test dot product + assertFloatEquals((float)(x1*x2+y1*y2), v1.dot(v2)); + assertFloatEquals((float)(x1*x2+y1*y2), v2.dot(v1)); + + // test addition + Vector vSum = v1.add(v2); + assertFloatEquals(x1+x2, vSum.x); + assertFloatEquals(y1+y2, vSum.y); + + // test subtraction + Vector vSub = v1.sub(v2); + assertFloatEquals(x1-x2, vSub.x); + assertFloatEquals(y1-y2, vSub.y); + vSub = v2.sub(v1); + assertFloatEquals(x2-x1, vSub.x); + assertFloatEquals(y2-y1, vSub.y); + + // test scaling + Vector vScale = v1.mul(x2); + assertFloatEquals(x1*x2, vScale.x); + assertFloatEquals(y1*x2, vScale.y); + + // test normalization + Vector vNorm = v1.normalize(); + assertFloatEquals(1, vNorm.mag()); + } + } + } + + public void assertFloatEquals(float expected, float actual){ + assertFloatEquals(expected, actual, (float)0.0001); + } + + public void assertFloatEquals(float expected, float actual, float range){ + assertTrue(abs(expected-actual) < range); + } + + @Test + public void testCrossProduct(){ + Vector v1 = new Vector(1, 2, 3); + Vector v2 = new Vector(4, 5, 6); + Vector cross = v1.cross(v2); + assertFloatEquals(-3, cross.x); + assertFloatEquals(6, cross.y); + assertFloatEquals(-3, cross.z); + + v1 = new Vector(-3, 7, -9); + v2 = new Vector((float)2.6, 66, (float)-3.14159); + cross = v1.cross(v2); + assertFloatEquals((float)572.00887, cross.x, (float)0.1); + assertFloatEquals((float)-32.82477, cross.y, (float)0.1); + assertFloatEquals((float)-216.2, cross.z, (float)0.1); + } + +} \ No newline at end of file