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

Added some unit tests for the ScanMatcher and fixed some broken functionality.

Browse Source
This commit is contained in:
quinn
2023-11-29 20:44:54 -05:00
parent 6a7d3eeffc
commit df57253287
5 changed files with 74 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.idea/misc.xml generated
View File

@@ -1,5 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectRootManager" version="2" languageLevel="JDK_20" default="true" project-jdk-name="openjdk-20" project-jdk-type="JavaSDK">
<component name="ProjectRootManager" version="2" languageLevel="JDK_21" default="true" project-jdk-name="openjdk-20" project-jdk-type="JavaSDK">
<output url="file://$PROJECT_DIR$/out" />
</component>
</project>

2
SLAM-Sim.iml
View File

@@ -11,7 +11,7 @@
<orderEntry type="module-library" exported="">
<library>
<CLASSES>
<root url="jar:///opt/processing/processing-4.2/core/library/core.jar!/" />
<root url="jar://$USER_HOME$/Documents/processing-4.3/core/library/core.jar!/" />
</CLASSES>
<JAVADOC />
<SOURCES />

19
src/ScanGraph/ScanGraph.java
View File

@@ -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;
}
}

83
src/ScanGraph/ScanMatcher.java
View File

@@ -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<ArrayList<Float>> correspondenceMatrix = new ArrayList<ArrayList<Float>>();
correspondenceMatrix.add(new ArrayList<Float>());
correspondenceMatrix.add(new ArrayList<Float>());
correspondenceMatrix.add(new ArrayList<Float>());
// 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);
}
}
}

38
tests/ScanGraph/ScanMatcherTest.java
View File

@@ -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<Vector> 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<Vector> 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
}
}