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

169
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
*/
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<SimpleMatrix> 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<Integer> newPointIndices = 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){
this.calculateCorrespondenceMatrix(newScan, oldScan);
this.calculateAveragePositions(newScan, oldScan);
}
public ArrayList<Integer> 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);
}
}
}