From c340c02085acde992965707f89202c00cbbec480 Mon Sep 17 00:00:00 2001 From: Quinn Date: Sat, 9 Dec 2023 15:29:51 -0500 Subject: [PATCH] Created a visualizer for the ICP algorithm to figure out what's going wrong. --- .idea/misc.xml | 1 - SLAM-Sim.iml | 3 + src/ScanGraph/ScanMatcher.java | 31 +++++-- tests/MatcherVisualizer.java | 125 ++++++++++++++++++++++++++ tests/ScanGraph/ScanMatcherTest.java | 75 ---------------- tests/ScanMatcherTest.java | 128 +++++++++++++++++++++++++++ 6 files changed, 280 insertions(+), 83 deletions(-) create mode 100644 tests/MatcherVisualizer.java delete mode 100644 tests/ScanGraph/ScanMatcherTest.java create mode 100644 tests/ScanMatcherTest.java diff --git a/.idea/misc.xml b/.idea/misc.xml index cf9abe6..b95853c 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,4 +1,3 @@ - diff --git a/SLAM-Sim.iml b/SLAM-Sim.iml index 8b189e9..501195a 100644 --- a/SLAM-Sim.iml +++ b/SLAM-Sim.iml @@ -44,5 +44,8 @@ + + + \ No newline at end of file diff --git a/src/ScanGraph/ScanMatcher.java b/src/ScanGraph/ScanMatcher.java index dc180ed..bec82fb 100644 --- a/src/ScanGraph/ScanMatcher.java +++ b/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 - this.calculateRotationAndTranslationMatrices(referenceScan, newScan); + // calculate the rotation and translation matrices between the new scan and the reference scan + 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; diff --git a/tests/MatcherVisualizer.java b/tests/MatcherVisualizer.java new file mode 100644 index 0000000..2bf9463 --- /dev/null +++ b/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 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 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 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; + } +} diff --git a/tests/ScanGraph/ScanMatcherTest.java b/tests/ScanGraph/ScanMatcherTest.java deleted file mode 100644 index b77c90b..0000000 --- a/tests/ScanGraph/ScanMatcherTest.java +++ /dev/null @@ -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 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 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 - } -} \ No newline at end of file diff --git a/tests/ScanMatcherTest.java b/tests/ScanMatcherTest.java new file mode 100644 index 0000000..a33edc1 --- /dev/null +++ b/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 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 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 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); + } +} \ No newline at end of file