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

Finished visualizer for ICP algorithm and confirmed it's working as intended.

Browse Source
This commit is contained in:
Quinn
2023-12-09 17:16:45 -05:00
parent c340c02085
commit 64bf8769a1
3 changed files with 117 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

165
src/ScanGraph/ScanMatcher.java
View File

@@ -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 * @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){ 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 // make a copy of the new scan so we don't modify the original
this.calculateRotationAndTranslationMatrices(referenceScan, newScan); ScanPoint scanBeingMatched = new ScanPoint(newScan);
// copy the new scan so we don't modify the original // calculate the rotation and translation matrices between the two scans
ScanPoint matchingScan = new ScanPoint(newScan); this.calculateRotationAndTranslationMatrices(referenceScan, scanBeingMatched);
SimpleMatrix lastRotationMatrix; SimpleMatrix cumulativeRotationMatrix = new SimpleMatrix(this.rotationMatrix);
SimpleMatrix lastTranslationVector; SimpleMatrix cumulativeTranslationVector = new SimpleMatrix(this.translationVector);
// iterate through the scan matching algorithm
for (int i = 0; i < iterations; i++) { for (int i = 0; i < iterations; i++) {
// update the new scan with the rotation matrix and translation vector // calculate the rotation and translation matrices between the two scans
matchingScan = this.applyRotationAndTranslationMatrices(matchingScan); 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 // calculate the error between the new scan and the reference scan
float error = this.getError(referenceScan, matchingScan); float error = this.getError(referenceScan, scanBeingMatched);
// if the error is less than the error threshold, then we have a valid match
// if the error is less than some threshold, then we have found a match if(error < errorThreshold){
if (error < errorThreshold) { this.rotationMatrix = cumulativeRotationMatrix;
return referenceScan; this.translationVector = cumulativeTranslationVector;
} return scanBeingMatched;
}
// cache the last rotation and translation matrices // otherwise, we need to keep iterating
lastRotationMatrix = new SimpleMatrix(this.rotationMatrix); // add the rotation and translation matrices to the cumulative rotation and translation matrices
lastTranslationVector = new SimpleMatrix(this.translationVector); cumulativeRotationMatrix = cumulativeRotationMatrix.mult(this.rotationMatrix);
cumulativeTranslationVector = cumulativeTranslationVector.plus(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);
} }
// if we get to this point, then we have not found a valid match
return null; 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 * @brief Compute the cross covariance matrix between the new scan and the reference scan
* @return a 2x2 matrix containing the cross covariance matrix * @return a 2x2 matrix containing the cross covariance matrix
*/ */
private SimpleMatrix crossCovarianceMatrix(ScanPoint referenceScan, ScanPoint newScan){ private SimpleMatrix crossCovarianceMatrix(ScanPoint referenceScan, ScanPoint newScan, CorrespondenceMatrix correspondenceMatrix){
Vector referenceScanAveragePosition = new Vector(averageScanPosition(referenceScan));
Vector newScanAveragePosition = new Vector(averageScanPosition(newScan));
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: // compute the cross covariance matrix which is given by the formula:
// covariance = the sum from 1 to N of (p_i) * (q_i)^T // 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 // 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 // the cross covariance matrix is a 2x2 matrix
float[][] crossCovarianceMatrix = new float[2][2]; float[][] crossCovarianceMatrix = new float[2][2];
for (int i = 0; i < correspondenceMatrix.getOldPointIndices().size(); i++) { for (int i = 0; i < correspondenceMatrix.getOldPointIndices().size(); i++) {
int oldIndex = correspondenceMatrix.getOldPointIndices().get(i); int oldIndex = correspondenceMatrix.getOldPointIndices().get(i);
int newIndex = correspondenceMatrix.getNewPointIndices().get(i); int newIndex = correspondenceMatrix.getNewPointIndices().get(i);
Vector oldPoint = referenceScan.getPoints().get(oldIndex); Vector oldPoint = referenceScan.getPoints().get(oldIndex);
Vector newPoint = newScan.getPoints().get(newIndex); Vector newPoint = newScan.getPoints().get(newIndex);
if (oldPoint != null && newPoint != null) { if (oldPoint != null && newPoint != null) {
Vector oldPointCentered = oldPoint.sub(referenceScanAveragePosition); Vector oldPointOffset = oldPoint.sub(referenceScanAveragePosition);
Vector newPointCentered = newPoint.sub(newScanAveragePosition); Vector newPointOffset = newPoint.sub(newScanAveragePosition);
crossCovarianceMatrix[0][0] += oldPointCentered.x * newPointCentered.x; crossCovarianceMatrix[0][0] += oldPointOffset.x * newPointOffset.x;
crossCovarianceMatrix[0][1] += oldPointCentered.x * newPointCentered.y; crossCovarianceMatrix[0][1] += oldPointOffset.x * newPointOffset.y;
crossCovarianceMatrix[1][0] += oldPointCentered.y * newPointCentered.x; crossCovarianceMatrix[1][0] += oldPointOffset.y * newPointOffset.x;
crossCovarianceMatrix[1][1] += oldPointCentered.y * newPointCentered.y; crossCovarianceMatrix[1][1] += oldPointOffset.y * newPointOffset.y;
} }
} }
return new SimpleMatrix(crossCovarianceMatrix); 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 * The rotation matrix is a 2x2 matrix and the translation vector is a 2x1 matrix
*/ */
public void calculateRotationAndTranslationMatrices(ScanPoint referenceScan, ScanPoint newScan){ public void calculateRotationAndTranslationMatrices(ScanPoint referenceScan, ScanPoint newScan){
CorrespondenceMatrix correspondenceMatrix = new CorrespondenceMatrix(newScan, referenceScan);
// compute the rotation matrix which is given by the formula: // compute the rotation matrix which is given by the formula:
// R = V * U^T // R = V * U^T
// where V and U are the singular value decomposition of the cross covariance matrix // where V and U are the singular value decomposition of the cross covariance matrix
// the rotation matrix is a 2x2 matrix // the rotation matrix is a 2x2 matrix
SimpleMatrix crossCovarianceMatrixSimple = crossCovarianceMatrix(referenceScan, newScan); 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());
SimpleMatrix newScanAveragePosition = averageScanPosition(newScan); SimpleMatrix newScanAveragePosition = this.averageScanPosition(newScan);
SimpleMatrix referenceScanAveragePosition = averageScanPosition(referenceScan); SimpleMatrix referenceScanAveragePosition = this.averageScanPosition(referenceScan);
this.translationVector = referenceScanAveragePosition.minus(rotationMatrix.mult(newScanAveragePosition)); this.translationVector = referenceScanAveragePosition.minus(rotationMatrix.mult(newScanAveragePosition));
} }
@@ -156,6 +137,25 @@ public class ScanMatcher{
return tempScan; 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){ public float getError(ScanPoint referenceScan, ScanPoint newScan){
// calculate the error between the new scan and the reference scan // calculate the error between the new scan and the reference scan
// q is reference scan and p is new scan // q is reference scan and p is new scan
@@ -181,8 +181,12 @@ class CorrespondenceMatrix{
private ArrayList<Integer> newPointIndices = new ArrayList<>(); private ArrayList<Integer> newPointIndices = new ArrayList<>();
private ArrayList<Float> distances = new ArrayList<>(); private ArrayList<Float> distances = new ArrayList<>();
private Vector averageOldPosition = new Vector(0, 0);
private Vector averageNewPosition = new Vector(0, 0);
CorrespondenceMatrix(ScanPoint newScan, ScanPoint oldScan){ CorrespondenceMatrix(ScanPoint newScan, ScanPoint oldScan){
this.calculateCorrespondenceMatrix(newScan, oldScan); this.calculateCorrespondenceMatrix(newScan, oldScan);
this.calculateAveragePositions(newScan, oldScan);
} }
public ArrayList<Integer> getOldPointIndices(){ public ArrayList<Integer> getOldPointIndices(){
@@ -197,6 +201,33 @@ class CorrespondenceMatrix{
return this.distances; 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 * @brief Calculate the correspondence matrix between two scans
* @param newScan the new scan * @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) { 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);
// }
this.oldPointIndices.add(closestIndex); this.oldPointIndices.add(closestIndex);
this.newPointIndices.add(newPointIndex); this.newPointIndices.add(newPointIndex);
this.distances.add(closestDistance); this.distances.add(closestDistance);
} }
} }
} }

10
tests/MatcherVisualizer.java
View File

@@ -23,13 +23,13 @@ public class MatcherVisualizer extends PApplet{
// 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); 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); this.referenceScan = appendScanPoints(scan1, scan2);
// 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); 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.scanToMatch = appendScanPoints(scan4, scan5);
this.scanBeingMatched = new ScanPoint(this.scanToMatch); this.scanBeingMatched = new ScanPoint(this.scanToMatch);
} }
@@ -109,17 +109,15 @@ public class MatcherVisualizer extends PApplet{
// 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 singleScanMatchError = matcher.getError(this.referenceScan, this.scanBeingMatched);
delayMillis(10); float error = matcher.getError(this.referenceScan, this.scanBeingMatched);
drawScan(this.scanBeingMatched, blue); drawScan(this.scanBeingMatched, blue);
// do an iterative scan match and calculate the error // 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.01f, 10);
// float iterativeScanMatchError = matcher.getError(scan1, matchedScan); // float iterativeScanMatchError = matcher.getError(scan1, matchedScan);
float x = 10+10;
float y = x+10;
} }
} }

7
tests/ScanMatcherTest.java
View File

@@ -94,16 +94,17 @@ class ScanMatcherTest{
@Test @Test
public void iterativeScanMatch() { public void iterativeScanMatch() {
float bendAngle = (float) (5 * 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
ScanPoint scan1 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12); 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); ScanPoint scan3 = appendScanPoints(scan1, scan2);
// 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 = (new Vector(10, 10)).rotate2D((float) Math.PI); Vector rotated = (new Vector(10, 10)).rotate2D((float) Math.PI);
ScanPoint scan4 = generateScanPoint(new Vector(10, 10), rotated, 12); 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); ScanPoint scan6 = appendScanPoints(scan4, scan5);
@@ -115,7 +116,7 @@ class ScanMatcherTest{
// do an iterative scan match and calculate the error // 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. // if it's null something has gone wrong with the algorithm because these scans can easily be matched.
assertNotNull(matchedScan); assertNotNull(matchedScan);