Merge pull request #11 from Cynopolis/create-unit-tests-for-the-scan-matching-algorithm

Create unit tests for the scan matching algorithm

.gitignore

@@ -27,3 +27,5 @@ bin/
 
 ### Mac OS ###
 .DS_Store
+
+map.txt

SLAM-Sim.iml

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

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/Processing.java

@@ -79,15 +79,10 @@ 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();
             }

src/ScanGraph/ScanGraph.java

@@ -10,6 +10,7 @@ import java.util.ArrayList;
 public class ScanGraph extends Graph {
 
     ScanPoint lastPoint;
+
     public ScanGraph(ScanPoint startingPoint) {
         super();
         this.lastPoint = startingPoint;
@@ -24,236 +25,22 @@ public class ScanGraph extends Graph{
     }
 
     /**
-     * @brief Get a new scan in and try to match it with all other scans in the graph
      * @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;
-            for(int i = 0; i < 5; i++) {
+            matchedScan = matcher.iterativeScanMatch(referenceScan, newScan, 0.1F, 10);
-                // calculate the rotation and translation matrices between the new scan and the reference scan
-                matcher.calculateRotationAndTranslationMatrices(referenceScan, newScan);
 
-                // update the new scan with the rotation matrix and translation vector
+            if(matchedScan != null){
-                newScan = matcher.applyRotationAndTranslationMatrices(newScan);
+                break;
-
-                // calculate the error between the new scan and the reference scan
-                float error = matcher.getError(referenceScan, newScan);
-
-                // if the error is less than some threshold, then we have found a match
-                if (error < 0.1) {
-                    return referenceScan;
             }
         }
-        }
+        return matchedScan;
-        return null;
-    }
-
-}
-
-
-/**
- * @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<>();
-
-    CorrespondenceMatrix(ScanPoint newScan, ScanPoint oldScan){
-        this.calculateCorrespondenceMatrix(newScan, oldScan);
-    }
-
-    public ArrayList<Integer> getOldPointIndices(){
-        return this.oldPointIndices;
-    }
-
-    public ArrayList<Integer> getNewPointIndices(){
-        return this.newPointIndices;
-    }
-
-    public ArrayList<Float> getDistances(){
-        return this.distances;
-    }
-
-    /**
-     * @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){
-        // compute the correspondence matrix between the two scans. It is a 3xN matrix where N is the number of points in the scan
-        // Row 1 is the index of the point in the old scan
-        // Row 2 is the index of the point in the new scan
-        // Row 3 is the distance between the two points
-        // if either scan has a null point, then skip that point
-
-        // initialize the correspondence matrix as an array of array lists
-        ArrayList<ArrayList<Float>> correspondenceMatrix = new ArrayList<ArrayList<Float>>();
-        correspondenceMatrix.add(new ArrayList<Float>());
-        correspondenceMatrix.add(new ArrayList<Float>());
-        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.getPoints().size(); newPointIndex++) {
-            Vector newPoint = newScan.getPoints().get(newPointIndex);
-            // if the new point is null, then skip it
-            if (newPoint == null) {
-                continue;
-            }
-            // find the closest point in the old scan
-            float closestDistance = Float.MAX_VALUE;
-            int closestIndex = -1;
-            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;
-                }
-                float distance = newPoint.sub(oldPoint).mag();
-                if (distance < closestDistance) {
-                    closestDistance = distance;
-                    closestIndex = j;
-                }
-            }
-            // only add the new point if it either:
-            // 1. has a closest point index which does not already exist in the correspondence matrix
-            // 2. has a closest point index which already exists in the correspondence matrix, but the distance is smaller than the existing distance
-            // In case 2, we want to replace the old point with the new point
-            if (closestIndex != -1) {
-                if (correspondenceMatrix.get(0).contains((float) closestIndex)) {
-                    int oldIndex = correspondenceMatrix.get(0).indexOf((float) closestIndex);
-                    if (correspondenceMatrix.get(2).get(oldIndex) > closestDistance) {
-                        correspondenceMatrix.get(0).set(oldIndex, (float) closestIndex);
-                        correspondenceMatrix.get(1).set(oldIndex, (float) newPointIndex);
-                        correspondenceMatrix.get(2).set(oldIndex, closestDistance);
-                    }
-                } else {
-                    correspondenceMatrix.get(0).add((float) closestIndex);
-                    correspondenceMatrix.get(1).add((float) newPointIndex);
-                    correspondenceMatrix.get(2).add(closestDistance);
-                }
-            }
-        }
-    }
-}
-
-class ScanMatcher{
-    // A 2x2 matrix describing a rotation to apply to the new scan
-    SimpleMatrix rotationMatrix;
-
-    // A 2x1 matrix describing a translation to apply to the new scan
-    SimpleMatrix translationVector;
-
-    ScanMatcher(){
-    }
-
-    /**
-     * @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());
-    }
-
-    /**
-     * @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){
-        Vector referenceScanAveragePosition = new Vector(averageScanPosition(referenceScan));
-        Vector newScanAveragePosition = new Vector(averageScanPosition(newScan));
-
-        CorrespondenceMatrix correspondenceMatrix = new CorrespondenceMatrix(newScan, referenceScan);
-
-        // 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 oldPointCentered = oldPoint.sub(referenceScanAveragePosition);
-                Vector newPointCentered = newPoint.sub(newScanAveragePosition);
-                crossCovarianceMatrix[0][0] += oldPointCentered.x * newPointCentered.x;
-                crossCovarianceMatrix[0][1] += oldPointCentered.x * newPointCentered.y;
-                crossCovarianceMatrix[1][0] += oldPointCentered.y * newPointCentered.x;
-                crossCovarianceMatrix[1][1] += oldPointCentered.y * newPointCentered.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){
-        // 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);
-        SimpleSVD<SimpleMatrix> svd = crossCovarianceMatrixSimple.svd();
-        this.rotationMatrix = svd.getU().mult(svd.getV().transpose());
-
-        SimpleMatrix newScanAveragePosition = averageScanPosition(newScan);
-        SimpleMatrix referenceScanAveragePosition = averageScanPosition(referenceScan);
-        this.translationVector = referenceScanAveragePosition.minus(rotationMatrix.mult(newScanAveragePosition));
-    }
-
-    public SimpleMatrix getRotationMatrix(){
-        return this.rotationMatrix;
-    }
-
-    public SimpleMatrix getTranslationVector(){
-        return this.translationVector;
-    }
-
-    public ScanPoint applyRotationAndTranslationMatrices(ScanPoint newScan){
-        // apply the rotation matrix and translation vector to the new scan
-        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.getPoints().set(i, new Vector((float) newPointMatrix.get(0), (float) newPointMatrix.get(1)));
-            }
-        }
-        return newScan;
-    }
-
-    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) error.elementSum();
     }
 }

src/ScanGraph/ScanMatcher.java

@@ -0,0 +1,290 @@
+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());
+
+        SimpleMatrix newScanAveragePosition = this.averageScanPosition(newScan);
+        SimpleMatrix referenceScanAveragePosition = this.averageScanPosition(referenceScan);
+        this.translationVector = referenceScanAveragePosition.minus(rotationMatrix.mult(newScanAveragePosition));
+    }
+
+    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);
+//                }
+                    this.oldPointIndices.add(closestIndex);
+                    this.newPointIndices.add(newPointIndex);
+                    this.distances.add(closestDistance);
+
+            }
+        }
+    }
+}

src/ScanGraph/ScanPoint.java

@@ -18,6 +18,17 @@ public class ScanPoint extends Vertex{
         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.
      */

src/Vector/Vector.java

@@ -96,10 +96,18 @@ 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();

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);
+
+        // generate two scans rotated by 45 degrees and append them together
+        Vector descriptor = new Vector(200, 200);
+        ScanPoint scan1 = generateScanPoint(new Vector(500, 500), descriptor, 12);
+        ScanPoint scan2 = generateScanPoint(new Vector(500, 500), descriptor.rotate2D((float) (6 * Math.PI / 9)), 12);
+        this.referenceScan = appendScanPoints(scan1, scan2);
+
+        // generate two scans offset by some amount and rotated by 55 degrees and append them together
+        Vector rotated = descriptor.rotate2D((float) Math.PI);
+        ScanPoint scan4 = generateScanPoint(new Vector(250, 300), rotated, 12);
+        ScanPoint scan5 = generateScanPoint(new Vector(250, 300), rotated.rotate2D((float) (6 * Math.PI / 9)), 12);
+        this.scanToMatch = appendScanPoints(scan4, scan5);
+        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);
+    }
+
+    public void delayMillis(long millis){
+        // get the current time
+        long start = System.currentTimeMillis();
+        long end = start + millis;
+        while(System.currentTimeMillis() < end){
+            // do nothing
+        }
+    }
+
+    /**
+     * @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);
+        delayMillis(10);
+        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 singleScanMatchError = matcher.getError(this.referenceScan, 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);
+    }
+}