Cynopolis/SLAM-Sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Wrote tests for the Vector class

Browse Source
This commit is contained in:
Quinn
2023-04-06 10:39:31 -05:00
parent fc9d4e497b
commit 7438d0f64e
19 changed files with 328 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files
SLAM-Sim.iml
+27
View File
@@ -4,6 +4,7 @@
<exclude-output />
<content url="file://$MODULE_DIR$">
<sourceFolder url="file://$MODULE_DIR$/src" isTestSource="false" />
<sourceFolder url="file://$MODULE_DIR$/tests" isTestSource="true" />
</content>
<orderEntry type="inheritedJdk" />
<orderEntry type="sourceFolder" forTests="false" />
@@ -16,5 +17,31 @@
<SOURCES />
</library>
</orderEntry>
<orderEntry type="module-library" scope="TEST">
<library name="JUnit4">
<CLASSES>
<root url="jar://$MODULE_DIR$/lib/junit-4.13.1.jar!/" />
<root url="jar://$MODULE_DIR$/lib/hamcrest-core-1.3.jar!/" />
</CLASSES>
<JAVADOC />
<SOURCES />
</library>
</orderEntry>
<orderEntry type="module-library" scope="TEST">
<library name="JUnit5.8.1">
<CLASSES>
<root url="jar://$MODULE_DIR$/lib/junit-jupiter-5.8.1.jar!/" />
<root url="jar://$MODULE_DIR$/lib/junit-jupiter-api-5.8.1.jar!/" />
<root url="jar://$MODULE_DIR$/lib/opentest4j-1.2.0.jar!/" />
<root url="jar://$MODULE_DIR$/lib/junit-platform-commons-1.8.1.jar!/" />
<root url="jar://$MODULE_DIR$/lib/apiguardian-api-1.1.2.jar!/" />
<root url="jar://$MODULE_DIR$/lib/junit-jupiter-params-5.8.1.jar!/" />
<root url="jar://$MODULE_DIR$/lib/junit-jupiter-engine-5.8.1.jar!/" />
<root url="jar://$MODULE_DIR$/lib/junit-platform-engine-1.8.1.jar!/" />
</CLASSES>
<JAVADOC />
<SOURCES />
</library>
</orderEntry>
</component>
</module>
lib/apiguardian-api-1.1.2.jar
BIN
View File
Binary file not shown.
lib/hamcrest-core-1.3.jar
BIN
View File
Binary file not shown.
lib/junit-4.13.1.jar
BIN
View File
Binary file not shown.
lib/junit-jupiter-5.8.1.jar
BIN
View File
Binary file not shown.
lib/junit-jupiter-api-5.8.1.jar
BIN
View File
Binary file not shown.
lib/junit-jupiter-engine-5.8.1.jar
BIN
View File
Binary file not shown.
lib/junit-jupiter-params-5.8.1.jar
BIN
View File
Binary file not shown.
lib/junit-platform-commons-1.8.1.jar
BIN
View File
Binary file not shown.
lib/junit-platform-engine-1.8.1.jar
BIN
View File
Binary file not shown.
lib/opentest4j-1.2.0.jar
BIN
View File
Binary file not shown.
src/Car.java
+6 -6
View File
@@ -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<PVector> pointList = view.getPoints();
ArrayList<Vector> 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);
src/Line.java
+98
View File
@@ -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<Vector> points){
bestFit(points);
proc = processing;
}
// least squares line of best fit algorithm
private void bestFit(List<Vector> 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();
}
}
src/Processing.java
+4 -5
View File
@@ -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});
}
src/SLAM.java
+22 -76
View File
@@ -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<Line> 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<PVector> randomSample(ArrayList<PVector> set, int indexRange, int subSampleSize){
private List<Vector> randomSample(ArrayList<Vector> 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<PVector> subSample = set.subList(randomIdx - indexRange, randomIdx + indexRange); // get the sub-sample
List<Vector> 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<PVector> randomSample = subSample.subList(0, subSampleSize); // get our random sample
List<Vector> 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<PVector> originalList, List<PVector> randomSample, float maxRange, int consensus){
private void extractFeature(ArrayList<Vector> originalList, List<Vector> randomSample, float maxRange, int consensus){
// get a line of best fit for this list.
Line bestFit = new Line(proc, randomSample);
int count = 0;
ArrayList<PVector> newRandomSample = new ArrayList<>();
for (PVector v : randomSample) {
ArrayList<Vector> 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<PVector> 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<Vector> 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<PVector> randomSample = this.randomSample(newPoints, indexRange, numSampleReadings);
List<Vector> 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<PVector> points){
bestFit(points);
proc = processing;
}
// least squares line of best fit algorithm
private void bestFit(List<PVector> 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();
}
}
src/Vector.java
+68
View File
@@ -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);
}
}
src/View.java
+16 -16
View File
@@ -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<Ray> 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<PVector> getPoints(){
ArrayList<PVector> points = new ArrayList<>();
public ArrayList<Vector> getPoints(){
ArrayList<Vector> 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;
}
src/Wall.java
+3 -3
View File
@@ -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;
}
tests/VectorTest.java
+84
View File
@@ -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);
}
}