diff --git a/.idea/misc.xml b/.idea/misc.xml
index b95853c..cbbff6a 100644
--- a/.idea/misc.xml
+++ b/.idea/misc.xml
@@ -1,5 +1,6 @@
+
-
+
\ No newline at end of file
diff --git a/SLAM-Sim.iml b/SLAM-Sim.iml
index 8b189e9..52325fd 100644
--- a/SLAM-Sim.iml
+++ b/SLAM-Sim.iml
@@ -11,7 +11,7 @@
-
+
diff --git a/src/ScanGraph/ScanGraph.java b/src/ScanGraph/ScanGraph.java
index baa5ba8..71c89cd 100644
--- a/src/ScanGraph/ScanGraph.java
+++ b/src/ScanGraph/ScanGraph.java
@@ -31,25 +31,16 @@ public class ScanGraph extends 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;
- }
+ if(matchedScan != null){
+ break;
}
}
- return null;
+ return matchedScan;
}
}
diff --git a/src/ScanGraph/ScanMatcher.java b/src/ScanGraph/ScanMatcher.java
index 8971aec..dc180ed 100644
--- a/src/ScanGraph/ScanMatcher.java
+++ b/src/ScanGraph/ScanMatcher.java
@@ -13,14 +13,40 @@ import static java.lang.Math.abs;
*/
class ScanMatcher{
// A 2x2 matrix describing a rotation to apply to the new scan
- SimpleMatrix rotationMatrix;
+ public SimpleMatrix rotationMatrix = null;
// A 2x1 matrix describing a translation to apply to the new scan
- SimpleMatrix translationVector;
+ public SimpleMatrix translationVector = null;
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){
+ for (int i = 0; i < iterations; i++) {
+ // calculate the rotation and translation matrices between the new scan and the reference scan
+ this.calculateRotationAndTranslationMatrices(referenceScan, newScan);
+
+ // update the new scan with the rotation matrix and translation vector
+ newScan = this.applyRotationAndTranslationMatrices(newScan);
+
+ // calculate the error between the new scan and the reference scan
+ float error = this.getError(referenceScan, newScan);
+
+ // if the error is less than some threshold, then we have found a match
+ if (error < errorThreshold) {
+ return referenceScan;
+ }
+ }
+
+ return null;
+ }
/**
* @brief Compute the average position of the scan
* @param scan the scan to compute the average position of
@@ -159,58 +185,39 @@ class CorrespondenceMatrix{
* @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
+ private void calculateCorrespondenceMatrix(ScanPoint newScan, ScanPoint referenceScan) {
for (int newPointIndex = 0; newPointIndex < newScan.getPoints().size(); newPointIndex++) {
Vector newPoint = newScan.getPoints().get(newPointIndex);
- // if the new point is null, then skip it
+
+ // Skip null points in the new scan
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
+
+ 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 = j;
+ closestIndex = oldPointIndex;
}
}
- // 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
+
+ // Add the correspondence if a closest point is found
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);
- }
+ this.oldPointIndices.add(closestIndex);
+ this.newPointIndices.add(newPointIndex);
+ this.distances.add(closestDistance);
}
}
}
diff --git a/tests/ScanGraph/ScanMatcherTest.java b/tests/ScanGraph/ScanMatcherTest.java
index d5f75a7..b77c90b 100644
--- a/tests/ScanGraph/ScanMatcherTest.java
+++ b/tests/ScanGraph/ScanMatcherTest.java
@@ -1,5 +1,6 @@
package ScanGraph;
+import org.ejml.simple.SimpleMatrix;
import org.junit.jupiter.api.Test;
import Vector.Vector;
@@ -16,32 +17,29 @@ class ScanMatcherTest {
* @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
*/
- ScanPoint generateScanPoint(Vector offset, Vector scanDescription){
+ ScanPoint generateScanPoint(Vector offset, Vector scanDescription, int numPoints){
// generate a scan point with the given offset and scan description
- Vector scanPosition = new Vector(0, 0);
ArrayList scan = new ArrayList<>();
- // calculate the total number of points in the scan
- int numPoints = (int) scanDescription.mag();
- // calculate the slope of the line the scan is on
- float m = scanDescription.y / scanDescription.x;
+ // 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);
- // add the points to the scan
- for(int i = 0; i < numPoints; i++){
- float x = i;
- float y = m * x;
- scan.add(new Vector(x + offset.x, y + offset.y));
+ for (int i = 0; i < numPoints; i++) {
+ scan.add(offset.add(directionVector.mul(i)));
}
- return new ScanPoint(scanPosition, 0, scan);
+ return new ScanPoint(new Vector(0, 0), 0, scan);
}
@Test
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);
- ScanPoint newScan = generateScanPoint(new Vector(0, 0), scanDescription.rotate2D((float) Math.PI / 4));
+ ScanPoint referenceScan = generateScanPoint(new Vector(0, 0), scanDescription, 10);
+ ScanPoint newScan = generateScanPoint(new Vector(0, 0), scanDescription.rotate2D((float) Math.PI / 4), 10);
+
+ Vector test = scanDescription.rotate2D((float) Math.PI / 4);
+ float mag = test.mag();
// calculate the rotation and translation matrices between the two scans
ScanMatcher matcher = new ScanMatcher();
@@ -53,7 +51,8 @@ class ScanMatcherTest {
ArrayList points = newScanWithRotationAndTranslation.getPoints();
Vector firstPoint = points.get(0);
Vector lastPoint = points.get(points.size() - 1);
- float angle = firstPoint.angleDiff(lastPoint);
+ Vector rotatedDirection = lastPoint.sub(firstPoint);
+ float angle = scanDescription.angleDiff(rotatedDirection);
// The angle between the first and last points should be zero
assertEquals(0, angle);
@@ -62,10 +61,15 @@ class ScanMatcherTest {
@Test
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));
- ScanPoint scan2 = generateScanPoint(new Vector(0, 0), new Vector(10, 10));
+ 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));
}
+
+ @Test
+ void iterativeScanMatch() {
+ // TODO: Write a test for this
+ }
}
\ No newline at end of file