Added some unit tests for the ScanMatcher and fixed some broken functionality.

.idea/misc.xml

@@ -1,5 +1,6 @@
+<?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">
+  <component name="ProjectRootManager" version="2" languageLevel="JDK_21" default="true" project-jdk-name="openjdk-20" project-jdk-type="JavaSDK">
     <output url="file://$PROJECT_DIR$/out" />
   </component>
 </project>

SLAM-Sim.iml

@@ -11,7 +11,7 @@
     <orderEntry type="module-library" exported="">
       <library>
         <CLASSES>
-          <root url="jar:///opt/processing/processing-4.2/core/library/core.jar!/" />
+          <root url="jar://$USER_HOME$/Documents/processing-4.3/core/library/core.jar!/" />
         </CLASSES>
         <JAVADOC />
         <SOURCES />

src/ScanGraph/ScanGraph.java

@@ -31,25 +31,16 @@ public class ScanGraph extends 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;
     }
 }

src/ScanGraph/ScanMatcher.java

@@ -13,14 +13,40 @@ import static java.lang.Math.abs;
  */
 class ScanMatcher{
     // A 2x2 matrix describing a rotation to apply to the new scan
-    SimpleMatrix rotationMatrix;
+    public SimpleMatrix rotationMatrix = null;
 
     // A 2x1 matrix describing a translation to apply to the new scan
-    SimpleMatrix translationVector;
+    public SimpleMatrix translationVector = null;
 
     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){
+        for (int i = 0; i < iterations; i++) {
+            // calculate the rotation and translation matrices between the new scan and the reference scan
+            this.calculateRotationAndTranslationMatrices(referenceScan, newScan);
+
+            // update the new scan with the rotation matrix and translation vector
+            newScan = this.applyRotationAndTranslationMatrices(newScan);
+
+            // calculate the error between the new scan and the reference scan
+            float error = this.getError(referenceScan, newScan);
+
+            // if the error is less than some threshold, then we have found a match
+            if (error < errorThreshold) {
+                return referenceScan;
+            }
+        }
+
+        return null;
+    }
     /**
      * @brief Compute the average position of the scan
      * @param scan the scan to compute the average position of
@@ -160,57 +186,38 @@ class CorrespondenceMatrix{
      * @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
+
+            // Skip null points in the new scan
             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);
+            for (int oldPointIndex = 0; oldPointIndex < referenceScan.getPoints().size(); oldPointIndex++) {
-                // if the old point is null, then skip it
+                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 = j;
+                    closestIndex = oldPointIndex;
                 }
             }
-            // only add the new point if it either:
+
-            // 1. has a closest point index which does not already exist in the correspondence matrix
+            // Add the correspondence if a closest point is found
-            // 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)) {
+                this.oldPointIndices.add(closestIndex);
-                    int oldIndex = correspondenceMatrix.get(0).indexOf((float) closestIndex);
+                this.newPointIndices.add(newPointIndex);
-                    if (correspondenceMatrix.get(2).get(oldIndex) > closestDistance) {
+                this.distances.add(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);
-                }
             }
         }
     }

tests/ScanGraph/ScanMatcherTest.java

@@ -1,5 +1,6 @@
 package ScanGraph;
 
+import org.ejml.simple.SimpleMatrix;
 import org.junit.jupiter.api.Test;
 import Vector.Vector;
 
@@ -16,32 +17,29 @@ class ScanMatcherTest {
      * @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
      */
-    ScanPoint generateScanPoint(Vector offset, Vector scanDescription){
+    ScanPoint generateScanPoint(Vector offset, Vector scanDescription, int numPoints){
         // generate a scan point with the given offset and scan description
-        Vector scanPosition = new Vector(0, 0);
         ArrayList<Vector> scan = new ArrayList<>();
 
-        // calculate the total number of points in the scan
+        // divide the scan description by the number of points to allow us to scale it back up in the loop
-        int numPoints = (int) scanDescription.mag();
+        Vector directionVector = scanDescription.div(numPoints-1);
-        // calculate the slope of the line the scan is on
-        float m = scanDescription.y / scanDescription.x;
 
-        // add the points to the scan
         for (int i = 0; i < numPoints; i++) {
-            float x = i;
+            scan.add(offset.add(directionVector.mul(i)));
-            float y = m * x;
-            scan.add(new Vector(x + offset.x, y + offset.y));
         }
 
-        return new ScanPoint(scanPosition, 0, scan);
+        return new ScanPoint(new Vector(0, 0), 0, scan);
     }
 
     @Test
     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);
+        ScanPoint referenceScan = generateScanPoint(new Vector(0, 0), scanDescription, 10);
-        ScanPoint newScan = generateScanPoint(new Vector(0, 0), scanDescription.rotate2D((float) Math.PI / 4));
+        ScanPoint newScan = generateScanPoint(new Vector(0, 0), scanDescription.rotate2D((float) Math.PI / 4), 10);
+
+        Vector test = scanDescription.rotate2D((float) Math.PI / 4);
+        float mag = test.mag();
 
         // calculate the rotation and translation matrices between the two scans
         ScanMatcher matcher = new ScanMatcher();
@@ -53,7 +51,8 @@ class ScanMatcherTest {
         ArrayList<Vector> points = newScanWithRotationAndTranslation.getPoints();
         Vector firstPoint = points.get(0);
         Vector lastPoint = points.get(points.size() - 1);
-        float angle = firstPoint.angleDiff(lastPoint);
+        Vector rotatedDirection = lastPoint.sub(firstPoint);
+        float angle = scanDescription.angleDiff(rotatedDirection);
 
         // The angle between the first and last points should be zero
         assertEquals(0, angle);
@@ -62,10 +61,15 @@ class ScanMatcherTest {
     @Test
     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));
+        ScanPoint scan1 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
-        ScanPoint scan2 = generateScanPoint(new Vector(0, 0), new Vector(10, 10));
+        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));
     }
+
+    @Test
+    void iterativeScanMatch() {
+        // TODO: Write a test for this
+    }
 }