Merge pull request #9 from Cynopolis/view-class-returns-ScanPoint-objects

Started converting the view class over to using ScanPoints
2023-11-24 18:16:40 -05:00
parent 0c59839dfa 9deba45afd
commit b09e34d6e9
7 changed files with 176 additions and 139 deletions
--- a/.idea/inspectionProfiles/Project_Default.xml
+++ b/.idea/inspectionProfiles/Project_Default.xml
@@ -0,0 +1,8 @@
+<component name="InspectionProjectProfileManager">
+  <profile version="1.0">
+    <option name="myName" value="Project Default" />
+    <inspection_tool class="JavadocDeclaration" enabled="true" level="WARNING" enabled_by_default="true">
+      <option name="ADDITIONAL_TAGS" value="brief" />
+    </inspection_tool>
+  </profile>
+</component>
--- 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
+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
--- 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);

        }
--- 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
--- a/src/ScanGraph/ScanGraph.java
+++ b/src/ScanGraph/ScanGraph.java
@@ -100,8 +100,8 @@ class CorrespondenceMatrix{
        correspondenceMatrix.add(new ArrayList<Float>());

        // 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;
--- 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<Vector> scan;

-    ScanPoint(Vector scanPosition, Vector orientation, ArrayList<Vector> scan) {
+    public ScanPoint(Vector scanPosition, float orientation, ArrayList<Vector> 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<Vector> getScan(){
+    public ArrayList<Vector> getPoints(){
        return this.scan;
    }

--- 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<Ray> 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<Vector> 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<Vector> 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);
    }

    /**