From f59b9b9094a4039afe7707a00dffe0d378718806 Mon Sep 17 00:00:00 2001 From: Quinn Date: Sun, 26 Nov 2023 13:55:48 -0500 Subject: [PATCH 1/6] Removed map.txt file because it changes too much. --- .gitignore | 4 +++- src/Graph/PointGraphWriter.java | 11 +++++++++-- src/Processing.java | 11 +++-------- 3 files changed, 15 insertions(+), 11 deletions(-) diff --git a/.gitignore b/.gitignore index f68d109..6ed4513 100644 --- a/.gitignore +++ b/.gitignore @@ -26,4 +26,6 @@ bin/ .vscode/ ### Mac OS ### -.DS_Store \ No newline at end of file +.DS_Store + +map.txt diff --git a/src/Graph/PointGraphWriter.java b/src/Graph/PointGraphWriter.java index 6a895bf..b3c38f6 100644 --- a/src/Graph/PointGraphWriter.java +++ b/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 vertices = new ArrayList<>(); while(reader.hasNextLine()){ String line = reader.nextLine(); diff --git a/src/Processing.java b/src/Processing.java index 15be76e..baed4b0 100644 --- a/src/Processing.java +++ b/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(); + PointGraphWriter writer = new PointGraphWriter(); + try { map = writer.loadFile("map.txt"); - } - catch (FileNotFoundException e){ - System.out.println("File not found"); - e.printStackTrace(); - } - catch (NumberFormatException e){ + } catch (NumberFormatException e) { System.out.println("Number format incorrect"); e.printStackTrace(); } From 6a7d3eeffc6c7104ef9136d305b461f5718422ee Mon Sep 17 00:00:00 2001 From: Quinn Date: Sun, 26 Nov 2023 15:34:24 -0500 Subject: [PATCH 2/6] Began writing tests for the scan matcher. --- src/ScanGraph/ScanGraph.java | 216 +------------------------- src/ScanGraph/ScanMatcher.java | 217 +++++++++++++++++++++++++++ src/ScanGraph/ScanPoint.java | 11 ++ src/Vector/Vector.java | 8 + tests/ScanGraph/ScanMatcherTest.java | 71 +++++++++ 5 files changed, 313 insertions(+), 210 deletions(-) create mode 100644 src/ScanGraph/ScanMatcher.java create mode 100644 tests/ScanGraph/ScanMatcherTest.java diff --git a/src/ScanGraph/ScanGraph.java b/src/ScanGraph/ScanGraph.java index fb4c3e5..baa5ba8 100644 --- a/src/ScanGraph/ScanGraph.java +++ b/src/ScanGraph/ScanGraph.java @@ -7,15 +7,16 @@ import org.ejml.simple.SimpleMatrix; import org.ejml.simple.SimpleSVD; import java.util.ArrayList; -public class ScanGraph extends Graph{ +public class ScanGraph extends Graph { ScanPoint lastPoint; - public ScanGraph(ScanPoint startingPoint){ + + public ScanGraph(ScanPoint startingPoint) { super(); this.lastPoint = startingPoint; } - public void addEdge(ScanPoint vEnd){ + public void addEdge(ScanPoint vEnd) { addVertex(vEnd); ScanEdge edge = new ScanEdge(this.lastPoint, vEnd); adjList.get((Vertex) this.lastPoint).add(edge); @@ -24,16 +25,16 @@ 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(); // 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++) { + for (int i = 0; i < 5; i++) { // calculate the rotation and translation matrices between the new scan and the reference scan matcher.calculateRotationAndTranslationMatrices(referenceScan, newScan); @@ -51,209 +52,4 @@ public class ScanGraph extends Graph{ } 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 oldPointIndices = new ArrayList<>(); - private ArrayList newPointIndices = new ArrayList<>(); - private ArrayList distances = new ArrayList<>(); - - CorrespondenceMatrix(ScanPoint newScan, ScanPoint oldScan){ - this.calculateCorrespondenceMatrix(newScan, oldScan); - } - - public ArrayList getOldPointIndices(){ - return this.oldPointIndices; - } - - public ArrayList getNewPointIndices(){ - return this.newPointIndices; - } - - public ArrayList 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> correspondenceMatrix = new ArrayList>(); - correspondenceMatrix.add(new ArrayList()); - correspondenceMatrix.add(new ArrayList()); - correspondenceMatrix.add(new ArrayList()); - - // 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 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(); - } } diff --git a/src/ScanGraph/ScanMatcher.java b/src/ScanGraph/ScanMatcher.java new file mode 100644 index 0000000..8971aec --- /dev/null +++ b/src/ScanGraph/ScanMatcher.java @@ -0,0 +1,217 @@ +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 + */ +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 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){ + // 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; + } + + 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 oldPointIndices = new ArrayList<>(); + private ArrayList newPointIndices = new ArrayList<>(); + private ArrayList distances = new ArrayList<>(); + + CorrespondenceMatrix(ScanPoint newScan, ScanPoint oldScan){ + this.calculateCorrespondenceMatrix(newScan, oldScan); + } + + public ArrayList getOldPointIndices(){ + return this.oldPointIndices; + } + + public ArrayList getNewPointIndices(){ + return this.newPointIndices; + } + + public ArrayList 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> correspondenceMatrix = new ArrayList>(); + correspondenceMatrix.add(new ArrayList()); + correspondenceMatrix.add(new ArrayList()); + correspondenceMatrix.add(new ArrayList()); + + // 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); + } + } + } + } +} \ No newline at end of file diff --git a/src/ScanGraph/ScanPoint.java b/src/ScanGraph/ScanPoint.java index 59e915a..a0210ca 100644 --- a/src/ScanGraph/ScanPoint.java +++ b/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. */ diff --git a/src/Vector/Vector.java b/src/Vector/Vector.java index c388589..cf20ddc 100644 --- a/src/Vector/Vector.java +++ b/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(); diff --git a/tests/ScanGraph/ScanMatcherTest.java b/tests/ScanGraph/ScanMatcherTest.java new file mode 100644 index 0000000..d5f75a7 --- /dev/null +++ b/tests/ScanGraph/ScanMatcherTest.java @@ -0,0 +1,71 @@ +package ScanGraph; + +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){ + // generate a scan point with the given offset and scan description + Vector scanPosition = new Vector(0, 0); + ArrayList scan = new ArrayList<>(); + + // calculate the total number of points in the scan + int numPoints = (int) scanDescription.mag(); + // 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; + float y = m * x; + scan.add(new Vector(x + offset.x, y + offset.y)); + } + + return new ScanPoint(scanPosition, 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 newScan = generateScanPoint(new Vector(0, 0), scanDescription.rotate2D((float) Math.PI / 4)); + + // 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); + float angle = firstPoint.angleDiff(lastPoint); + + // 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)); + ScanPoint scan2 = generateScanPoint(new Vector(0, 0), new Vector(10, 10)); + ScanMatcher matcher = new ScanMatcher(); + matcher.calculateRotationAndTranslationMatrices(scan1, scan2); + assertEquals(0, matcher.getError(scan1, scan2)); + } +} \ No newline at end of file From df572532879b9982d5b283dbe6e96ee81a5111f2 Mon Sep 17 00:00:00 2001 From: quinn Date: Wed, 29 Nov 2023 20:44:54 -0500 Subject: [PATCH 3/6] Added some unit tests for the ScanMatcher and fixed some broken functionality. --- .idea/misc.xml | 3 +- SLAM-Sim.iml | 2 +- src/ScanGraph/ScanGraph.java | 19 ++----- src/ScanGraph/ScanMatcher.java | 83 +++++++++++++++------------- tests/ScanGraph/ScanMatcherTest.java | 38 +++++++------ 5 files changed, 74 insertions(+), 71 deletions(-) diff --git a/.idea/misc.xml b/.idea/misc.xml index b95853c..cbbff6a 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,5 +1,6 @@ + - + \ No newline at end of file diff --git a/SLAM-Sim.iml b/SLAM-Sim.iml index 8b189e9..52325fd 100644 --- a/SLAM-Sim.iml +++ b/SLAM-Sim.iml @@ -11,7 +11,7 @@ - + diff --git a/src/ScanGraph/ScanGraph.java b/src/ScanGraph/ScanGraph.java index baa5ba8..71c89cd 100644 --- a/src/ScanGraph/ScanGraph.java +++ b/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++) { - // calculate the rotation and translation matrices between the new scan and the reference scan - matcher.calculateRotationAndTranslationMatrices(referenceScan, newScan); + matchedScan = matcher.iterativeScanMatch(referenceScan, newScan, 0.1F, 10); - // update the new scan with the rotation matrix and translation vector - newScan = matcher.applyRotationAndTranslationMatrices(newScan); - - // 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; - } + if(matchedScan != null){ + break; } } - return null; + return matchedScan; } } diff --git a/src/ScanGraph/ScanMatcher.java b/src/ScanGraph/ScanMatcher.java index 8971aec..dc180ed 100644 --- a/src/ScanGraph/ScanMatcher.java +++ b/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 @@ -159,58 +185,39 @@ class CorrespondenceMatrix{ * @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> correspondenceMatrix = new ArrayList>(); - correspondenceMatrix.add(new ArrayList()); - correspondenceMatrix.add(new ArrayList()); - correspondenceMatrix.add(new ArrayList()); - - // go through all of the points in the new scan and find the closest point in the old scan + private void calculateCorrespondenceMatrix(ScanPoint newScan, ScanPoint referenceScan) { 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); - // if the old point is null, then skip it + + 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 = 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 - // 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 + + // Add the correspondence if a closest point is found 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); - } + this.oldPointIndices.add(closestIndex); + this.newPointIndices.add(newPointIndex); + this.distances.add(closestDistance); } } } diff --git a/tests/ScanGraph/ScanMatcherTest.java b/tests/ScanGraph/ScanMatcherTest.java index d5f75a7..b77c90b 100644 --- a/tests/ScanGraph/ScanMatcherTest.java +++ b/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 scan = new ArrayList<>(); - // calculate the total number of points in the scan - int numPoints = (int) scanDescription.mag(); - // calculate the slope of the line the scan is on - float m = scanDescription.y / scanDescription.x; + // 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); - // add the points to the scan - for(int i = 0; i < numPoints; i++){ - float x = i; - float y = m * x; - scan.add(new Vector(x + offset.x, y + offset.y)); + for (int i = 0; i < numPoints; i++) { + scan.add(offset.add(directionVector.mul(i))); } - 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 newScan = generateScanPoint(new Vector(0, 0), scanDescription.rotate2D((float) Math.PI / 4)); + 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(); @@ -53,7 +51,8 @@ class ScanMatcherTest { ArrayList 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 scan2 = 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), 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 From dbb6b519e6aa211132292a072ff194c58a4e882c Mon Sep 17 00:00:00 2001 From: Quinn Date: Fri, 1 Dec 2023 13:46:01 -0500 Subject: [PATCH 4/6] Undid stupid windows OS specific changes. --- .idea/misc.xml | 2 +- SLAM-Sim.iml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.idea/misc.xml b/.idea/misc.xml index cbbff6a..cf9abe6 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/SLAM-Sim.iml b/SLAM-Sim.iml index 52325fd..8b189e9 100644 --- a/SLAM-Sim.iml +++ b/SLAM-Sim.iml @@ -11,7 +11,7 @@ - + From c340c02085acde992965707f89202c00cbbec480 Mon Sep 17 00:00:00 2001 From: Quinn Date: Sat, 9 Dec 2023 15:29:51 -0500 Subject: [PATCH 5/6] 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 From 64bf8769a17bd5c36d6a3bca42c836253fc95221 Mon Sep 17 00:00:00 2001 From: Quinn Date: Sat, 9 Dec 2023 17:16:45 -0500 Subject: [PATCH 6/6] Finished visualizer for ICP algorithm and confirmed it's working as intended. --- src/ScanGraph/ScanMatcher.java | 169 +++++++++++++++++++++------------ tests/MatcherVisualizer.java | 10 +- tests/ScanMatcherTest.java | 7 +- 3 files changed, 117 insertions(+), 69 deletions(-) diff --git a/src/ScanGraph/ScanMatcher.java b/src/ScanGraph/ScanMatcher.java index bec82fb..0bc5c46 100644 --- a/src/ScanGraph/ScanMatcher.java +++ b/src/ScanGraph/ScanMatcher.java @@ -29,87 +29,66 @@ public 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){ - // calculate the rotation and translation matrices between the new scan and the reference scan - this.calculateRotationAndTranslationMatrices(referenceScan, newScan); + // make a copy of the new scan so we don't modify the original + ScanPoint scanBeingMatched = new ScanPoint(newScan); - // copy the new scan so we don't modify the original - ScanPoint matchingScan = new ScanPoint(newScan); + // calculate the rotation and translation matrices between the two scans + this.calculateRotationAndTranslationMatrices(referenceScan, scanBeingMatched); - SimpleMatrix lastRotationMatrix; - SimpleMatrix lastTranslationVector; + 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++) { - // update the new scan with the rotation matrix and translation vector - matchingScan = this.applyRotationAndTranslationMatrices(matchingScan); - + // 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, matchingScan); - - // if the error is less than some threshold, then we have found a match - if (error < errorThreshold) { - return referenceScan; + 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; } - - // 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); + // 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 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)); + private SimpleMatrix crossCovarianceMatrix(ScanPoint referenceScan, ScanPoint newScan, CorrespondenceMatrix correspondenceMatrix){ - CorrespondenceMatrix correspondenceMatrix = new CorrespondenceMatrix(newScan, referenceScan); + 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 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; + 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); @@ -120,16 +99,18 @@ public class ScanMatcher{ * 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); + SimpleMatrix crossCovarianceMatrixSimple = crossCovarianceMatrix(referenceScan, newScan, correspondenceMatrix); SimpleSVD svd = crossCovarianceMatrixSimple.svd(); this.rotationMatrix = svd.getU().mult(svd.getV().transpose()); - SimpleMatrix newScanAveragePosition = averageScanPosition(newScan); - SimpleMatrix referenceScanAveragePosition = averageScanPosition(referenceScan); + SimpleMatrix newScanAveragePosition = this.averageScanPosition(newScan); + SimpleMatrix referenceScanAveragePosition = this.averageScanPosition(referenceScan); this.translationVector = referenceScanAveragePosition.minus(rotationMatrix.mult(newScanAveragePosition)); } @@ -156,6 +137,25 @@ public class ScanMatcher{ 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 @@ -181,8 +181,12 @@ class CorrespondenceMatrix{ private ArrayList newPointIndices = new ArrayList<>(); private ArrayList 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 getOldPointIndices(){ @@ -197,6 +201,33 @@ class CorrespondenceMatrix{ 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 @@ -230,11 +261,29 @@ class CorrespondenceMatrix{ } } - // Add the correspondence if a closest point is found + // if we find a closest point... if (closestIndex != -1) { - this.oldPointIndices.add(closestIndex); - this.newPointIndices.add(newPointIndex); - this.distances.add(closestDistance); +// // 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); + } } } diff --git a/tests/MatcherVisualizer.java b/tests/MatcherVisualizer.java index 2bf9463..3895836 100644 --- a/tests/MatcherVisualizer.java +++ b/tests/MatcherVisualizer.java @@ -23,13 +23,13 @@ public class MatcherVisualizer extends PApplet{ // 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); + 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) Math.PI / 4), 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); } @@ -109,17 +109,15 @@ public class MatcherVisualizer extends PApplet{ // do a single scan match and calculate the error ScanMatcher matcher = new ScanMatcher(); - matcher.calculateRotationAndTranslationMatrices(this.referenceScan, this.scanBeingMatched); +// matcher.calculateRotationAndTranslationMatrices(this.referenceScan, this.scanBeingMatched); this.scanBeingMatched = matcher.applyRotationAndTranslationMatrices(this.scanBeingMatched); float singleScanMatchError = matcher.getError(this.referenceScan, this.scanBeingMatched); - delayMillis(10); + 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); - float x = 10+10; - float y = x+10; } } diff --git a/tests/ScanMatcherTest.java b/tests/ScanMatcherTest.java index a33edc1..729e8ec 100644 --- a/tests/ScanMatcherTest.java +++ b/tests/ScanMatcherTest.java @@ -94,16 +94,17 @@ class ScanMatcherTest{ @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((float) Math.PI / 4), 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((float) Math.PI / 4), 12); + ScanPoint scan5 = generateScanPoint(new Vector(10, 10), rotated.rotate2D(bendAngle), 12); ScanPoint scan6 = appendScanPoints(scan4, scan5); @@ -115,7 +116,7 @@ class ScanMatcherTest{ // do an iterative scan match and calculate the error - ScanPoint matchedScan = matcher.iterativeScanMatch(scan1, scan2, 0.01f, 10); + 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);