diff --git a/.gitignore b/.gitignore index 6ed4513..b70cd3a 100644 --- a/.gitignore +++ b/.gitignore @@ -29,3 +29,4 @@ bin/ .DS_Store map.txt +processing-4.3/ diff --git a/src/ScanGraph/ScanMatcher.java b/src/ScanGraph/ScanMatcher.java index 0bc5c46..2f06c0f 100644 --- a/src/ScanGraph/ScanMatcher.java +++ b/src/ScanGraph/ScanMatcher.java @@ -108,10 +108,25 @@ public class ScanMatcher{ SimpleMatrix crossCovarianceMatrixSimple = crossCovarianceMatrix(referenceScan, newScan, correspondenceMatrix); SimpleSVD svd = crossCovarianceMatrixSimple.svd(); this.rotationMatrix = svd.getU().mult(svd.getV().transpose()); + // calculate what the angle of the rotation matrix is + float angle = (float) Math.atan2(this.rotationMatrix.get(1, 0), this.rotationMatrix.get(0, 0)); + // scale the angle by a small amount to make the rotation matrix more accurate + angle*= 1.75F; + this.rotationMatrix = new SimpleMatrix(new double[][]{{Math.cos(angle), -Math.sin(angle)}, {Math.sin(angle), Math.cos(angle)}}); - SimpleMatrix newScanAveragePosition = this.averageScanPosition(newScan); - SimpleMatrix referenceScanAveragePosition = this.averageScanPosition(referenceScan); - this.translationVector = referenceScanAveragePosition.minus(rotationMatrix.mult(newScanAveragePosition)); + // percentage of the local position to use in the translation calculation + double weightedAverage = 0.9; + + SimpleMatrix localNewScanAveragePosition = new SimpleMatrix(correspondenceMatrix.getAverageNewPosition().toArray());//this.averageScanPosition(newScan); + SimpleMatrix globalNewScanAveragePosition = new SimpleMatrix(this.averageScanPosition(newScan)); + SimpleMatrix weightedAverageNewScanPostion = localNewScanAveragePosition.scale(weightedAverage).plus(globalNewScanAveragePosition.scale(1-weightedAverage)); + + SimpleMatrix localReferenceScanAveragePosition = new SimpleMatrix(correspondenceMatrix.getAverageOldPosition().toArray()); //this.averageScanPosition(referenceScan); + SimpleMatrix globalReferenceScanAveragePosition = new SimpleMatrix(this.averageScanPosition(referenceScan)); + SimpleMatrix weightedAverageReferenceScanPostion = localReferenceScanAveragePosition.scale(weightedAverage).plus(globalReferenceScanAveragePosition.scale(1-weightedAverage)); + + + this.translationVector = weightedAverageReferenceScanPostion.minus(rotationMatrix.mult(weightedAverageNewScanPostion)); } public SimpleMatrix getRotationMatrix(){ @@ -263,27 +278,23 @@ class CorrespondenceMatrix{ // if we find a closest point... if (closestIndex != -1) { -// // 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); -// } + // 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); - + } } } } diff --git a/tests/MatcherVisualizer.java b/tests/MatcherVisualizer.java index 3895836..aba279c 100644 --- a/tests/MatcherVisualizer.java +++ b/tests/MatcherVisualizer.java @@ -19,18 +19,29 @@ public class MatcherVisualizer extends PApplet{ public void settings(){ processing = this; size(1000, 1000); + float connectingAngle = (float) (3 * Math.PI / 9); // 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) (6 * Math.PI / 9)), 12); + Vector position = new Vector(500, 500); + ScanPoint scan1 = generateScanPoint(position, descriptor, 12); + ScanPoint scan2 = generateScanPoint(position, descriptor.rotate2D(connectingAngle), 12); + Vector p1 = scan1.getPoints().get(11); + Vector p2 = scan2.getPoints().get(11); + ScanPoint scan3 = generateScanPoint(p1, p2.sub(p1), 12); this.referenceScan = appendScanPoints(scan1, scan2); + this.referenceScan = appendScanPoints(this.referenceScan, scan3); // 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) (6 * Math.PI / 9)), 12); + Vector offset = new Vector(100, 150); + ScanPoint scan4 = generateScanPoint(position.add(offset), rotated, 12); + ScanPoint scan5 = generateScanPoint(position.add(offset), rotated.rotate2D(connectingAngle), 12); + p1 = scan4.getPoints().get(11); + p2 = scan5.getPoints().get(11); + ScanPoint scan6 = generateScanPoint(p1, p2.sub(p1), 12); this.scanToMatch = appendScanPoints(scan4, scan5); + this.scanToMatch = appendScanPoints(this.scanToMatch, scan6); this.scanBeingMatched = new ScanPoint(this.scanToMatch); } public void draw(){ @@ -71,15 +82,6 @@ public class MatcherVisualizer extends PApplet{ 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 @@ -104,14 +106,12 @@ public class MatcherVisualizer extends PApplet{ 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); + matcher.calculateRotationAndTranslationMatrices(this.referenceScan, this.scanBeingMatched); this.scanBeingMatched = matcher.applyRotationAndTranslationMatrices(this.scanBeingMatched); - float singleScanMatchError = matcher.getError(this.referenceScan, this.scanBeingMatched); float error = matcher.getError(this.referenceScan, this.scanBeingMatched); drawScan(this.scanBeingMatched, blue);