diff --git a/.idea/inspectionProfiles/Project_Default.xml b/.idea/inspectionProfiles/Project_Default.xml
new file mode 100644
index 0000000..53aac9c
--- /dev/null
+++ b/.idea/inspectionProfiles/Project_Default.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/map.txt b/map.txt
index 7a823b0..1edddbe 100644
--- a/map.txt
+++ b/map.txt
@@ -1,112 +1,112 @@
numVerts,60
numEdges,50
-vert,206.0,522.0
-vert,554.0,422.0
-vert,525.0,281.0
-vert,404.0,666.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,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
-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,520.0,399.0
vert,329.0,345.0
-vert,327.0,615.0
+vert,403.0,483.0
+vert,503.0,375.0
vert,367.0,347.0
+vert,715.0,655.0
+vert,300.0,277.0
+vert,165.0,809.0
+vert,713.0,810.0
+vert,294.0,378.0
+vert,165.0,521.0
+vert,165.0,235.0
+vert,602.0,652.0
+vert,327.0,615.0
+vert,599.0,418.0
+vert,211.0,346.0
+vert,482.0,615.0
+vert,259.0,476.0
vert,304.0,479.0
-vert,621.0,379.0
+vert,601.0,491.0
+vert,485.0,693.0
+vert,714.0,488.0
+vert,520.0,338.0
+vert,713.0,539.0
+vert,577.0,423.0
+vert,294.0,347.0
+vert,525.0,281.0
+vert,168.0,471.0
+vert,213.0,812.0
+vert,405.0,237.0
+vert,367.0,375.0
+vert,366.0,275.0
+vert,411.0,811.0
+vert,711.0,234.0
+vert,401.0,616.0
+vert,223.0,273.0
+vert,330.0,376.0
+vert,267.0,723.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,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,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
+vert,483.0,810.0
+vert,621.0,379.0
+vert,603.0,537.0
+vert,606.0,811.0
+vert,503.0,357.0
+vert,210.0,380.0
+vert,591.0,332.0
+vert,334.0,727.0
+vert,570.0,324.0
+vert,618.0,397.0
+vert,545.0,326.0
+vert,611.0,359.0
+vert,404.0,666.0
+vert,302.0,249.0
+vert,267.0,616.0
+vert,554.0,422.0
+vert,603.0,346.0
+vert,493.0,238.0
+vert,206.0,522.0
+edge,start,1,end,2
+edge,start,2,end,5
+edge,start,4,end,36
+edge,start,4,end,20
+edge,start,5,end,45
+edge,end,3,start,6
+edge,start,6,end,32
+edge,start,9,end,10
+edge,start,10,end,35
+edge,start,12,end,59
+edge,end,9,start,13
+edge,end,7,start,14
+edge,start,14,end,44
+edge,start,15,end,55
+edge,start,15,end,48
+edge,start,16,end,26
+edge,start,22,end,41
+edge,start,24,end,51
+edge,start,26,end,56
+edge,end,17,start,27
+edge,end,11,start,27
+edge,start,28,end,33
+edge,end,19,start,29
+edge,start,30,end,59
+edge,end,13,start,31
+edge,start,35,end,58
+edge,end,0,start,37
+edge,end,8,start,37
+edge,end,3,start,38
+edge,end,32,start,38
+edge,start,39,end,55
+edge,start,39,end,48
+edge,end,18,start,40
+edge,end,23,start,40
+edge,start,42,end,50
+edge,end,21,start,43
+edge,end,25,start,43
+edge,end,24,start,45
+edge,end,17,start,46
+edge,end,11,start,46
+edge,start,47,end,57
+edge,end,47,start,49
+edge,end,16,start,50
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
+edge,end,42,start,52
+edge,end,34,start,53
+edge,end,0,start,54
+edge,end,8,start,54
+edge,end,1,start,56
+edge,end,52,start,57
\ No newline at end of file
diff --git a/src/Car.java b/src/Car.java
index 6a0f716..4993166 100644
--- a/src/Car.java
+++ b/src/Car.java
@@ -50,7 +50,7 @@ public class Car{
//With all the views that the car has, get their point list
void updateScan(PointGraph map){
for(View view : views){
- view.look(map);
+ view.calculatePointScan(map);
slam.RANSAC(view);
}
diff --git a/src/SLAM.java b/src/SLAM.java
index 74fedeb..d722f2d 100644
--- a/src/SLAM.java
+++ b/src/SLAM.java
@@ -1,7 +1,6 @@
import Vector.*;
import processing.core.*;
-import java.lang.reflect.Array;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
@@ -96,7 +95,7 @@ public class SLAM{
* @param view a laser scan view
*/
public void RANSAC(View view){
- unassociatedPoints.addScan(view.getPos(), view.getPoints());
+ unassociatedPoints.addScan(view.getPos(), view.getScan().getPoints());
float degreeRange = radians(5); // range to randomly sample readings within
int numSampleReadings = 15; // number of readings to randomly sample
diff --git a/src/ScanGraph/ScanGraph.java b/src/ScanGraph/ScanGraph.java
index bb719b9..fb4c3e5 100644
--- a/src/ScanGraph/ScanGraph.java
+++ b/src/ScanGraph/ScanGraph.java
@@ -100,8 +100,8 @@ class CorrespondenceMatrix{
correspondenceMatrix.add(new ArrayList());
// go through all of the points in the new scan and find the closest point in the old scan
- for (int newPointIndex = 0; newPointIndex < newScan.getScan().size(); newPointIndex++) {
- Vector newPoint = newScan.getScan().get(newPointIndex);
+ for (int newPointIndex = 0; newPointIndex < newScan.getPoints().size(); newPointIndex++) {
+ Vector newPoint = newScan.getPoints().get(newPointIndex);
// if the new point is null, then skip it
if (newPoint == null) {
continue;
@@ -109,8 +109,8 @@ class CorrespondenceMatrix{
// find the closest point in the old scan
float closestDistance = Float.MAX_VALUE;
int closestIndex = -1;
- for (int j = 0; j < referenceScan.getScan().size(); j++) {
- Vector oldPoint = referenceScan.getScan().get(j);
+ for (int j = 0; j < referenceScan.getPoints().size(); j++) {
+ Vector oldPoint = referenceScan.getPoints().get(j);
// if the old point is null, then skip it
if (oldPoint == null) {
continue;
@@ -161,7 +161,7 @@ class ScanMatcher{
private SimpleMatrix averageScanPosition(ScanPoint scan){
Vector averagePosition = new Vector(0, 0);
int invalidPoints = 0;
- for (Vector point : scan.getScan()) {
+ for (Vector point : scan.getPoints()) {
if (point != null) {
averagePosition = averagePosition.add(point);
}
@@ -169,7 +169,7 @@ class ScanMatcher{
invalidPoints++;
}
}
- return new SimpleMatrix(averagePosition.div(scan.getScan().size() - invalidPoints).toArray());
+ return new SimpleMatrix(averagePosition.div(scan.getPoints().size() - invalidPoints).toArray());
}
/**
@@ -190,8 +190,8 @@ class ScanMatcher{
for (int i = 0; i < correspondenceMatrix.getOldPointIndices().size(); i++) {
int oldIndex = correspondenceMatrix.getOldPointIndices().get(i);
int newIndex = correspondenceMatrix.getNewPointIndices().get(i);
- Vector oldPoint = referenceScan.getScan().get(oldIndex);
- Vector newPoint = newScan.getScan().get(newIndex);
+ Vector oldPoint = referenceScan.getPoints().get(oldIndex);
+ Vector newPoint = newScan.getPoints().get(newIndex);
if (oldPoint != null && newPoint != null) {
Vector oldPointCentered = oldPoint.sub(referenceScanAveragePosition);
Vector newPointCentered = newPoint.sub(newScanAveragePosition);
@@ -232,12 +232,12 @@ class ScanMatcher{
public ScanPoint applyRotationAndTranslationMatrices(ScanPoint newScan){
// apply the rotation matrix and translation vector to the new scan
- for (int i = 0; i < newScan.getScan().size(); i++) {
- Vector point = newScan.getScan().get(i);
+ for (int i = 0; i < newScan.getPoints().size(); i++) {
+ Vector point = newScan.getPoints().get(i);
if (point != null) {
SimpleMatrix pointMatrix = new SimpleMatrix(point.toArray());
SimpleMatrix newPointMatrix = rotationMatrix.mult(pointMatrix).plus(translationVector);
- newScan.getScan().set(i, new Vector((float) newPointMatrix.get(0), (float) newPointMatrix.get(1)));
+ newScan.getPoints().set(i, new Vector((float) newPointMatrix.get(0), (float) newPointMatrix.get(1)));
}
}
return newScan;
diff --git a/src/ScanGraph/ScanPoint.java b/src/ScanGraph/ScanPoint.java
index 303756b..59e915a 100644
--- a/src/ScanGraph/ScanPoint.java
+++ b/src/ScanGraph/ScanPoint.java
@@ -8,10 +8,10 @@ import java.util.ArrayList;
public class ScanPoint extends Vertex{
private Vector position;
- private Vector orientation;
+ private float orientation;
private ArrayList scan;
- ScanPoint(Vector scanPosition, Vector orientation, ArrayList scan) {
+ public ScanPoint(Vector scanPosition, float orientation, ArrayList scan) {
super();
this.position = scanPosition;
this.orientation = orientation;
@@ -25,11 +25,11 @@ public class ScanPoint extends Vertex{
return position;
}
- public Vector getOrientation(){
+ public float getOrientation(){
return this.orientation;
}
- public ArrayList getScan(){
+ public ArrayList getPoints(){
return this.scan;
}
diff --git a/src/View.java b/src/View.java
index 3cc745e..eea7304 100644
--- a/src/View.java
+++ b/src/View.java
@@ -2,18 +2,25 @@ import Graph.PointGraph;
import Vector.Vector;
import processing.core.*;
import java.util.ArrayList;
+import ScanGraph.ScanPoint;
public class View {
- Vector pose;
+ Vector position;
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
+ /**
+ * @brief Constructor for the View class
+ * @param processing The PApplet that the view will be drawn on
+ * @param newPose The position of the view
+ * @param numberOfRays The number of rays that the view will have
+ * @param FOV The field of view of the view
+ */
View(PApplet processing, Vector newPose, int numberOfRays, float FOV) {
proc = processing;
- this.pose = newPose;
+ this.position = newPose;
this.FOV = FOV;
this.setRayNum(numberOfRays, FOV, this.angle);
}
@@ -24,34 +31,42 @@ public class View {
rays.clear();
float angle = (float) (angleOffset); //the 0.01 fixes some bugs
for (int i = 0; i < numberOfRays; i++) {
- Ray ray = new Ray(pose, angle);
+ Ray ray = new Ray(position, angle);
angle = angle + rayStep;
rays.add(ray);
}
}
- //sees if the ray will collide with the walls in the wall list
- public void look(PointGraph map) {
+ /**
+ * @brief Calculates the points of intersection of the rays with the map
+ * @param map The map that the view is looking at
+ */
+ public void calculatePointScan(PointGraph map) {
for (Ray ray : rays) {
ray.castRay(map);
if(ray.hasCollided()){
ray.getPoint().draw(proc);
-// ray.drawRay(proc);
}
}
}
- //changes the position of the view
- public void setPos(Vector newPose) {
- pose = newPose;
+ /**
+ * @brief Sets the position of the view
+ * @param newPosition The new position of the view
+ */
+ public void setPos(Vector newPosition) {
+ position = newPosition;
for (Ray ray : rays) {
- ray.setPos(pose);
+ ray.setPos(position);
}
}
- //changes the angle of the view
- public void setAngle(float angle) {
- this.angle = angle;
+ /**
+ * @brief Sets the angle of the view
+ * @param newAngle The new angle of the view
+ */
+ public void setAngle(float newAngle) {
+ this.angle = newAngle;
for(Ray ray : rays){
float angleOffset = ray.getAngle() - this.angle;
ray.setAngle(this.angle+angleOffset);
@@ -64,24 +79,39 @@ public class View {
this.setRayNum(this.rays.size(), this.FOV, this.angle);
}
+ /**
+ * @return The position of the view
+ */
public Vector getPos() {
- return pose;
+ return position;
}
+ /**
+ * @return The angle of the view
+ */
public float getAngle() {
return this.angle;
}
+ /**
+ * @return The field of view of the view
+ */
public float getFOV() {
return this.FOV;
}
+ /**
+ * @return The number of rays that the view has
+ */
public int getRayNum() {
return this.rays.size();
}
- //gets the point that each ray has collided with
- public ArrayList getPoints() {
+ /**
+ * @brief Get the most recent scan from the view
+ * @return A ScanPoint object containing the position, angle and points of the view
+ */
+ public ScanPoint getScan() {
ArrayList points = new ArrayList<>();
for (Ray ray : rays) {
@@ -92,7 +122,7 @@ public class View {
points.add(point);
}
}
- return points;
+ return new ScanPoint(this.position,this.angle, points);
}
/**