diff --git a/.gitignore b/.gitignore
index f68d109..6ed4513 100644
--- a/.gitignore
+++ b/.gitignore
@@ -26,4 +26,6 @@ bin/
.vscode/
### Mac OS ###
-.DS_Store
\ No newline at end of file
+.DS_Store
+
+map.txt
diff --git a/SLAM-Sim.iml b/SLAM-Sim.iml
index 8b189e9..501195a 100644
--- a/SLAM-Sim.iml
+++ b/SLAM-Sim.iml
@@ -44,5 +44,8 @@
+
+
+
\ No newline at end of file
diff --git a/src/Graph/PointGraphWriter.java b/src/Graph/PointGraphWriter.java
index 6a895bf..b3c38f6 100644
--- a/src/Graph/PointGraphWriter.java
+++ b/src/Graph/PointGraphWriter.java
@@ -52,10 +52,17 @@ public class PointGraphWriter {
file.close();
}
- public PointGraph loadFile(String filename) throws FileNotFoundException, NumberFormatException {
+ public PointGraph loadFile(String filename) throws NumberFormatException {
PointGraph g = new PointGraph();
File file = new File(filename);
- Scanner reader = new Scanner(file);
+ Scanner reader;
+ try {
+ reader = new Scanner(file);
+ }
+ catch (FileNotFoundException e){
+ System.out.println("File not found");
+ return g;
+ }
ArrayList vertices = new ArrayList<>();
while(reader.hasNextLine()){
String line = reader.nextLine();
diff --git a/src/Processing.java b/src/Processing.java
index 15be76e..baed4b0 100644
--- a/src/Processing.java
+++ b/src/Processing.java
@@ -79,15 +79,10 @@ public class Processing extends PApplet {
}
if(key == 'l'){
System.out.println("Attempting to load a map from file");
- try{
- PointGraphWriter writer = new PointGraphWriter();
+ PointGraphWriter writer = new PointGraphWriter();
+ try {
map = writer.loadFile("map.txt");
- }
- catch (FileNotFoundException e){
- System.out.println("File not found");
- e.printStackTrace();
- }
- catch (NumberFormatException e){
+ } catch (NumberFormatException e) {
System.out.println("Number format incorrect");
e.printStackTrace();
}
diff --git a/src/ScanGraph/ScanGraph.java b/src/ScanGraph/ScanGraph.java
index fb4c3e5..71c89cd 100644
--- a/src/ScanGraph/ScanGraph.java
+++ b/src/ScanGraph/ScanGraph.java
@@ -7,15 +7,16 @@ import org.ejml.simple.SimpleMatrix;
import org.ejml.simple.SimpleSVD;
import java.util.ArrayList;
-public class ScanGraph extends Graph{
+public class ScanGraph extends Graph {
ScanPoint lastPoint;
- public ScanGraph(ScanPoint startingPoint){
+
+ public ScanGraph(ScanPoint startingPoint) {
super();
this.lastPoint = startingPoint;
}
- public void addEdge(ScanPoint vEnd){
+ public void addEdge(ScanPoint vEnd) {
addVertex(vEnd);
ScanEdge edge = new ScanEdge(this.lastPoint, vEnd);
adjList.get((Vertex) this.lastPoint).add(edge);
@@ -24,236 +25,22 @@ public class ScanGraph extends Graph{
}
/**
- * @brief Get a new scan in and try to match it with all other scans in the graph
* @param newScan the scan to match
* @return null if no match can be found, or an existing scan the matches the new scan.
+ * @brief Get a new scan in and try to match it with all other scans in the graph
*/
private ScanPoint getAssociatedScan(ScanPoint newScan) {
ScanMatcher matcher = new ScanMatcher();
+ ScanPoint matchedScan = null;
// go through all of our available scans and try to match the new scan with the old scans. If no match can be found return null
for (Vertex v : adjList.keySet()) {
ScanPoint referenceScan = (ScanPoint) v;
- for(int i = 0; i < 5; i++) {
- // calculate the rotation and translation matrices between the new scan and the reference scan
- matcher.calculateRotationAndTranslationMatrices(referenceScan, newScan);
+ matchedScan = matcher.iterativeScanMatch(referenceScan, newScan, 0.1F, 10);
- // update the new scan with the rotation matrix and translation vector
- newScan = matcher.applyRotationAndTranslationMatrices(newScan);
-
- // calculate the error between the new scan and the reference scan
- float error = matcher.getError(referenceScan, newScan);
-
- // if the error is less than some threshold, then we have found a match
- if (error < 0.1) {
- return referenceScan;
- }
- }
- }
- return null;
- }
-
-}
-
-
-/**
- * @brief A class to hold the correspondence matrix between two scans
- * The correspondence matrix is a 3xN matrix where N is the number of valid points in the scan.
- * This calculates the closest point in the old scan for each point in the new scan and gets rid of redundant closest points.
- */
-class CorrespondenceMatrix{
- private ArrayList oldPointIndices = new ArrayList<>();
- private ArrayList newPointIndices = new ArrayList<>();
- private ArrayList distances = new ArrayList<>();
-
- CorrespondenceMatrix(ScanPoint newScan, ScanPoint oldScan){
- this.calculateCorrespondenceMatrix(newScan, oldScan);
- }
-
- public ArrayList getOldPointIndices(){
- return this.oldPointIndices;
- }
-
- public ArrayList getNewPointIndices(){
- return this.newPointIndices;
- }
-
- public ArrayList getDistances(){
- return this.distances;
- }
-
- /**
- * @brief Calculate the correspondence matrix between two scans
- * @param newScan the new scan
- * @param referenceScan the reference scan
- */
- private void calculateCorrespondenceMatrix(ScanPoint newScan, ScanPoint referenceScan){
- // compute the correspondence matrix between the two scans. It is a 3xN matrix where N is the number of points in the scan
- // Row 1 is the index of the point in the old scan
- // Row 2 is the index of the point in the new scan
- // Row 3 is the distance between the two points
- // if either scan has a null point, then skip that point
-
- // initialize the correspondence matrix as an array of array lists
- ArrayList> correspondenceMatrix = new ArrayList>();
- correspondenceMatrix.add(new ArrayList());
- correspondenceMatrix.add(new ArrayList());
- correspondenceMatrix.add(new ArrayList());
-
- // go through all of the points in the new scan and find the closest point in the old scan
- for (int newPointIndex = 0; newPointIndex < newScan.getPoints().size(); newPointIndex++) {
- Vector newPoint = newScan.getPoints().get(newPointIndex);
- // if the new point is null, then skip it
- if (newPoint == null) {
- continue;
- }
- // find the closest point in the old scan
- float closestDistance = Float.MAX_VALUE;
- int closestIndex = -1;
- for (int j = 0; j < referenceScan.getPoints().size(); j++) {
- Vector oldPoint = referenceScan.getPoints().get(j);
- // if the old point is null, then skip it
- if (oldPoint == null) {
- continue;
- }
- float distance = newPoint.sub(oldPoint).mag();
- if (distance < closestDistance) {
- closestDistance = distance;
- closestIndex = j;
- }
- }
- // only add the new point if it either:
- // 1. has a closest point index which does not already exist in the correspondence matrix
- // 2. has a closest point index which already exists in the correspondence matrix, but the distance is smaller than the existing distance
- // In case 2, we want to replace the old point with the new point
- if (closestIndex != -1) {
- if (correspondenceMatrix.get(0).contains((float) closestIndex)) {
- int oldIndex = correspondenceMatrix.get(0).indexOf((float) closestIndex);
- if (correspondenceMatrix.get(2).get(oldIndex) > closestDistance) {
- correspondenceMatrix.get(0).set(oldIndex, (float) closestIndex);
- correspondenceMatrix.get(1).set(oldIndex, (float) newPointIndex);
- correspondenceMatrix.get(2).set(oldIndex, closestDistance);
- }
- } else {
- correspondenceMatrix.get(0).add((float) closestIndex);
- correspondenceMatrix.get(1).add((float) newPointIndex);
- correspondenceMatrix.get(2).add(closestDistance);
- }
+ if(matchedScan != null){
+ break;
}
}
- }
-}
-
-class ScanMatcher{
- // A 2x2 matrix describing a rotation to apply to the new scan
- SimpleMatrix rotationMatrix;
-
- // A 2x1 matrix describing a translation to apply to the new scan
- SimpleMatrix translationVector;
-
- ScanMatcher(){
- }
-
- /**
- * @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));
-
- CorrespondenceMatrix correspondenceMatrix = new CorrespondenceMatrix(newScan, referenceScan);
-
- // 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;
- }
- }
- return new SimpleMatrix(crossCovarianceMatrix);
- }
-
- /**
- * @brief Compute the rotation and translation matrices between the new scan and the reference scan. Then cache them as private variables.
- * The rotation matrix is a 2x2 matrix and the translation vector is a 2x1 matrix
- */
- public void calculateRotationAndTranslationMatrices(ScanPoint referenceScan, ScanPoint newScan){
- // 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);
- SimpleSVD svd = crossCovarianceMatrixSimple.svd();
- this.rotationMatrix = svd.getU().mult(svd.getV().transpose());
-
- SimpleMatrix newScanAveragePosition = averageScanPosition(newScan);
- SimpleMatrix referenceScanAveragePosition = averageScanPosition(referenceScan);
- this.translationVector = referenceScanAveragePosition.minus(rotationMatrix.mult(newScanAveragePosition));
- }
-
- public SimpleMatrix getRotationMatrix(){
- return this.rotationMatrix;
- }
-
- public SimpleMatrix getTranslationVector(){
- return this.translationVector;
- }
-
- public ScanPoint applyRotationAndTranslationMatrices(ScanPoint newScan){
- // apply the rotation matrix and translation vector to the new scan
- for (int i = 0; i < newScan.getPoints().size(); i++) {
- Vector point = newScan.getPoints().get(i);
- if (point != null) {
- SimpleMatrix pointMatrix = new SimpleMatrix(point.toArray());
- SimpleMatrix newPointMatrix = rotationMatrix.mult(pointMatrix).plus(translationVector);
- newScan.getPoints().set(i, new Vector((float) newPointMatrix.get(0), (float) newPointMatrix.get(1)));
- }
- }
- return newScan;
- }
-
- 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
- // error is given as abs(Q_mean - R * P_mean)
- // where Q_mean is the average position of the reference scan
- // P_mean is the average position of the new scan
- // R is the rotation matrix
-
- SimpleMatrix newScanAveragePosition = averageScanPosition(newScan);
- SimpleMatrix referenceScanAveragePosition = averageScanPosition(referenceScan);
- SimpleMatrix error = referenceScanAveragePosition.minus(rotationMatrix.mult(newScanAveragePosition));
- return (float) error.elementSum();
+ return matchedScan;
}
}
diff --git a/src/ScanGraph/ScanMatcher.java b/src/ScanGraph/ScanMatcher.java
new file mode 100644
index 0000000..0bc5c46
--- /dev/null
+++ b/src/ScanGraph/ScanMatcher.java
@@ -0,0 +1,290 @@
+package ScanGraph;
+
+import Vector.Vector;
+import org.ejml.simple.SimpleMatrix;
+import org.ejml.simple.SimpleSVD;
+
+import java.util.ArrayList;
+
+import static java.lang.Math.abs;
+
+/**
+ * @brief A class that can match two point scans together
+ */
+public class ScanMatcher{
+ // A 2x2 matrix describing a rotation to apply to the new scan
+ public SimpleMatrix rotationMatrix = null;
+
+ // A 2x1 matrix describing a translation to apply to the new scan
+ public SimpleMatrix translationVector = null;
+
+ public ScanMatcher(){
+ }
+
+ /**
+ * @brief iteratively calculate new rotation and transpose matrices to determien if the two scans match
+ * @param referenceScan the scan to be referenced
+ * @param newScan the scan that will be rotated and moved until it matches the reference scan
+ * @param iterations The number of iterations that the scan matcher will attempt
+ * @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){
+ // make a copy of the new scan so we don't modify the original
+ ScanPoint scanBeingMatched = new ScanPoint(newScan);
+
+ // calculate the rotation and translation matrices between the two scans
+ this.calculateRotationAndTranslationMatrices(referenceScan, scanBeingMatched);
+
+ 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++) {
+ // 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, 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;
+ }
+ // 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 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, CorrespondenceMatrix correspondenceMatrix){
+
+ 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 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);
+ }
+
+ /**
+ * @brief Compute the rotation and translation matrices between the new scan and the reference scan. Then cache them as private variables.
+ * 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, correspondenceMatrix);
+ SimpleSVD svd = crossCovarianceMatrixSimple.svd();
+ this.rotationMatrix = svd.getU().mult(svd.getV().transpose());
+
+ SimpleMatrix newScanAveragePosition = this.averageScanPosition(newScan);
+ SimpleMatrix referenceScanAveragePosition = this.averageScanPosition(referenceScan);
+ this.translationVector = referenceScanAveragePosition.minus(rotationMatrix.mult(newScanAveragePosition));
+ }
+
+ public SimpleMatrix getRotationMatrix(){
+ return this.rotationMatrix;
+ }
+
+ public SimpleMatrix getTranslationVector(){
+ return this.translationVector;
+ }
+
+ public ScanPoint applyRotationAndTranslationMatrices(ScanPoint newScan){
+ // copy the new scan so we don't modify the original
+ ScanPoint tempScan = new ScanPoint(newScan);
+ // apply the rotation matrix and translation vector to the new scan
+ for (int i = 0; i < tempScan.getPoints().size(); i++) {
+ Vector point = tempScan.getPoints().get(i);
+ if (point != null) {
+ SimpleMatrix pointMatrix = new SimpleMatrix(point.toArray());
+ SimpleMatrix newPointMatrix = rotationMatrix.mult(pointMatrix).plus(translationVector);
+ tempScan.getPoints().set(i, new Vector((float) newPointMatrix.get(0), (float) newPointMatrix.get(1)));
+ }
+ }
+ 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
+ // error is given as abs(Q_mean - R * P_mean)
+ // where Q_mean is the average position of the reference scan
+ // P_mean is the average position of the new scan
+ // R is the rotation matrix
+
+ SimpleMatrix newScanAveragePosition = averageScanPosition(newScan);
+ SimpleMatrix referenceScanAveragePosition = averageScanPosition(referenceScan);
+ SimpleMatrix error = referenceScanAveragePosition.minus(rotationMatrix.mult(newScanAveragePosition));
+ return (float) abs(error.elementSum());
+ }
+}
+
+/**
+ * @brief A class to hold the correspondence matrix between two scans
+ * The correspondence matrix is a 3xN matrix where N is the number of valid points in the scan.
+ * This calculates the closest point in the old scan for each point in the new scan and gets rid of redundant closest points.
+ */
+class CorrespondenceMatrix{
+ private ArrayList oldPointIndices = new ArrayList<>();
+ private ArrayList newPointIndices = new ArrayList<>();
+ private ArrayList 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 getOldPointIndices(){
+ return this.oldPointIndices;
+ }
+
+ public ArrayList getNewPointIndices(){
+ return this.newPointIndices;
+ }
+
+ public ArrayList getDistances(){
+ 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
+ * @param referenceScan the reference scan
+ */
+ private void calculateCorrespondenceMatrix(ScanPoint newScan, ScanPoint referenceScan) {
+ for (int newPointIndex = 0; newPointIndex < newScan.getPoints().size(); newPointIndex++) {
+ Vector newPoint = newScan.getPoints().get(newPointIndex);
+
+ // Skip null points in the new scan
+ if (newPoint == null) {
+ continue;
+ }
+
+ float closestDistance = Float.MAX_VALUE;
+ int closestIndex = -1;
+
+ for (int oldPointIndex = 0; oldPointIndex < referenceScan.getPoints().size(); oldPointIndex++) {
+ Vector oldPoint = referenceScan.getPoints().get(oldPointIndex);
+
+ // Skip null points in the old scan
+ if (oldPoint == null) {
+ continue;
+ }
+
+ float distance = newPoint.sub(oldPoint).mag();
+
+ if (distance < closestDistance) {
+ closestDistance = distance;
+ closestIndex = oldPointIndex;
+ }
+ }
+
+ // 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);
+// }
+ this.oldPointIndices.add(closestIndex);
+ this.newPointIndices.add(newPointIndex);
+ this.distances.add(closestDistance);
+
+ }
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/ScanGraph/ScanPoint.java b/src/ScanGraph/ScanPoint.java
index 59e915a..a0210ca 100644
--- a/src/ScanGraph/ScanPoint.java
+++ b/src/ScanGraph/ScanPoint.java
@@ -18,6 +18,17 @@ public class ScanPoint extends Vertex{
this.scan = scan;
}
+ /**
+ * @brief Copy constructor
+ * @param other The scan point to copy
+ */
+ public ScanPoint(ScanPoint other){
+ super();
+ this.position = new Vector(other.getPos().x, other.getPos().y);
+ this.orientation = other.getOrientation();
+ this.scan = new ArrayList<>(other.getPoints());
+ }
+
/**
* @return a two eleement float array containing the x and y coordinates of the vertex respectively.
*/
diff --git a/src/Vector/Vector.java b/src/Vector/Vector.java
index c388589..cf20ddc 100644
--- a/src/Vector/Vector.java
+++ b/src/Vector/Vector.java
@@ -96,10 +96,18 @@ public class Vector {
return angle;
}
+ /**
+ * @return The angle of the vector in radians
+ */
public float angle(){
return (float) atan2(y, x);
}
+ /**
+ * @brief Rotate a 2D vector by a given angle
+ * @param angle The angle to rotate the vector by in radians
+ * @return The rotated vector
+ */
public Vector rotate2D(float angle){
float distance = mag();
float currentAngle = this.angle();
diff --git a/tests/MatcherVisualizer.java b/tests/MatcherVisualizer.java
new file mode 100644
index 0000000..3895836
--- /dev/null
+++ b/tests/MatcherVisualizer.java
@@ -0,0 +1,123 @@
+import ScanGraph.ScanMatcher;
+import ScanGraph.ScanPoint;
+import Vector.Vector;
+import processing.core.PApplet;
+
+import java.util.ArrayList;
+
+public class MatcherVisualizer extends PApplet{
+
+ public static PApplet processing;
+ ScanPoint referenceScan;
+ ScanPoint scanToMatch;
+ ScanPoint scanBeingMatched;
+
+ public static void main(String[] args) {
+ PApplet.main("MatcherVisualizer");
+ }
+
+ public void settings(){
+ processing = this;
+ size(1000, 1000);
+
+ // 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);
+ this.referenceScan = appendScanPoints(scan1, scan2);
+
+ // 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);
+ this.scanToMatch = appendScanPoints(scan4, scan5);
+ this.scanBeingMatched = new ScanPoint(this.scanToMatch);
+ }
+ public void draw(){
+ iterativeScanMatch();
+// background(0);
+ }
+
+ /**
+ * @brief Generate a scan point from a scan description
+ * @param offset The offset of the scan point from the origin
+ * @param scanDescription A vector which describes the length of the line and direction of the line
+ * @return A scan point with the given offset and scan description
+ */
+ public static ScanPoint generateScanPoint(Vector offset, Vector scanDescription, int numPoints){
+ // generate a scan point with the given offset and scan description
+ ArrayList scan = new ArrayList<>();
+
+ // divide the scan description by the number of points to allow us to scale it back up in the loop
+ Vector directionVector = scanDescription.div(numPoints-1);
+
+ for (int i = 0; i < numPoints; i++) {
+ scan.add(offset.add(directionVector.mul(i)));
+ }
+
+ return new ScanPoint(new Vector(0, 0), 0, scan);
+ }
+
+ /**
+ * @brief Append two scan points together
+ * @param scan1 The first scan point to append
+ * @param scan2 The second scan point to append
+ * @return A scan point that is the combination of the two scan points
+ */
+ public static ScanPoint appendScanPoints(ScanPoint scan1, ScanPoint scan2){
+ ArrayList points = new ArrayList<>();
+ points.addAll(scan1.getPoints());
+ points.addAll(scan2.getPoints());
+ 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
+ * @param color The color to draw the scan point
+ */
+ public void drawScan(ScanPoint scan, int[] color) {
+ processing.stroke(color[0], color[1], color[2]);
+ processing.fill(color[0], color[1], color[2]);
+ ArrayList points = scan.getPoints();
+ for (int i = 0; i < points.size() - 1; i++) {
+ Vector point = points.get(i);
+ processing.ellipse(point.x, point.y, 5, 5);
+ }
+ }
+
+ public void iterativeScanMatch() {
+ background(0);
+ int[] red = {255, 0, 0};
+ int[] green = {0, 255, 0};
+ int[] blue = {0, 0, 255};
+
+
+
+ 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);
+ 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);
+
+ // do an iterative scan match and calculate the error
+// ScanPoint matchedScan = matcher.iterativeScanMatch(scan1, scan2, 0.01f, 10);
+
+// float iterativeScanMatchError = matcher.getError(scan1, matchedScan);
+ }
+}
diff --git a/tests/ScanMatcherTest.java b/tests/ScanMatcherTest.java
new file mode 100644
index 0000000..729e8ec
--- /dev/null
+++ b/tests/ScanMatcherTest.java
@@ -0,0 +1,129 @@
+import ScanGraph.ScanMatcher;
+import ScanGraph.ScanPoint;
+import org.junit.Before;
+import org.junit.jupiter.api.BeforeEach;
+import processing.core.PApplet;
+
+import org.junit.jupiter.api.Test;
+import Vector.Vector;
+
+import java.util.ArrayList;
+
+import static org.junit.jupiter.api.Assertions.*;
+import static processing.core.PApplet.main;
+
+class ScanMatcherTest{
+ /**
+ * @brief Generate a scan point from a scan description
+ * @param offset The offset of the scan point from the origin
+ * @param scanDescription A vector which describes the length of the line and direction of the line
+ * @return A scan point with the given offset and scan description
+ */
+ public ScanPoint generateScanPoint(Vector offset, Vector scanDescription, int numPoints){
+ // generate a scan point with the given offset and scan description
+ ArrayList scan = new ArrayList<>();
+
+ // divide the scan description by the number of points to allow us to scale it back up in the loop
+ Vector directionVector = scanDescription.div(numPoints-1);
+
+ for (int i = 0; i < numPoints; i++) {
+ scan.add(offset.add(directionVector.mul(i)));
+ }
+
+ return new ScanPoint(new Vector(0, 0), 0, scan);
+ }
+
+ /**
+ * @brief Append two scan points together
+ * @param scan1 The first scan point to append
+ * @param scan2 The second scan point to append
+ * @return A scan point that is the combination of the two scan points
+ */
+ public ScanPoint appendScanPoints(ScanPoint scan1, ScanPoint scan2){
+ ArrayList points = new ArrayList<>();
+ points.addAll(scan1.getPoints());
+ points.addAll(scan2.getPoints());
+ return new ScanPoint(new Vector(0, 0), 0, points);
+ }
+
+ @Test
+ public void applyRotationAndTranslationMatrices() {
+ // generate one scan that is level and another that is rotated 45 degrees.
+ Vector scanDescription = new Vector(10, 0);
+ ScanPoint referenceScan = generateScanPoint(new Vector(0, 0), scanDescription, 10);
+ ScanPoint newScan = generateScanPoint(new Vector(0, 0), scanDescription.rotate2D((float) Math.PI / 4), 10);
+
+ // calculate the rotation and translation matrices between the two scans
+ ScanMatcher matcher = new ScanMatcher();
+ matcher.calculateRotationAndTranslationMatrices(referenceScan, newScan);
+ // apply the rotation and translation matrices to the new scan
+ ScanPoint newScanWithRotationAndTranslation = matcher.applyRotationAndTranslationMatrices(newScan);
+
+ // Get the first and last points of the new scan with rotation and translation and calculate the angle between them
+ ArrayList points = newScanWithRotationAndTranslation.getPoints();
+ Vector firstPoint = points.get(0);
+ Vector lastPoint = points.get(points.size() - 1);
+ Vector rotatedDirection = lastPoint.sub(firstPoint);
+ float angle = scanDescription.angleDiff(rotatedDirection);
+
+ // The angle between the first and last points should be zero
+ assertEquals(0, angle);
+ }
+
+ @Test
+ public void getError() {
+ // generate two scans that are the same. The error should be zero.
+ ScanPoint scan1 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
+ ScanPoint scan2 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
+ ScanMatcher matcher = new ScanMatcher();
+ matcher.calculateRotationAndTranslationMatrices(scan1, scan2);
+ assertEquals(0, matcher.getError(scan1, scan2));
+
+ // generate two scans that are the same but one is offset by 10 in the y direction. The error should be 10.
+ scan1 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
+ scan2 = generateScanPoint(new Vector(0, 10), new Vector(10, 10), 12);
+ matcher.calculateRotationAndTranslationMatrices(scan1, scan2);
+ assertEquals(10, matcher.getError(scan1, scan2));
+
+ // generate two scans that are the same but one is rotated by 45 degrees. The error should be near zero.
+ scan1 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
+ scan2 = generateScanPoint(new Vector(0, 0), new Vector(10, 10).rotate2D((float) Math.PI / 4), 12);
+ matcher.calculateRotationAndTranslationMatrices(scan1, scan2);
+ assertEquals(0, matcher.getError(scan1, scan2), 0.1);
+ }
+
+ @Test
+ public void iterativeScanMatch() {
+ float bendAngle = (float) (5 * Math.PI / 9);
+ // generate two scans rotated by 45 degrees and append them together
+ ScanPoint scan1 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
+ ScanPoint scan2 = generateScanPoint(new Vector(0, 0), new Vector(10, 10).rotate2D(bendAngle), 12);
+ ScanPoint scan3 = appendScanPoints(scan1, scan2);
+
+
+ // 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);
+ ScanPoint scan4 = generateScanPoint(new Vector(10, 10), rotated, 12);
+ ScanPoint scan5 = generateScanPoint(new Vector(10, 10), rotated.rotate2D(bendAngle), 12);
+ ScanPoint scan6 = appendScanPoints(scan4, scan5);
+
+
+ // do a single scan match and calculate the error
+ ScanMatcher matcher = new ScanMatcher();
+ matcher.calculateRotationAndTranslationMatrices(scan3, scan6);
+ ScanPoint oneCalcMatch = matcher.applyRotationAndTranslationMatrices(scan6);
+ float singleScanMatchError = matcher.getError(scan3, oneCalcMatch);
+
+
+ // do an iterative scan match and calculate the error
+ 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.
+ assertNotNull(matchedScan);
+
+ float iterativeScanMatchError = matcher.getError(scan1, matchedScan);
+
+ // the iterative scan match should have a lower error than the single scan match
+ assertTrue(iterativeScanMatchError < singleScanMatchError);
+ }
+}
\ No newline at end of file