diff --git a/.idea/libraries/ejml.xml b/.idea/libraries/ejml.xml
new file mode 100644
index 0000000..2eb6e2d
--- /dev/null
+++ b/.idea/libraries/ejml.xml
@@ -0,0 +1,13 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
index cf9abe6..b95853c 100644
--- a/.idea/misc.xml
+++ b/.idea/misc.xml
@@ -1,4 +1,3 @@
-
diff --git a/SLAM-Sim.iml b/SLAM-Sim.iml
index 42bad0a..8b189e9 100644
--- a/SLAM-Sim.iml
+++ b/SLAM-Sim.iml
@@ -43,5 +43,6 @@
+
\ No newline at end of file
diff --git a/lib/ejml-v0.42-libs/ejml-cdense-0.42-sources.jar b/lib/ejml-v0.42-libs/ejml-cdense-0.42-sources.jar
new file mode 100644
index 0000000..ebaaca0
Binary files /dev/null and b/lib/ejml-v0.42-libs/ejml-cdense-0.42-sources.jar differ
diff --git a/lib/ejml-v0.42-libs/ejml-cdense-0.42.jar b/lib/ejml-v0.42-libs/ejml-cdense-0.42.jar
new file mode 100644
index 0000000..1e1d343
Binary files /dev/null and b/lib/ejml-v0.42-libs/ejml-cdense-0.42.jar differ
diff --git a/lib/ejml-v0.42-libs/ejml-core-0.42-sources.jar b/lib/ejml-v0.42-libs/ejml-core-0.42-sources.jar
new file mode 100644
index 0000000..48795ba
Binary files /dev/null and b/lib/ejml-v0.42-libs/ejml-core-0.42-sources.jar differ
diff --git a/lib/ejml-v0.42-libs/ejml-core-0.42.jar b/lib/ejml-v0.42-libs/ejml-core-0.42.jar
new file mode 100644
index 0000000..60f40d1
Binary files /dev/null and b/lib/ejml-v0.42-libs/ejml-core-0.42.jar differ
diff --git a/lib/ejml-v0.42-libs/ejml-ddense-0.42-sources.jar b/lib/ejml-v0.42-libs/ejml-ddense-0.42-sources.jar
new file mode 100644
index 0000000..d964cb0
Binary files /dev/null and b/lib/ejml-v0.42-libs/ejml-ddense-0.42-sources.jar differ
diff --git a/lib/ejml-v0.42-libs/ejml-ddense-0.42.jar b/lib/ejml-v0.42-libs/ejml-ddense-0.42.jar
new file mode 100644
index 0000000..25a6d1a
Binary files /dev/null and b/lib/ejml-v0.42-libs/ejml-ddense-0.42.jar differ
diff --git a/lib/ejml-v0.42-libs/ejml-dsparse-0.42-sources.jar b/lib/ejml-v0.42-libs/ejml-dsparse-0.42-sources.jar
new file mode 100644
index 0000000..59f9c73
Binary files /dev/null and b/lib/ejml-v0.42-libs/ejml-dsparse-0.42-sources.jar differ
diff --git a/lib/ejml-v0.42-libs/ejml-dsparse-0.42.jar b/lib/ejml-v0.42-libs/ejml-dsparse-0.42.jar
new file mode 100644
index 0000000..fb4c6f6
Binary files /dev/null and b/lib/ejml-v0.42-libs/ejml-dsparse-0.42.jar differ
diff --git a/lib/ejml-v0.42-libs/ejml-experimental-0.42-sources.jar b/lib/ejml-v0.42-libs/ejml-experimental-0.42-sources.jar
new file mode 100644
index 0000000..733e3be
Binary files /dev/null and b/lib/ejml-v0.42-libs/ejml-experimental-0.42-sources.jar differ
diff --git a/lib/ejml-v0.42-libs/ejml-experimental-0.42.jar b/lib/ejml-v0.42-libs/ejml-experimental-0.42.jar
new file mode 100644
index 0000000..2163349
Binary files /dev/null and b/lib/ejml-v0.42-libs/ejml-experimental-0.42.jar differ
diff --git a/lib/ejml-v0.42-libs/ejml-fdense-0.42-sources.jar b/lib/ejml-v0.42-libs/ejml-fdense-0.42-sources.jar
new file mode 100644
index 0000000..74c5da1
Binary files /dev/null and b/lib/ejml-v0.42-libs/ejml-fdense-0.42-sources.jar differ
diff --git a/lib/ejml-v0.42-libs/ejml-fdense-0.42.jar b/lib/ejml-v0.42-libs/ejml-fdense-0.42.jar
new file mode 100644
index 0000000..45e7516
Binary files /dev/null and b/lib/ejml-v0.42-libs/ejml-fdense-0.42.jar differ
diff --git a/lib/ejml-v0.42-libs/ejml-simple-0.42-sources.jar b/lib/ejml-v0.42-libs/ejml-simple-0.42-sources.jar
new file mode 100644
index 0000000..f1f3aec
Binary files /dev/null and b/lib/ejml-v0.42-libs/ejml-simple-0.42-sources.jar differ
diff --git a/lib/ejml-v0.42-libs/ejml-simple-0.42.jar b/lib/ejml-v0.42-libs/ejml-simple-0.42.jar
new file mode 100644
index 0000000..3bca512
Binary files /dev/null and b/lib/ejml-v0.42-libs/ejml-simple-0.42.jar differ
diff --git a/lib/ejml-v0.42-libs/ejml-zdense-0.42-sources.jar b/lib/ejml-v0.42-libs/ejml-zdense-0.42-sources.jar
new file mode 100644
index 0000000..194bf14
Binary files /dev/null and b/lib/ejml-v0.42-libs/ejml-zdense-0.42-sources.jar differ
diff --git a/lib/ejml-v0.42-libs/ejml-zdense-0.42.jar b/lib/ejml-v0.42-libs/ejml-zdense-0.42.jar
new file mode 100644
index 0000000..d765f2a
Binary files /dev/null and b/lib/ejml-v0.42-libs/ejml-zdense-0.42.jar differ
diff --git a/src/Graph/Edge.java b/src/Graph/Edge.java
index fc87508..df11d03 100644
--- a/src/Graph/Edge.java
+++ b/src/Graph/Edge.java
@@ -20,7 +20,7 @@ public class Edge {
* @param vStart the vertex the edge starts at
* @param vEnd the vertex the edge ends at
*/
- Edge(Vertex vStart, Vertex vEnd){
+ protected Edge(Vertex vStart, Vertex vEnd){
this.vStart = vStart;
this.vEnd = vEnd;
}
diff --git a/src/ScanGraph/ScanEdge.java b/src/ScanGraph/ScanEdge.java
new file mode 100644
index 0000000..d892f24
--- /dev/null
+++ b/src/ScanGraph/ScanEdge.java
@@ -0,0 +1,57 @@
+package ScanGraph;
+
+import Graph.Edge;
+import Vector.Line;
+import Vector.LineInterface;
+import Vector.Vector;
+import processing.core.PApplet;
+
+import static java.lang.Math.PI;
+
+public class ScanEdge extends Edge implements LineInterface {
+
+ protected ScanPoint vStart;
+ protected ScanPoint vEnd;
+ protected Line line;
+ public ScanEdge(ScanPoint vStart, ScanPoint vEnd){
+ super(vStart, vEnd);
+ this.vStart = vStart;
+ this.vEnd = vEnd;
+
+ this.line = new Line(vStart.getPos(), vEnd.getPos());
+ }
+
+ public Vector getDirection(){
+ return line.getDirection();
+ }
+
+ public Vector getPosition(){
+ return line.getPosition();
+ }
+
+ public float getLength(){
+ return line.getLength();
+ }
+
+ public float getAngle(){
+ return line.getAngle();
+ }
+
+ public Vector endPoint(){
+ return line.endPoint();
+ }
+
+ public float getDistance(Vector point){
+ return line.getDistance(point);
+ }
+
+ public void draw(PApplet proc){
+ line.draw(proc);
+ Vector leftFlange = line.getDirection().rotate2D((float)(-3*PI/4)).normalize().mul(20);
+ Vector rightFlange = line.getDirection().rotate2D((float) (3*PI/4)).normalize().mul(20);
+ Line l1 = new Line(line.endPoint(), line.endPoint().add(leftFlange));
+ Line l2 = new Line(line.endPoint(), line.endPoint().add(rightFlange));
+ l1.draw(proc);
+ l2.draw(proc);
+ }
+}
diff --git a/src/ScanGraph/ScanGraph.java b/src/ScanGraph/ScanGraph.java
new file mode 100644
index 0000000..46315c1
--- /dev/null
+++ b/src/ScanGraph/ScanGraph.java
@@ -0,0 +1,178 @@
+package ScanGraph;
+
+import Graph.Graph;
+import Graph.Vertex;
+import Vector.Vector;
+
+import java.util.ArrayList;
+
+public class ScanGraph extends Graph{
+
+ ScanPoint lastPoint;
+ public ScanGraph(ScanPoint startingPoint){
+ super();
+ this.lastPoint = startingPoint;
+ }
+
+ public void addEdge(ScanPoint vEnd){
+ addVertex(vEnd);
+ ScanEdge edge = new ScanEdge(this.lastPoint, vEnd);
+ adjList.get((Vertex) this.lastPoint).add(edge);
+
+ this.lastPoint = vEnd;
+ }
+
+ /**
+ * @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.
+ */
+ private ScanPoint getAssociatedScan(ScanPoint newScan) {
+
+ // 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;
+ // p is the newScan and q is the referenceScan
+ CorrespondenceMatrix correspondenceMatrix = new CorrespondenceMatrix(newScan, referenceScan);
+
+ // compute the average position of the new scan
+ Vector averagePosition = new Vector(0, 0);
+ int invalidPoints = 0;
+ for (Vector point : newScan.getScan()) {
+ if (point != null) {
+ averagePosition = averagePosition.add(point);
+ }
+ else{
+ invalidPoints++;
+ }
+ }
+ averagePosition = averagePosition.div(newScan.getScan().size() - invalidPoints);
+
+ // compute the average position of the reference scan
+ Vector averageReferencePosition = new Vector(0, 0);
+ invalidPoints = 0;
+ for (Vector point : referenceScan.getScan()) {
+ if (point != null) {
+ averageReferencePosition = averageReferencePosition.add(point);
+ }
+ else{
+ invalidPoints++;
+ }
+ }
+ averageReferencePosition = averageReferencePosition.div(referenceScan.getScan().size() - invalidPoints);
+
+ // 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.getScan().get(oldIndex);
+ Vector newPoint = newScan.getScan().get(newIndex);
+ if (oldPoint != null && newPoint != null) {
+ Vector oldPointCentered = oldPoint.sub(averageReferencePosition);
+ Vector newPointCentered = newPoint.sub(averagePosition);
+ 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;
+ }
+ }
+
+ // compute the single value decomposition of the cross covariance matrix
+
+ }
+
+ return null;
+ }
+
+ private void singleValueDecomposition(float[][] matrix){
+ // compute the single value decomposition of the matrix
+
+ // matrix multiply the matrix by its transpose
+
+ }
+
+}
+
+
+/**
+ * @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
+ */
+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;
+ }
+
+ 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.getScan().size(); newPointIndex++) {
+ Vector newPoint = newScan.getScan().get(newPointIndex);
+ if (newPoint == null) {
+ continue;
+ }
+ float closestDistance = Float.MAX_VALUE;
+ int closestIndex = -1;
+ for (int j = 0; j < referenceScan.getScan().size(); j++) {
+ Vector oldPoint = referenceScan.getScan().get(j);
+ 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);
+ }
+ }
+ }
+ }
+}
diff --git a/src/ScanGraph/PointScan.java b/src/ScanGraph/ScanPoint.java
similarity index 59%
rename from src/ScanGraph/PointScan.java
rename to src/ScanGraph/ScanPoint.java
index 423b46c..303756b 100644
--- a/src/ScanGraph/PointScan.java
+++ b/src/ScanGraph/ScanPoint.java
@@ -1,31 +1,23 @@
package ScanGraph;
-import Graph.PointGraph;
import Graph.Vertex;
import Vector.Vector;
-import processing.core.PApplet;
import java.util.ArrayList;
-public class PointScan extends Vertex{
+public class ScanPoint extends Vertex{
private Vector position;
+ private Vector orientation;
private ArrayList scan;
- PointScan(Vector scanPosition, ArrayList scan){
+ ScanPoint(Vector scanPosition, Vector orientation, ArrayList scan) {
super();
this.position = scanPosition;
+ this.orientation = orientation;
this.scan = scan;
}
- /**
- * @param x the new x position of the vertex
- * @param y the new y posiiton of the vertex
- */
- public void setPos(float x, float y){
- this.position = new Vector(x, y);
- }
-
/**
* @return a two eleement float array containing the x and y coordinates of the vertex respectively.
*/
@@ -33,6 +25,10 @@ public class PointScan extends Vertex{
return position;
}
+ public Vector getOrientation(){
+ return this.orientation;
+ }
+
public ArrayList getScan(){
return this.scan;
}