Cynopolis/SLAM-Sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Improved the power of the scan matcher to better work with more cases.

Browse Source
This commit is contained in:
quinn
2023-12-11 15:14:12 -05:00
parent f7fd6fc6a8
commit 8a5d5275c4
3 changed files with 49 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -29,3 +29,4 @@ bin/
.DS_Store
map.txt
processing-4.3/

53
src/ScanGraph/ScanMatcher.java
View File

@@ -108,10 +108,25 @@ public class ScanMatcher{
SimpleMatrix crossCovarianceMatrixSimple = crossCovarianceMatrix(referenceScan, newScan, correspondenceMatrix);
SimpleSVD<SimpleMatrix> 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);
}
}
}
}

32
tests/MatcherVisualizer.java
View File

@@ -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);