From 64bf8769a17bd5c36d6a3bca42c836253fc95221 Mon Sep 17 00:00:00 2001 From: Quinn Date: Sat, 9 Dec 2023 17:16:45 -0500 Subject: [PATCH] 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);