Improved the power of the scan matcher to better work with more cases.

Create unit tests for the scan matching algorithm

.gitignore

@@ -27,3 +27,6 @@ bin/
 
 ### Mac OS ###
 .DS_Store
+
+map.txt
+processing-4.3/

.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>

.idea/libraries/ejml.xml

@@ -0,0 +1,13 @@
+<component name="libraryTable">
+  <library name="ejml">
+    <CLASSES>
+      <root url="file://$PROJECT_DIR$/lib/ejml-v0.42-libs" />
+    </CLASSES>
+    <JAVADOC />
+    <SOURCES>
+      <root url="file://$PROJECT_DIR$/lib/ejml-v0.42-libs" />
+    </SOURCES>
+    <jarDirectory url="file://$PROJECT_DIR$/lib/ejml-v0.42-libs" recursive="false" />
+    <jarDirectory url="file://$PROJECT_DIR$/lib/ejml-v0.42-libs" recursive="false" type="SOURCES" />
+  </library>
+</component>

.idea/misc.xml

@@ -1,4 +1,3 @@
-<?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
   <component name="ProjectRootManager" version="2" languageLevel="JDK_20" default="true" project-jdk-name="openjdk-20" project-jdk-type="JavaSDK">
     <output url="file://$PROJECT_DIR$/out" />

SLAM-Sim.iml

@@ -43,5 +43,9 @@
         <SOURCES />
       </library>
     </orderEntry>
+    <orderEntry type="library" exported="" name="ejml" level="project" />
+
+
+
   </component>
 </module>

map.txt

@@ -1,79 +0,0 @@
-numVerts,43
-numEdges,34
-vert,601.0,491.0
-vert,602.0,652.0
-vert,300.0,277.0
-vert,213.0,812.0
-vert,715.0,655.0
-vert,294.0,378.0
-vert,259.0,476.0
-vert,224.0,247.0
-vert,304.0,479.0
-vert,485.0,693.0
-vert,713.0,810.0
-vert,367.0,347.0
-vert,223.0,273.0
-vert,404.0,666.0
-vert,329.0,345.0
-vert,482.0,615.0
-vert,606.0,811.0
-vert,483.0,810.0
-vert,267.0,616.0
-vert,603.0,537.0
-vert,330.0,376.0
-vert,267.0,723.0
-vert,714.0,488.0
-vert,401.0,616.0
-vert,294.0,347.0
-vert,206.0,522.0
-vert,711.0,234.0
-vert,493.0,238.0
-vert,302.0,249.0
-vert,403.0,483.0
-vert,334.0,727.0
-vert,168.0,471.0
-vert,487.0,485.0
-vert,327.0,615.0
-vert,405.0,237.0
-vert,211.0,346.0
-vert,210.0,380.0
-vert,165.0,235.0
-vert,411.0,811.0
-vert,165.0,809.0
-vert,713.0,539.0
-vert,367.0,375.0
-vert,165.0,521.0
-edge,start,1,end,4
-edge,start,1,end,16
-edge,start,3,end,25
-edge,start,9,end,17
-edge,start,10,end,26
-edge,start,11,end,14
-edge,start,11,end,41
-edge,end,7,start,12
-edge,end,2,start,12
-edge,start,13,end,38
-edge,end,0,start,19
-edge,start,19,end,40
-edge,end,14,start,20
-edge,start,20,end,41
-edge,end,18,start,21
-edge,start,21,end,30
-edge,start,24,end,35
-edge,end,5,start,24
-edge,start,26,end,27
-edge,end,7,start,28
-edge,end,2,start,28
-edge,end,23,start,29
-edge,end,8,start,29
-edge,end,6,start,31
-edge,end,15,start,32
-edge,end,22,start,32
-edge,end,18,start,33
-edge,end,30,start,33
-edge,start,34,end,37
-edge,end,35,start,36
-edge,end,5,start,36
-edge,start,37,end,39
-edge,end,10,start,39
-edge,end,25,start,42

src/Car.java

@@ -38,22 +38,21 @@ 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.updateScan(map);
-//        this.slam.drawLines();
+        if(!SLAMIsHidden){
+            this.slam.drawFeatures(proc);
+        }
     }
 
     //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);
 
-        for(View view : views){
-            ArrayList<Vector> pointList = view.getPoints();
-//            slam.RANSAC(pointList, view.getFOV() / view.getRayNum());
         }
     }
 

src/Graph/Edge.java

@@ -20,7 +20,7 @@ public class Edge {
      * @param vStart the vertex the edge starts at
      * @param vEnd the vertex the edge ends at
      */
-    Edge(Vertex vStart, Vertex vEnd){
+    protected Edge(Vertex vStart, Vertex vEnd){
         this.vStart = vStart;
         this.vEnd = vEnd;
     }

src/Graph/PointGraphWriter.java

@@ -52,10 +52,17 @@ public class PointGraphWriter {
         file.close();
     }
 
-    public PointGraph loadFile(String filename) throws FileNotFoundException, NumberFormatException {
+    public PointGraph loadFile(String filename) throws NumberFormatException {
         PointGraph g = new PointGraph();
         File file = new File(filename);
-        Scanner reader = new Scanner(file);
+        Scanner reader;
+        try {
+            reader = new Scanner(file);
+        }
+        catch (FileNotFoundException e){
+            System.out.println("File not found");
+            return g;
+        }
         ArrayList<PointVertex> vertices = new ArrayList<>();
         while(reader.hasNextLine()){
             String line = reader.nextLine();

src/Graph/PointVertex.java

@@ -4,6 +4,7 @@ import processing.core.PApplet;
 
 public class PointVertex extends Vertex {
     private Vector position;
+
     private int[] color = new int[]{127, 255, 0, 0};
 
     /**

src/Processing.java

@@ -12,6 +12,8 @@ public class Processing extends PApplet {
     public static PApplet processing;
 
     PointGraph map = new PointGraph();
+    boolean mapIsHidden = false;
+    boolean SLAMIsHidden = false;
 
     public static void main(String[] args) {
         PApplet.main("Processing");
@@ -21,7 +23,7 @@ public class Processing extends PApplet {
         processing = this;
         car = new Car(processing, 100,100,50,40);
         size(1000, 1000);
-        car.addView(360,180);
+        car.addView(90,180);
 
 //        for(int i = 0; i < 10; i++){
 //            PointVertex vStart = new PointVertex(random(50, 950), random(50, 950));
@@ -32,11 +34,12 @@ public class Processing extends PApplet {
     }
     public void draw(){
         background(0);
+        if(!mapIsHidden){
             map.draw(processing);
-        car.drawCar(map);
+        }
+        car.drawCar(map, SLAMIsHidden);
         strokeWeight(2);
         stroke(255);
-        //car.drive(new int[] {0, 0});
     }
 
     public void keyPressed(){
@@ -76,19 +79,20 @@ public class Processing extends PApplet {
         }
         if(key == 'l'){
             System.out.println("Attempting to load a map from file");
-            try{
             PointGraphWriter writer = new PointGraphWriter();
+            try {
                 map = writer.loadFile("map.txt");
-            }
+            } catch (NumberFormatException e) {
-            catch (FileNotFoundException e){
-                System.out.println("File not found");
-                e.printStackTrace();
-            }
-            catch (NumberFormatException e){
                 System.out.println("Number format incorrect");
                 e.printStackTrace();
             }
         }
+        if(key == 'h'){
+            mapIsHidden = !mapIsHidden;
+        }
+        if(key == 'j'){
+            SLAMIsHidden = !SLAMIsHidden;
+        }
     }
 
     public void mousePressed(){
@@ -117,7 +121,4 @@ public class Processing extends PApplet {
         PointVertex v = new PointVertex(clickPosition);
         map.addVertex(v);
     }
-
-
-
 }

src/SLAM.java

@@ -5,10 +5,12 @@ import java.util.ArrayList;
 import java.util.Collections;
 import java.util.List;
 
+import static java.lang.Math.random;
 import static processing.core.PApplet.*;
 
 public class SLAM{
     ArrayList<Line> lines = new ArrayList<>();
+    ShortTermMem unassociatedPoints = new ShortTermMem();
     private static PApplet proc;
 
     SLAM(PApplet processing){
@@ -17,38 +19,41 @@ public class SLAM{
 
     /**
      * @param set the set to take a sub sample of
-     * @param indexRange the range within to take the sub sample
      * @param subSampleSize the size of the sub sample
-     * @return A random subset of the set within an indexRange and of size: subSampleSize
+     * @param minAngle the minimum angle allowed in the subset
+     * @param maxAngle the maximum angle allowed in the subset
+     * @return A random subset of the set within the angle range
      */
-    private List<Vector> randomSample(ArrayList<Vector> set, int indexRange, int subSampleSize){
+    private List<Vector> randomSampleInAngleRange(ArrayList<Vector> set, int subSampleSize, float minAngle, float maxAngle){
-        // select a random laser data reading
-        int randomIdx = (int) proc.random(set.size() - 1); // index 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.
+        // create an arraylist with all points within the angle range from the given set
-        List<Vector> subSample;
+        ArrayList<Vector> pointsInAngleRange = new ArrayList<>();
-        int rangeStart = randomIdx - indexRange >= 0 ? randomIdx - indexRange : 0;
+        for(Vector point : set){
-        int rangeEnd = randomIdx + indexRange < set.size() ? randomIdx + indexRange : set.size()-1;
+            if(minAngle <= point.z && point.z <= maxAngle){
-        subSample = set.subList(rangeStart, rangeEnd); // get the sub-sample
+                pointsInAngleRange.add(point);
-        Collections.shuffle(subSample); // shuffle the list
+            }
-        List<Vector> randomSample = subSample.subList(0, rangeEnd-rangeStart); // get our random sample
-        if (!randomSample.contains(point)) {
-            randomSample.add(point);
         }
 
-        return randomSample;
+        // shuffle the list to randomize it
+        Collections.shuffle(pointsInAngleRange);
+
+        // if the list is too small, just return the whole list
+        if(pointsInAngleRange.size() < subSampleSize){
+            return pointsInAngleRange;
+        }
+        // return a subSample of the list
+        return pointsInAngleRange.subList(0, subSampleSize);
     }
 
     /**
-     * @param originalList the list which the randomSample of points originated from
      * @param randomSample a random subsampling of points from the originalList
      * @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<Vector> originalList, List<Vector> randomSample, float maxRange, int consensus){
+    private void extractFeature(List<Vector> 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<Vector> newRandomSample = new ArrayList<>();
         for (Vector v : randomSample) {
@@ -63,41 +68,69 @@ public class SLAM{
             lines.add(bestFit);
             // remove the associated readings from the total available readings.
             for (Vector v : newRandomSample) {
-                originalList.remove(v);
+                this.unassociatedPoints.remove(v);
             }
         }
     }
 
-    /**
+    private void fitToPreviousReadings(List<Vector> sample, float maxRange){
-     * @param newPoints a new scan of points to perform feature detection on
+        // keep track of points that were succesffully associated so they can be removed from the sample at the end
-     * @param raysPerDegree How many degrees apart are each ray that was cast
+        ArrayList<Vector> pointsToRemove = new ArrayList<>();
-     */
+        // try to associate points from the smaple with pre-existing lines
-    public void RANSAC(ArrayList<Vector> newPoints, float raysPerDegree){
+        for(Vector v: sample){
-        float degreeRange = radians(10/2); // range to randomly sample readings within
+            for(Line l : lines){
-        int indexRange = (int) (degreeRange / raysPerDegree);
+                if(l.getDistance(v) < maxRange){
-        int numSampleReadings = 10; // number of readings to randomly sample
+                    l.refitLine(v);
-        // constrain numSampleReadings so that it cant be higher than possible
+                    pointsToRemove.add(v);
-        if(numSampleReadings >= 2 * indexRange){
-            numSampleReadings = 2 * indexRange;
                 }
-        int consensus = 6; // the number of points that need to lie near a line for it to be considered valid.
+            }
+        }
+
+        for(Vector v : pointsToRemove){
+            sample.remove(v);
+        }
+    }
+
+    /**
+     * @param view a laser scan view
+     */
+    public void RANSAC(View view){
+        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
+
+        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++) {
             // if there aren't enough points left in the set to form a consensus, we're done.
-            if(newPoints.size() < consensus){
+            if(this.unassociatedPoints.size() < maxRange){
                 break;
             }
 
+            // get a random angle between -PI and PI
+            float randomAngle = (float) (2*PI*(random()) - 0.5);
+
             // get a random sub sample of newPoints within the index range of a given size
-            List<Vector> randomSample = this.randomSample(newPoints, indexRange, numSampleReadings);
+            List<Vector> 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(newPoints, randomSample, maxRange, consensus);
+                extractFeature(randomSample, maxRange, consensus);
-
             }
 
         }
 
     }
+
+    public void drawFeatures(PApplet proc){
+        for(Line line : lines){
+            line.draw(proc);
+        }
+    }
+
+}

src/ScanGraph/ScanEdge.java

@@ -0,0 +1,57 @@
+package ScanGraph;
+
+import Graph.Edge;
+import Vector.Line;
+import Vector.LineInterface;
+import Vector.Vector;
+import processing.core.PApplet;
+
+import static java.lang.Math.PI;
+
+public class ScanEdge extends Edge implements LineInterface {
+
+    protected ScanPoint vStart;
+    protected ScanPoint vEnd;
+    protected Line line;
+    public ScanEdge(ScanPoint vStart, ScanPoint vEnd){
+        super(vStart, vEnd);
+        this.vStart = vStart;
+        this.vEnd = vEnd;
+
+        this.line = new Line(vStart.getPos(), vEnd.getPos());
+    }
+
+    public Vector getDirection(){
+        return line.getDirection();
+    }
+
+    public Vector getPosition(){
+        return line.getPosition();
+    }
+
+    public float getLength(){
+        return line.getLength();
+    }
+
+    public float getAngle(){
+        return line.getAngle();
+    }
+
+    public Vector endPoint(){
+        return line.endPoint();
+    }
+
+    public float getDistance(Vector point){
+        return line.getDistance(point);
+    }
+
+    public void draw(PApplet proc){
+        line.draw(proc);
+        Vector leftFlange = line.getDirection().rotate2D((float)(-3*PI/4)).normalize().mul(20);
+        Vector rightFlange = line.getDirection().rotate2D((float) (3*PI/4)).normalize().mul(20);
+        Line l1 = new Line(line.endPoint(), line.endPoint().add(leftFlange));
+        Line l2 = new Line(line.endPoint(), line.endPoint().add(rightFlange));
+        l1.draw(proc);
+        l2.draw(proc);
+    }
+}

src/ScanGraph/ScanGraph.java

@@ -0,0 +1,46 @@
+package ScanGraph;
+
+import Graph.Graph;
+import Graph.Vertex;
+import Vector.Vector;
+import org.ejml.simple.SimpleMatrix;
+import org.ejml.simple.SimpleSVD;
+
+import java.util.ArrayList;
+public class ScanGraph extends Graph {
+
+    ScanPoint lastPoint;
+
+    public ScanGraph(ScanPoint startingPoint) {
+        super();
+        this.lastPoint = startingPoint;
+    }
+
+    public void addEdge(ScanPoint vEnd) {
+        addVertex(vEnd);
+        ScanEdge edge = new ScanEdge(this.lastPoint, vEnd);
+        adjList.get((Vertex) this.lastPoint).add(edge);
+
+        this.lastPoint = vEnd;
+    }
+
+    /**
+     * @param newScan the scan to match
+     * @return null if no match can be found, or an existing scan the matches the new scan.
+     * @brief Get a new scan in and try to match it with all other scans in the graph
+     */
+    private ScanPoint getAssociatedScan(ScanPoint newScan) {
+        ScanMatcher matcher = new ScanMatcher();
+        ScanPoint matchedScan = null;
+        // go through all of our available scans and try to match the new scan with the old scans. If no match can be found return null
+        for (Vertex v : adjList.keySet()) {
+            ScanPoint referenceScan = (ScanPoint) v;
+            matchedScan = matcher.iterativeScanMatch(referenceScan, newScan, 0.1F, 10);
+
+            if(matchedScan != null){
+                break;
+            }
+        }
+        return matchedScan;
+    }
+}

src/ScanGraph/ScanMatcher.java

@@ -0,0 +1,301 @@
+package ScanGraph;
+
+import Vector.Vector;
+import org.ejml.simple.SimpleMatrix;
+import org.ejml.simple.SimpleSVD;
+
+import java.util.ArrayList;
+
+import static java.lang.Math.abs;
+
+/**
+ * @brief A class that can match two point scans together
+ */
+public class ScanMatcher{
+    // A 2x2 matrix describing a rotation to apply to the new scan
+    public SimpleMatrix rotationMatrix = null;
+
+    // A 2x1 matrix describing a translation to apply to the new scan
+    public SimpleMatrix translationVector = null;
+
+    public ScanMatcher(){
+    }
+
+    /**
+     * @brief iteratively calculate new rotation and transpose matrices to determien if the two scans match
+     * @param referenceScan the scan to be referenced
+     * @param newScan the scan that will be rotated and moved until it matches the reference scan
+     * @param iterations The number of iterations that the scan matcher will attempt
+     * @param errorThreshold The error threshold that the match will have to meet before considering it a valid match
+     */
+    public ScanPoint iterativeScanMatch(ScanPoint referenceScan, ScanPoint newScan, float errorThreshold, int iterations){
+        // make a copy of the new scan so we don't modify the original
+        ScanPoint scanBeingMatched = new ScanPoint(newScan);
+
+        // calculate the rotation and translation matrices between the two scans
+        this.calculateRotationAndTranslationMatrices(referenceScan, scanBeingMatched);
+
+        SimpleMatrix cumulativeRotationMatrix = new SimpleMatrix(this.rotationMatrix);
+        SimpleMatrix cumulativeTranslationVector = new SimpleMatrix(this.translationVector);
+
+        // iterate through the scan matching algorithm
+        for (int i = 0; i < iterations; i++) {
+            // calculate the rotation and translation matrices between the two scans
+            this.calculateRotationAndTranslationMatrices(referenceScan, scanBeingMatched);
+            // apply the rotation and translation matrices to the new scan
+            scanBeingMatched = this.applyRotationAndTranslationMatrices(scanBeingMatched);
+            // calculate the error between the new scan and the reference scan
+            float error = this.getError(referenceScan, scanBeingMatched);
+            // if the error is less than the error threshold, then we have a valid match
+            if(error < errorThreshold){
+                this.rotationMatrix = cumulativeRotationMatrix;
+                this.translationVector = cumulativeTranslationVector;
+                return scanBeingMatched;
+            }
+            // otherwise, we need to keep iterating
+            // add the rotation and translation matrices to the cumulative rotation and translation matrices
+            cumulativeRotationMatrix = cumulativeRotationMatrix.mult(this.rotationMatrix);
+            cumulativeTranslationVector = cumulativeTranslationVector.plus(this.translationVector);
+        }
+
+        // if we get to this point, then we have not found a valid match
+        return null;
+    }
+
+    /**
+     * @brief Compute the cross covariance matrix between the new scan and the reference scan
+     * @return a 2x2 matrix containing the cross covariance matrix
+     */
+    private SimpleMatrix crossCovarianceMatrix(ScanPoint referenceScan, ScanPoint newScan, CorrespondenceMatrix correspondenceMatrix){
+
+        Vector referenceScanAveragePosition = correspondenceMatrix.getAverageOldPosition();
+        Vector newScanAveragePosition = correspondenceMatrix.getAverageNewPosition();
+
+        // compute the cross covariance matrix which is given by the formula:
+        // covariance = the sum from 1 to N of (p_i) * (q_i)^T
+        // where p_i is the ith point in the new scan and q_i is the ith point in the reference scan and N is the number of points in the scan
+        // the cross covariance matrix is a 2x2 matrix
+        float[][] crossCovarianceMatrix = new float[2][2];
+
+        for (int i = 0; i < correspondenceMatrix.getOldPointIndices().size(); i++) {
+            int oldIndex = correspondenceMatrix.getOldPointIndices().get(i);
+            int newIndex = correspondenceMatrix.getNewPointIndices().get(i);
+            Vector oldPoint = referenceScan.getPoints().get(oldIndex);
+            Vector newPoint = newScan.getPoints().get(newIndex);
+            if (oldPoint != null && newPoint != null) {
+                Vector oldPointOffset = oldPoint.sub(referenceScanAveragePosition);
+                Vector newPointOffset = newPoint.sub(newScanAveragePosition);
+                crossCovarianceMatrix[0][0] += oldPointOffset.x * newPointOffset.x;
+                crossCovarianceMatrix[0][1] += oldPointOffset.x * newPointOffset.y;
+                crossCovarianceMatrix[1][0] += oldPointOffset.y * newPointOffset.x;
+                crossCovarianceMatrix[1][1] += oldPointOffset.y * newPointOffset.y;
+            }
+        }
+        return new SimpleMatrix(crossCovarianceMatrix);
+    }
+
+    /**
+     * @brief Compute the rotation and translation matrices between the new scan and the reference scan. Then cache them as private variables.
+     * The rotation matrix is a 2x2 matrix and the translation vector is a 2x1 matrix
+     */
+    public void calculateRotationAndTranslationMatrices(ScanPoint referenceScan, ScanPoint newScan){
+        CorrespondenceMatrix correspondenceMatrix = new CorrespondenceMatrix(newScan, referenceScan);
+
+        // compute the rotation matrix which is given by the formula:
+        // R = V * U^T
+        // where V and U are the singular value decomposition of the cross covariance matrix
+        // the rotation matrix is a 2x2 matrix
+        SimpleMatrix crossCovarianceMatrixSimple = crossCovarianceMatrix(referenceScan, newScan, correspondenceMatrix);
+        SimpleSVD<SimpleMatrix> svd = crossCovarianceMatrixSimple.svd();
+        this.rotationMatrix = svd.getU().mult(svd.getV().transpose());
+        // calculate what the angle of the rotation matrix is
+        float angle = (float) Math.atan2(this.rotationMatrix.get(1, 0), this.rotationMatrix.get(0, 0));
+        // scale the angle by a small amount to make the rotation matrix more accurate
+        angle*= 1.75F;
+        this.rotationMatrix = new SimpleMatrix(new double[][]{{Math.cos(angle), -Math.sin(angle)}, {Math.sin(angle), Math.cos(angle)}});
+
+        // percentage of the local position to use in the translation calculation
+        double weightedAverage = 0.9;
+
+        SimpleMatrix localNewScanAveragePosition = new SimpleMatrix(correspondenceMatrix.getAverageNewPosition().toArray());//this.averageScanPosition(newScan);
+        SimpleMatrix globalNewScanAveragePosition = new SimpleMatrix(this.averageScanPosition(newScan));
+        SimpleMatrix weightedAverageNewScanPostion = localNewScanAveragePosition.scale(weightedAverage).plus(globalNewScanAveragePosition.scale(1-weightedAverage));
+
+        SimpleMatrix localReferenceScanAveragePosition = new SimpleMatrix(correspondenceMatrix.getAverageOldPosition().toArray()); //this.averageScanPosition(referenceScan);
+        SimpleMatrix globalReferenceScanAveragePosition = new SimpleMatrix(this.averageScanPosition(referenceScan));
+        SimpleMatrix weightedAverageReferenceScanPostion = localReferenceScanAveragePosition.scale(weightedAverage).plus(globalReferenceScanAveragePosition.scale(1-weightedAverage));
+
+
+        this.translationVector = weightedAverageReferenceScanPostion.minus(rotationMatrix.mult(weightedAverageNewScanPostion));
+    }
+
+    public SimpleMatrix getRotationMatrix(){
+        return this.rotationMatrix;
+    }
+
+    public SimpleMatrix getTranslationVector(){
+        return this.translationVector;
+    }
+
+    public ScanPoint applyRotationAndTranslationMatrices(ScanPoint newScan){
+        // copy the new scan so we don't modify the original
+        ScanPoint tempScan = new ScanPoint(newScan);
+        // apply the rotation matrix and translation vector to the new scan
+        for (int i = 0; i < tempScan.getPoints().size(); i++) {
+            Vector point = tempScan.getPoints().get(i);
+            if (point != null) {
+                SimpleMatrix pointMatrix = new SimpleMatrix(point.toArray());
+                SimpleMatrix newPointMatrix = rotationMatrix.mult(pointMatrix).plus(translationVector);
+                tempScan.getPoints().set(i, new Vector((float) newPointMatrix.get(0), (float) newPointMatrix.get(1)));
+            }
+        }
+        return tempScan;
+    }
+
+    /**
+     * @brief Compute the average position of the scan
+     * @param scan the scan to compute the average position of
+     * @return a 2x1 matrix containing the x,y coordinates of the average position of the scan
+     */
+    private SimpleMatrix averageScanPosition(ScanPoint scan){
+        Vector averagePosition = new Vector(0, 0);
+        int invalidPoints = 0;
+        for (Vector point : scan.getPoints()) {
+            if (point != null) {
+                averagePosition = averagePosition.add(point);
+            }
+            else{
+                invalidPoints++;
+            }
+        }
+        return new SimpleMatrix(averagePosition.div(scan.getPoints().size() - invalidPoints).toArray());
+    }
+
+    public float getError(ScanPoint referenceScan, ScanPoint newScan){
+        // calculate the error between the new scan and the reference scan
+        // q is reference scan and p is new scan
+        // error is given as abs(Q_mean - R * P_mean)
+        // where Q_mean is the average position of the reference scan
+        // P_mean is the average position of the new scan
+        // R is the rotation matrix
+
+        SimpleMatrix newScanAveragePosition = averageScanPosition(newScan);
+        SimpleMatrix referenceScanAveragePosition = averageScanPosition(referenceScan);
+        SimpleMatrix error = referenceScanAveragePosition.minus(rotationMatrix.mult(newScanAveragePosition));
+        return (float) abs(error.elementSum());
+    }
+}
+
+/**
+ * @brief A class to hold the correspondence matrix between two scans
+ * The correspondence matrix is a 3xN matrix where N is the number of valid points in the scan.
+ * This calculates the closest point in the old scan for each point in the new scan and gets rid of redundant closest points.
+ */
+class CorrespondenceMatrix{
+    private ArrayList<Integer> oldPointIndices = new ArrayList<>();
+    private ArrayList<Integer> newPointIndices = new ArrayList<>();
+    private ArrayList<Float> distances = new ArrayList<>();
+
+    private Vector averageOldPosition = new Vector(0, 0);
+    private Vector averageNewPosition = new Vector(0, 0);
+
+    CorrespondenceMatrix(ScanPoint newScan, ScanPoint oldScan){
+        this.calculateCorrespondenceMatrix(newScan, oldScan);
+        this.calculateAveragePositions(newScan, oldScan);
+    }
+
+    public ArrayList<Integer> getOldPointIndices(){
+        return this.oldPointIndices;
+    }
+
+    public ArrayList<Integer> getNewPointIndices(){
+        return this.newPointIndices;
+    }
+
+    public ArrayList<Float> getDistances(){
+        return this.distances;
+    }
+
+    public Vector getAverageOldPosition(){
+        return this.averageOldPosition;
+    }
+
+    public Vector getAverageNewPosition(){
+        return this.averageNewPosition;
+    }
+
+    private void calculateAveragePositions(ScanPoint newScan, ScanPoint oldScan){
+        int invalidPoints = 0;
+        for (int i = 0; i < this.oldPointIndices.size(); i++){
+            int oldIndex = this.oldPointIndices.get(i);
+            int newIndex = this.newPointIndices.get(i);
+            Vector oldPoint = oldScan.getPoints().get(oldIndex);
+            Vector newPoint = newScan.getPoints().get(newIndex);
+            if (oldPoint != null && newPoint != null) {
+                this.averageOldPosition = this.averageOldPosition.add(oldPoint);
+                this.averageNewPosition = this.averageNewPosition.add(newPoint);
+            }
+            else{
+                invalidPoints++;
+            }
+        }
+        this.averageOldPosition = this.averageOldPosition.div(this.oldPointIndices.size() - invalidPoints);
+        this.averageNewPosition = this.averageNewPosition.div(this.newPointIndices.size() - invalidPoints);
+    }
+
+    /**
+     * @brief Calculate the correspondence matrix between two scans
+     * @param newScan the new scan
+     * @param referenceScan the reference scan
+     */
+    private void calculateCorrespondenceMatrix(ScanPoint newScan, ScanPoint referenceScan) {
+        for (int newPointIndex = 0; newPointIndex < newScan.getPoints().size(); newPointIndex++) {
+            Vector newPoint = newScan.getPoints().get(newPointIndex);
+
+            // Skip null points in the new scan
+            if (newPoint == null) {
+                continue;
+            }
+
+            float closestDistance = Float.MAX_VALUE;
+            int closestIndex = -1;
+
+            for (int oldPointIndex = 0; oldPointIndex < referenceScan.getPoints().size(); oldPointIndex++) {
+                Vector oldPoint = referenceScan.getPoints().get(oldPointIndex);
+
+                // Skip null points in the old scan
+                if (oldPoint == null) {
+                    continue;
+                }
+
+                float distance = newPoint.sub(oldPoint).mag();
+
+                if (distance < closestDistance) {
+                    closestDistance = distance;
+                    closestIndex = oldPointIndex;
+                }
+            }
+
+            // if we find a closest point...
+            if (closestIndex != -1) {
+                // check if the oldPointIndex is already in the list of oldPointIndices
+                if(this.oldPointIndices.contains(closestIndex)){
+                    int index = this.oldPointIndices.indexOf(closestIndex);
+                    // if the index is already in our list, then we need to check if the new point is closer than the old point
+                    if(this.distances.get(index) > closestDistance){
+                        // if the new point is closer than the old point, then we need to replace the old point with the new point
+                        this.oldPointIndices.set(index, closestIndex);
+                        this.newPointIndices.set(index, newPointIndex);
+                        this.distances.set(index, closestDistance);
+                    }
+                }
+                // if the index is not in our list, then we need to add it
+                else{
+                    this.oldPointIndices.add(closestIndex);
+                    this.newPointIndices.add(newPointIndex);
+                    this.distances.add(closestDistance);
+                }
+            }
+        }
+    }
+}

src/ScanGraph/ScanPoint.java

@@ -0,0 +1,47 @@
+package ScanGraph;
+
+import Graph.Vertex;
+import Vector.Vector;
+
+import java.util.ArrayList;
+
+public class ScanPoint extends Vertex{
+
+    private Vector position;
+    private float orientation;
+    private ArrayList<Vector> scan;
+
+    public ScanPoint(Vector scanPosition, float orientation, ArrayList<Vector> scan) {
+        super();
+        this.position = scanPosition;
+        this.orientation = orientation;
+        this.scan = scan;
+    }
+
+    /**
+     * @brief Copy constructor
+     * @param other The scan point to copy
+     */
+    public ScanPoint(ScanPoint other){
+        super();
+        this.position = new Vector(other.getPos().x, other.getPos().y);
+        this.orientation = other.getOrientation();
+        this.scan = new ArrayList<>(other.getPoints());
+    }
+
+    /**
+     * @return a two eleement float array containing the x and y coordinates of the vertex respectively.
+     */
+    public Vector getPos(){
+        return position;
+    }
+
+    public float getOrientation(){
+        return this.orientation;
+    }
+
+    public ArrayList<Vector> getPoints(){
+        return this.scan;
+    }
+
+}

src/ShortTermMem.java

@@ -0,0 +1,59 @@
+import java.util.ArrayList;
+import Vector.Vector;
+public class ShortTermMem {
+    ArrayList<ArrayList<Vector>> scans = new ArrayList<>();
+    ArrayList<Vector> scanPositions = new ArrayList<>();
+
+    ArrayList<Long> scanTimes = new ArrayList<>();
+    private int size = 0;
+
+    public void addScan(Vector scanPosition, ArrayList<Vector> scan){
+        size += scan.size();
+        scans.add(scan);
+        scanPositions.add(scanPosition);
+        scanTimes.add(System.currentTimeMillis());
+        purgeScans();
+    }
+
+    public ArrayList<Vector> getPoints(){
+        ArrayList<Vector> points = new ArrayList<>();
+        for(ArrayList<Vector> pointList : this.scans){
+            points.addAll(pointList);
+        }
+        return points;
+    }
+
+    public void remove(Vector point){
+        for(ArrayList<Vector> 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;
+    }
+}

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<Vector> 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<Vector> points){
+    public void bestFit(List<Vector> fitPoints){
+        this.points.addAll(fitPoints);
         // get the mean of all the points
         Vector mean = new Vector();
         for(Vector point : points){
@@ -37,26 +48,28 @@ public class Line implements LineInterface{
 
         // this section calculates the direction vector of the line of best fit
         Vector direction = new Vector();
-        float length = 1;
+        float length = 0;
         // 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.
+            // get the average distance avery point is away from the mean.
             float dist = abs(point.sub(mean).mag());
-            if(dist > length){
+            length += dist;
-                length = 2*dist;
-            }
 
         }
+        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);
         }
         else{
             this.direction = new Vector(1, direction.y/direction.x);
         }
+        // scale the direction vector to be the correct length of the line.
         this.direction = this.direction.normalize().mul(length);
+
         this.position = mean.sub(this.direction.div(2));
     }
 
@@ -84,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){

src/Vector/Vector.java

@@ -1,5 +1,8 @@
 package Vector;
 
+import org.ejml.simple.SimpleMatrix;
+import processing.core.PApplet;
+
 import static java.lang.Math.*;
 import static processing.core.PApplet.cos;
 import static processing.core.PApplet.sin;
@@ -21,6 +24,18 @@ public class Vector {
         this.z = z;
     }
 
+    public Vector(SimpleMatrix matrix){
+        // initialize x,y if matrix is 2x1 and x,y,z if matrix is 3x1
+        if(matrix.getNumRows() == 2){
+            this.x = (float)matrix.get(0,0);
+            this.y = (float)matrix.get(1,0);
+        }else if(matrix.getNumRows() == 3){
+            this.x = (float)matrix.get(0,0);
+            this.y = (float)matrix.get(1,0);
+            this.z = (float)matrix.get(2,0);
+        }
+    }
+    
     public Vector add(Vector other){
         return new Vector(this.x + other.x, this.y + other.y, this.z + other.z);
     }
@@ -81,13 +96,29 @@ public class Vector {
         return angle;
     }
 
+    /**
+     * @return The angle of the vector in radians
+     */
     public float angle(){
         return (float) atan2(y, x);
     }
 
+    /**
+     * @brief Rotate a 2D vector by a given angle
+     * @param angle The angle to rotate the vector by in radians
+     * @return The rotated vector
+     */
     public Vector rotate2D(float angle){
         float distance = mag();
         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);
+    }
+
+    public float[] toArray() {
+        return new float[]{x, y};
+    }
 }

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);
     }
@@ -22,36 +29,48 @@ 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) (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.drawRay(proc);
+                ray.getPoint().draw(proc);
             }
         }
     }
 
-    //changes the position of the view
+    /**
-    public void setPos(Vector newPose) {
+     * @brief Sets the position of the view
-        pose = newPose;
+     * @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) {
+     * @brief Sets the angle of the view
-        this.angle = angle;
+     * @param newAngle The new angle of the view
-        this.setRayNum(rays.size(), this.FOV, angle);
+     */
+    public void setAngle(float newAngle) {
+        this.angle = newAngle;
+        for(Ray ray : rays){
+            float angleOffset = ray.getAngle() - this.angle;
+            ray.setAngle(this.angle+angleOffset);
+        }
     }
 
     //changes the field of view of the view
@@ -60,32 +79,63 @@ 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) {
             if(ray.hasCollided()){
-                points.add(ray.getPoint());
+                Vector point = ray.getPoint();
+                // store the angle information for that point in the z coordinate
+                point.z = ray.getAngle();
+                points.add(point);
             }
         }
-        return points;
+        return new ScanPoint(this.position,this.angle, points);
+    }
+
+    /**
+     * @return A list of the angles where a collision was detected
+     */
+    public ArrayList<Float> getAngles(){
+        ArrayList<Float> angles = new ArrayList<>();
+        for (Ray ray : rays) {
+            if (ray.hasCollided()){
+                angles.add(ray.getAngle());
+            }
+        }
+        return angles;
     }
 }
 

tests/MatcherVisualizer.java

@@ -0,0 +1,123 @@
+import ScanGraph.ScanMatcher;
+import ScanGraph.ScanPoint;
+import Vector.Vector;
+import processing.core.PApplet;
+
+import java.util.ArrayList;
+
+public class MatcherVisualizer extends PApplet{
+
+    public static PApplet processing;
+    ScanPoint referenceScan;
+    ScanPoint scanToMatch;
+    ScanPoint scanBeingMatched;
+
+    public static void main(String[] args) {
+        PApplet.main("MatcherVisualizer");
+    }
+
+    public void settings(){
+        processing = this;
+        size(1000, 1000);
+        float connectingAngle = (float) (3 * Math.PI / 9);
+
+        // generate two scans rotated by 45 degrees and append them together
+        Vector descriptor = new Vector(200, 200);
+        Vector position = new Vector(500, 500);
+        ScanPoint scan1 = generateScanPoint(position, descriptor, 12);
+        ScanPoint scan2 = generateScanPoint(position, descriptor.rotate2D(connectingAngle), 12);
+        Vector p1 = scan1.getPoints().get(11);
+        Vector p2 = scan2.getPoints().get(11);
+        ScanPoint scan3 = generateScanPoint(p1, p2.sub(p1), 12);
+        this.referenceScan = appendScanPoints(scan1, scan2);
+        this.referenceScan = appendScanPoints(this.referenceScan, scan3);
+
+        // generate two scans offset by some amount and rotated by 55 degrees and append them together
+        Vector rotated = descriptor.rotate2D((float) Math.PI);
+        Vector offset = new Vector(100, 150);
+        ScanPoint scan4 = generateScanPoint(position.add(offset), rotated, 12);
+        ScanPoint scan5 = generateScanPoint(position.add(offset), rotated.rotate2D(connectingAngle), 12);
+        p1 = scan4.getPoints().get(11);
+        p2 = scan5.getPoints().get(11);
+        ScanPoint scan6 = generateScanPoint(p1, p2.sub(p1), 12);
+        this.scanToMatch = appendScanPoints(scan4, scan5);
+        this.scanToMatch = appendScanPoints(this.scanToMatch, scan6);
+        this.scanBeingMatched = new ScanPoint(this.scanToMatch);
+    }
+    public void draw(){
+        iterativeScanMatch();
+//        background(0);
+    }
+
+    /**
+     * @brief Generate a scan point from a scan description
+     * @param offset The offset of the scan point from the origin
+     * @param scanDescription A vector which describes the length of the line and direction of the line
+     * @return A scan point with the given offset and scan description
+     */
+    public static ScanPoint generateScanPoint(Vector offset, Vector scanDescription, int numPoints){
+        // generate a scan point with the given offset and scan description
+        ArrayList<Vector> scan = new ArrayList<>();
+
+        // divide the scan description by the number of points to allow us to scale it back up in the loop
+        Vector directionVector = scanDescription.div(numPoints-1);
+
+        for (int i = 0; i < numPoints; i++) {
+            scan.add(offset.add(directionVector.mul(i)));
+        }
+
+        return new ScanPoint(new Vector(0, 0), 0, scan);
+    }
+
+    /**
+     * @brief Append two scan points together
+     * @param scan1 The first scan point to append
+     * @param scan2 The second scan point to append
+     * @return A scan point that is the combination of the two scan points
+     */
+    public static ScanPoint appendScanPoints(ScanPoint scan1, ScanPoint scan2){
+        ArrayList<Vector> points = new ArrayList<>();
+        points.addAll(scan1.getPoints());
+        points.addAll(scan2.getPoints());
+        return new ScanPoint(new Vector(0, 0), 0, points);
+    }
+
+    /**
+     * @brief Draw a scan point to the screen
+     * @param scan The scan point to draw
+     * @param color The color to draw the scan point
+     */
+    public void drawScan(ScanPoint scan, int[] color) {
+        processing.stroke(color[0], color[1], color[2]);
+        processing.fill(color[0], color[1], color[2]);
+        ArrayList<Vector> points = scan.getPoints();
+        for (int i = 0; i < points.size() - 1; i++) {
+            Vector point = points.get(i);
+            processing.ellipse(point.x, point.y, 5, 5);
+        }
+    }
+
+    public void iterativeScanMatch() {
+        background(0);
+        int[] red = {255, 0, 0};
+        int[] green = {0, 255, 0};
+        int[] blue = {0, 0, 255};
+
+
+
+        drawScan(this.referenceScan, red);
+        drawScan(this.scanToMatch, green);
+
+        // do a single scan match and calculate the error
+        ScanMatcher matcher = new ScanMatcher();
+        matcher.calculateRotationAndTranslationMatrices(this.referenceScan, this.scanBeingMatched);
+        this.scanBeingMatched = matcher.applyRotationAndTranslationMatrices(this.scanBeingMatched);
+        float error = matcher.getError(this.referenceScan, this.scanBeingMatched);
+        drawScan(this.scanBeingMatched, blue);
+
+        // do an iterative scan match and calculate the error
+//        ScanPoint matchedScan = matcher.iterativeScanMatch(scan1, scan2, 0.01f, 10);
+
+//        float iterativeScanMatchError = matcher.getError(scan1, matchedScan);
+    }
+}

tests/ScanMatcherTest.java

@@ -0,0 +1,129 @@
+import ScanGraph.ScanMatcher;
+import ScanGraph.ScanPoint;
+import org.junit.Before;
+import org.junit.jupiter.api.BeforeEach;
+import processing.core.PApplet;
+
+import org.junit.jupiter.api.Test;
+import Vector.Vector;
+
+import java.util.ArrayList;
+
+import static org.junit.jupiter.api.Assertions.*;
+import static processing.core.PApplet.main;
+
+class ScanMatcherTest{
+    /**
+     * @brief Generate a scan point from a scan description
+     * @param offset The offset of the scan point from the origin
+     * @param scanDescription A vector which describes the length of the line and direction of the line
+     * @return A scan point with the given offset and scan description
+     */
+    public ScanPoint generateScanPoint(Vector offset, Vector scanDescription, int numPoints){
+        // generate a scan point with the given offset and scan description
+        ArrayList<Vector> scan = new ArrayList<>();
+
+        // divide the scan description by the number of points to allow us to scale it back up in the loop
+        Vector directionVector = scanDescription.div(numPoints-1);
+
+        for (int i = 0; i < numPoints; i++) {
+            scan.add(offset.add(directionVector.mul(i)));
+        }
+
+        return new ScanPoint(new Vector(0, 0), 0, scan);
+    }
+
+    /**
+     * @brief Append two scan points together
+     * @param scan1 The first scan point to append
+     * @param scan2 The second scan point to append
+     * @return A scan point that is the combination of the two scan points
+     */
+    public ScanPoint appendScanPoints(ScanPoint scan1, ScanPoint scan2){
+        ArrayList<Vector> points = new ArrayList<>();
+        points.addAll(scan1.getPoints());
+        points.addAll(scan2.getPoints());
+        return new ScanPoint(new Vector(0, 0), 0, points);
+    }
+
+    @Test
+    public void applyRotationAndTranslationMatrices() {
+        // generate one scan that is level and another that is rotated 45 degrees.
+        Vector scanDescription = new Vector(10, 0);
+        ScanPoint referenceScan = generateScanPoint(new Vector(0, 0), scanDescription, 10);
+        ScanPoint newScan = generateScanPoint(new Vector(0, 0), scanDescription.rotate2D((float) Math.PI / 4), 10);
+
+        // calculate the rotation and translation matrices between the two scans
+        ScanMatcher matcher = new ScanMatcher();
+        matcher.calculateRotationAndTranslationMatrices(referenceScan, newScan);
+        // apply the rotation and translation matrices to the new scan
+        ScanPoint newScanWithRotationAndTranslation = matcher.applyRotationAndTranslationMatrices(newScan);
+
+        // Get the first and last points of the new scan with rotation and translation and calculate the angle between them
+        ArrayList<Vector> points = newScanWithRotationAndTranslation.getPoints();
+        Vector firstPoint = points.get(0);
+        Vector lastPoint = points.get(points.size() - 1);
+        Vector rotatedDirection = lastPoint.sub(firstPoint);
+        float angle = scanDescription.angleDiff(rotatedDirection);
+
+        // The angle between the first and last points should be zero
+        assertEquals(0, angle);
+    }
+
+    @Test
+    public void getError() {
+        // generate two scans that are the same. The error should be zero.
+        ScanPoint scan1 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
+        ScanPoint scan2 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
+        ScanMatcher matcher = new ScanMatcher();
+        matcher.calculateRotationAndTranslationMatrices(scan1, scan2);
+        assertEquals(0, matcher.getError(scan1, scan2));
+
+        // generate two scans that are the same but one is offset by 10 in the y direction. The error should be 10.
+        scan1 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
+        scan2 = generateScanPoint(new Vector(0, 10), new Vector(10, 10), 12);
+        matcher.calculateRotationAndTranslationMatrices(scan1, scan2);
+        assertEquals(10, matcher.getError(scan1, scan2));
+
+        // generate two scans that are the same but one is rotated by 45 degrees. The error should be near zero.
+        scan1 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
+        scan2 = generateScanPoint(new Vector(0, 0), new Vector(10, 10).rotate2D((float) Math.PI / 4), 12);
+        matcher.calculateRotationAndTranslationMatrices(scan1, scan2);
+        assertEquals(0, matcher.getError(scan1, scan2), 0.1);
+    }
+
+    @Test
+    public void iterativeScanMatch() {
+        float bendAngle = (float) (5 * Math.PI / 9);
+        // generate two scans rotated by 45 degrees and append them together
+        ScanPoint scan1 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
+        ScanPoint scan2 = generateScanPoint(new Vector(0, 0), new Vector(10, 10).rotate2D(bendAngle), 12);
+        ScanPoint scan3 = appendScanPoints(scan1, scan2);
+
+
+        // generate two scans offset by some amount and rotated by 55 degrees and append them together
+        Vector rotated = (new Vector(10, 10)).rotate2D((float) Math.PI);
+        ScanPoint scan4 = generateScanPoint(new Vector(10, 10), rotated, 12);
+        ScanPoint scan5 = generateScanPoint(new Vector(10, 10), rotated.rotate2D(bendAngle), 12);
+        ScanPoint scan6 = appendScanPoints(scan4, scan5);
+
+
+        // do a single scan match and calculate the error
+        ScanMatcher matcher = new ScanMatcher();
+        matcher.calculateRotationAndTranslationMatrices(scan3, scan6);
+        ScanPoint oneCalcMatch = matcher.applyRotationAndTranslationMatrices(scan6);
+        float singleScanMatchError = matcher.getError(scan3, oneCalcMatch);
+
+
+        // do an iterative scan match and calculate the error
+        ScanPoint matchedScan = matcher.iterativeScanMatch(scan1, scan2, 0.0001f, 10);
+
+        // if it's null something has gone wrong with the algorithm because these scans can easily be matched.
+        assertNotNull(matchedScan);
+
+        float iterativeScanMatchError = matcher.getError(scan1, matchedScan);
+
+        // the iterative scan match should have a lower error than the single scan match
+        assertTrue(iterativeScanMatchError < singleScanMatchError);
+    }
+}