Created a visualizer for the ICP algorithm to figure out what's going wrong.

.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

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

src/ScanGraph/ScanMatcher.java

@@ -11,14 +11,14 @@ import static java.lang.Math.abs;
 /**
  * @brief A class that can match two point scans together
  */
-class ScanMatcher{
+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;
 
-    ScanMatcher(){
+    public ScanMatcher(){
     }
 
     /**
@@ -29,20 +29,37 @@ class ScanMatcher{
      * @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
-            // calculate the rotation and translation matrices between the new scan and the reference scan
+        this.calculateRotationAndTranslationMatrices(referenceScan, newScan);
-            this.calculateRotationAndTranslationMatrices(referenceScan, newScan);
 
+        // copy the new scan so we don't modify the original
+        ScanPoint matchingScan = new ScanPoint(newScan);
+
+        SimpleMatrix lastRotationMatrix;
+        SimpleMatrix lastTranslationVector;
+
+        for (int i = 0; i < iterations; i++) {
             // update the new scan with the rotation matrix and translation vector
-            newScan = this.applyRotationAndTranslationMatrices(newScan);
+            matchingScan = this.applyRotationAndTranslationMatrices(matchingScan);
 
             // calculate the error between the new scan and the reference scan
-            float error = this.getError(referenceScan, newScan);
+            float error = this.getError(referenceScan, matchingScan);
 
             // if the error is less than some threshold, then we have found a match
             if (error < errorThreshold) {
                 return referenceScan;
             }
+
+            // cache the last rotation and translation matrices
+            lastRotationMatrix = new SimpleMatrix(this.rotationMatrix);
+            lastTranslationVector = new SimpleMatrix(this.translationVector);
+
+            // calculate the rotation and translation matrices between the new scan and the reference scan
+            this.calculateRotationAndTranslationMatrices(referenceScan, matchingScan);
+
+            // combine the last rotation and translation matrices with the new rotation and translation matrices
+            this.rotationMatrix = this.rotationMatrix.mult(lastRotationMatrix);
+            this.translationVector = this.translationVector.plus(lastTranslationVector);
         }
 
         return null;

tests/MatcherVisualizer.java

@@ -0,0 +1,125 @@
+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) Math.PI / 4), 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) Math.PI / 4), 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);
+        delayMillis(10);
+        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);
+        float x = 10+10;
+        float y = x+10;
+    }
+}

tests/ScanGraph/ScanMatcherTest.java

@@ -1,75 +0,0 @@
-package ScanGraph;
-
-import org.ejml.simple.SimpleMatrix;
-import org.junit.jupiter.api.Test;
-import Vector.Vector;
-
-import java.lang.reflect.Array;
-import java.util.ArrayList;
-
-import static org.junit.jupiter.api.Assertions.*;
-
-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
-     */
-    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);
-    }
-
-    @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, 10);
-        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();
-        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
-    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));
-    }
-
-    @Test
-    void iterativeScanMatch() {
-        // TODO: Write a test for this
-    }
-}

tests/ScanMatcherTest.java

@@ -0,0 +1,128 @@
+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() {
+        // 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((float) Math.PI / 4), 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((float) Math.PI / 4), 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.01f, 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);
+    }
+}