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

Began writing tests for the scan matcher.

Browse Source
This commit is contained in:
Quinn
2023-11-26 15:34:24 -05:00
parent f59b9b9094
commit 6a7d3eeffc
5 changed files with 313 additions and 210 deletions
Download Patch File Download Diff File Expand all files Collapse all files

208
src/ScanGraph/ScanGraph.java
View File

@@ -10,6 +10,7 @@ import java.util.ArrayList;
public class ScanGraph extends Graph {
ScanPoint lastPoint;
public ScanGraph(ScanPoint startingPoint) {
super();
this.lastPoint = startingPoint;
@@ -24,9 +25,9 @@ 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();
@@ -51,209 +52,4 @@ public class ScanGraph extends Graph{
}
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<Integer> oldPointIndices = new ArrayList<>();
private ArrayList<Integer> newPointIndices = new ArrayList<>();
private ArrayList<Float> distances = new ArrayList<>();
CorrespondenceMatrix(ScanPoint newScan, ScanPoint oldScan){
this.calculateCorrespondenceMatrix(newScan, oldScan);
}
public ArrayList<Integer> getOldPointIndices(){
return this.oldPointIndices;
}
public ArrayList<Integer> getNewPointIndices(){
return this.newPointIndices;
}
public ArrayList<Float> 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<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
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);
}
}
}
}
}
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<SimpleMatrix> 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();
}
}

217
src/ScanGraph/ScanMatcher.java Normal file
View File

@@ -0,0 +1,217 @@
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
*/
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<SimpleMatrix> 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){
// 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;
}
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<Integer> oldPointIndices = new ArrayList<>();
private ArrayList<Integer> newPointIndices = new ArrayList<>();
private ArrayList<Float> distances = new ArrayList<>();
CorrespondenceMatrix(ScanPoint newScan, ScanPoint oldScan){
this.calculateCorrespondenceMatrix(newScan, oldScan);
}
public ArrayList<Integer> getOldPointIndices(){
return this.oldPointIndices;
}
public ArrayList<Integer> getNewPointIndices(){
return this.newPointIndices;
}
public ArrayList<Float> 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<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
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);
}
}
}
}
}

11
src/ScanGraph/ScanPoint.java
View File

@@ -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.
*/

8
src/Vector/Vector.java
View File

@@ -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();

71
tests/ScanGraph/ScanMatcherTest.java Normal file
View File

@@ -0,0 +1,71 @@
package ScanGraph;
import org.junit.jupiter.api.Test;
import Vector.Vector;
import java.lang.reflect.Array;
import java.util.ArrayList;
import static org.junit.jupiter.api.Assertions.*;
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
*/
ScanPoint generateScanPoint(Vector offset, Vector scanDescription){
// 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;
// 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));
}
return new ScanPoint(scanPosition, 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));
// 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<Vector> points = newScanWithRotationAndTranslation.getPoints();
Vector firstPoint = points.get(0);
Vector lastPoint = points.get(points.size() - 1);
float angle = firstPoint.angleDiff(lastPoint);
// The angle between the first and last points should be zero
assertEquals(0, angle);
}
@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));
ScanMatcher matcher = new ScanMatcher();
matcher.calculateRotationAndTranslationMatrices(scan1, scan2);
assertEquals(0, matcher.getError(scan1, scan2));
}
}