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

Merge branch 'slam-feature-graph' into 5-close-graph-loop

Browse Source
This commit is contained in:
Quinn
2023-12-11 18:55:39 -05:00
parent 2d55388f24 8a5d5275c4
commit c2a24e6f19
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 .DS_Store
map.txt 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); SimpleMatrix crossCovarianceMatrixSimple = crossCovarianceMatrix(referenceScan, newScan, correspondenceMatrix);
SimpleSVD<SimpleMatrix> svd = crossCovarianceMatrixSimple.svd(); SimpleSVD<SimpleMatrix> svd = crossCovarianceMatrixSimple.svd();
this.rotationMatrix = svd.getU().mult(svd.getV().transpose()); 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); // percentage of the local position to use in the translation calculation
SimpleMatrix referenceScanAveragePosition = this.averageScanPosition(referenceScan); double weightedAverage = 0.9;
this.translationVector = referenceScanAveragePosition.minus(rotationMatrix.mult(newScanAveragePosition));
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(){ public SimpleMatrix getRotationMatrix(){
@@ -264,27 +279,23 @@ class CorrespondenceMatrix{
// if we find a closest point... // if we find a closest point...
if (closestIndex != -1) { if (closestIndex != -1) {
// // check if the oldPointIndex is already in the list of oldPointIndices // check if the oldPointIndex is already in the list of oldPointIndices
// if(this.oldPointIndices.contains(closestIndex)){ if(this.oldPointIndices.contains(closestIndex)){
// int index = this.oldPointIndices.indexOf(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 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(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 // 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.oldPointIndices.set(index, closestIndex);
// this.newPointIndices.set(index, newPointIndex); this.newPointIndices.set(index, newPointIndex);
// this.distances.set(index, closestDistance); this.distances.set(index, closestDistance);
// } }
// } }
// // if the index is not in our list, then we need to add it // if the index is not in our list, then we need to add it
// else{ else{
// this.oldPointIndices.add(closestIndex);
// this.newPointIndices.add(newPointIndex);
// this.distances.add(closestDistance);
// }
this.oldPointIndices.add(closestIndex); this.oldPointIndices.add(closestIndex);
this.newPointIndices.add(newPointIndex); this.newPointIndices.add(newPointIndex);
this.distances.add(closestDistance); this.distances.add(closestDistance);
}
} }
} }
} }

32
tests/MatcherVisualizer.java
View File

@@ -19,18 +19,29 @@ public class MatcherVisualizer extends PApplet{
public void settings(){ public void settings(){
processing = this; processing = this;
size(1000, 1000); size(1000, 1000);
float connectingAngle = (float) (3 * Math.PI / 9);
// generate two scans rotated by 45 degrees and append them together // generate two scans rotated by 45 degrees and append them together
Vector descriptor = new Vector(200, 200); Vector descriptor = new Vector(200, 200);
ScanPoint scan1 = generateScanPoint(new Vector(500, 500), descriptor, 12); Vector position = new Vector(500, 500);
ScanPoint scan2 = generateScanPoint(new Vector(500, 500), descriptor.rotate2D((float) (6 * Math.PI / 9)), 12); 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(scan1, scan2);
this.referenceScan = appendScanPoints(this.referenceScan, scan3);
// generate two scans offset by some amount and rotated by 55 degrees and append them together // generate two scans offset by some amount and rotated by 55 degrees and append them together
Vector rotated = descriptor.rotate2D((float) Math.PI); Vector rotated = descriptor.rotate2D((float) Math.PI);
ScanPoint scan4 = generateScanPoint(new Vector(250, 300), rotated, 12); Vector offset = new Vector(100, 150);
ScanPoint scan5 = generateScanPoint(new Vector(250, 300), rotated.rotate2D((float) (6 * Math.PI / 9)), 12); 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(scan4, scan5);
this.scanToMatch = appendScanPoints(this.scanToMatch, scan6);
this.scanBeingMatched = new ScanPoint(this.scanToMatch); this.scanBeingMatched = new ScanPoint(this.scanToMatch);
} }
public void draw(){ public void draw(){
@@ -71,15 +82,6 @@ public class MatcherVisualizer extends PApplet{
return new ScanPoint(new Vector(0, 0), 0, points); 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 * @brief Draw a scan point to the screen
* @param scan The scan point to draw * @param scan The scan point to draw
@@ -104,14 +106,12 @@ public class MatcherVisualizer extends PApplet{
drawScan(this.referenceScan, red); drawScan(this.referenceScan, red);
delayMillis(10);
drawScan(this.scanToMatch, green); drawScan(this.scanToMatch, green);
// do a single scan match and calculate the error // do a single scan match and calculate the error
ScanMatcher matcher = new ScanMatcher(); ScanMatcher matcher = new ScanMatcher();
// matcher.calculateRotationAndTranslationMatrices(this.referenceScan, this.scanBeingMatched); matcher.calculateRotationAndTranslationMatrices(this.referenceScan, this.scanBeingMatched);
this.scanBeingMatched = matcher.applyRotationAndTranslationMatrices(this.scanBeingMatched); this.scanBeingMatched = matcher.applyRotationAndTranslationMatrices(this.scanBeingMatched);
float singleScanMatchError = matcher.getError(this.referenceScan, this.scanBeingMatched);
float error = matcher.getError(this.referenceScan, this.scanBeingMatched); float error = matcher.getError(this.referenceScan, this.scanBeingMatched);
drawScan(this.scanBeingMatched, blue); drawScan(this.scanBeingMatched, blue);