Added a physics based aproach to optimizing the pose graph

.idea/libraries/core.xml

@@ -0,0 +1,9 @@
+<component name="libraryTable">
+  <library name="core">
+    <CLASSES>
+      <root url="jar://$PROJECT_DIR$/../../processing-4.3/core/library/core.jar!/" />
+    </CLASSES>
+    <JAVADOC />
+    <SOURCES />
+  </library>
+</component>

src/Graph/Edge.java

@@ -10,7 +10,7 @@ public class Edge {
      * @param vEnd the vertex the edge ends at
      * @param weight the weight of the edge
      */
-    Edge(Vertex vStart, Vertex vEnd, float weight){
+    public Edge(Vertex vStart, Vertex vEnd, float weight){
         this.vStart = vStart;
         this.vEnd = vEnd;
         this.weight = weight;
@@ -20,7 +20,7 @@ public class Edge {
      * @param vStart the vertex the edge starts at
      * @param vEnd the vertex the edge ends at
      */
-    protected Edge(Vertex vStart, Vertex vEnd){
+    public Edge(Vertex vStart, Vertex vEnd){
         this.vStart = vStart;
         this.vEnd = vEnd;
     }

src/ScanGraph/ScanEdge.java

@@ -8,50 +8,40 @@ import processing.core.PApplet;
 
 import static java.lang.Math.PI;
 
-public class ScanEdge extends Edge implements LineInterface {
+public class ScanEdge extends Edge {
-
-    protected ScanPoint vStart;
-    protected ScanPoint vEnd;
     protected Line line;
+
+    // Additional properties specific to scan edges
+    private boolean isLoopClosure = false;
+
+    /**
+     * @brief Constructor for a scan edge
+     * @param vStart the starting vertex
+     * @param vEnd   the ending vertex
+     */
     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();
+     * @brief Constructor for a scan edge
+     * @param vStart the starting vertex
+     * @param vEnd   the ending vertex
+     * @param weight the weight of the edge
+     */
+    public ScanEdge(ScanPoint vStart, ScanPoint vEnd, float weight) {
+        super(vStart, vEnd, weight);
+        this.line = new Line(vStart.getPos(), vEnd.getPos());
     }
 
-    public Vector getPosition(){
+    // Getter and setter for loop closure flag
-        return line.getPosition();
+    public boolean isLoopClosure() {
+        return isLoopClosure;
     }
 
-    public float getLength(){
+    public void setLoopClosure(boolean loopClosure) {
-        return line.getLength();
+        isLoopClosure = loopClosure;
-    }
-
-    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);
     }
 }

src/ScanGraph/ScanGraph.java

@@ -1,6 +1,7 @@
 package ScanGraph;
 
 import Graph.Graph;
+import Graph.Edge;
 import Graph.Vertex;
 import Vector.Vector;
 import org.ejml.simple.SimpleMatrix;
@@ -16,12 +17,26 @@ public class ScanGraph extends Graph {
         this.lastPoint = startingPoint;
     }
 
-    public void addEdge(ScanPoint vEnd) {
+    /**
-        addVertex(vEnd);
+     * @brief Add a new scan to the graph
-        ScanEdge edge = new ScanEdge(this.lastPoint, vEnd);
+     * @param newScan the new scan to add
-        adjList.get((Vertex) this.lastPoint).add(edge);
+     */
+    public void addScan(ScanPoint newScan) {
+        addVertex(newScan);
 
-        this.lastPoint = vEnd;
+        // check if the new scan matches any of the existing scans
+        MatchedScanTransform matchedScan = getAssociatedScan(newScan);
+        if (matchedScan.scan != null) {
+            // if it does, add a loop closure constraint
+            addLoopClosureConstraint(this.lastPoint, matchedScan);
+        }
+        else{
+            // if it doesn't match anything else, add an edge between the last scan and the new scan
+            ScanEdge edge = new ScanEdge(this.lastPoint, newScan);
+            adjList.get((Vertex) this.lastPoint).add(edge);
+        }
+
+        this.lastPoint = newScan;
     }
 
     /**
@@ -29,18 +44,127 @@ public class ScanGraph extends Graph {
      * @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) {
+    private MatchedScanTransform 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;
-            matchedScan = matcher.iterativeScanMatch(referenceScan, newScan, 0.1F, 10);
+            matchedScan = matcher.iterativeScanMatch(referenceScan, newScan, 0.00001F, 5);
 
             if(matchedScan != null){
+                // apply the transformation to the new scan
                 break;
             }
         }
-        return matchedScan;
+
+        return new MatchedScanTransform(matcher.getRotationMatrix(), matcher.getTranslationVector(), matchedScan);
+    }
+
+    /**
+     * @param referenceScan the existing scan in the graph
+     * @param matchedScan the new scan that matches the reference scan
+     */
+    private void addLoopClosureConstraint(ScanPoint referenceScan, MatchedScanTransform matchedScan) {
+        // Compute relative transformation between referenceScan and newScan
+        // You need to implement the logic for computing the relative transformation
+
+        // Create a loop closure edge and add it to the graph
+        ScanEdge loopClosureEdge = new ScanEdge(referenceScan, matchedScan.scan);
+        loopClosureEdge.setLoopClosure(true);  // Mark the edge as a loop closure
+        adjList.get(referenceScan).add(loopClosureEdge);
+
+        // Optimize the graph after adding the loop closure constraint
+        optimizeGraph();
+    }
+
+    /**
+     * Perform graph optimization using the Levenberg-Marquardt algorithm
+     */
+    private void optimizeGraph() {
+        // Create a matrix for pose parameters (you may need to adjust the size based on your requirements)
+        int numPoses = adjList.size();
+        int poseDim = 3; // Assuming 3D poses
+        SimpleMatrix poses = new SimpleMatrix(numPoses * poseDim, 1);
+
+        // Populate the poses matrix with current pose estimates from the graph
+        int i = 0;
+        for (Vertex v : adjList.keySet()) {
+            ScanPoint scan = (ScanPoint) v;
+            Vector poseVector = scan.getPos(); // You need to implement the method to get the pose vector
+            poses.set(i++, 0, poseVector.x);
+            poses.set(i++, 0, poseVector.y);
+            poses.set(i++, 0, scan.getAngle());
+        }
+
+        // TODO: Create a matrix for loop closure constraints (you need to implement this)
+        SimpleMatrix loopClosureConstraints = computeLoopClosureConstraints();
+
+        // Use Levenberg-Marquardt optimization to adjust poses
+        SimpleSVD<SimpleMatrix> svd = poses.svd();
+        SimpleMatrix U = svd.getU();
+        SimpleMatrix S = svd.getW();
+        SimpleMatrix Vt = svd.getV().transpose();
+
+        // Adjust poses using loop closure constraints
+        SimpleMatrix adjustment = Vt.mult(loopClosureConstraints).mult(U.transpose())
+                .scale(0.01); // Adjust this factor based on your problem
+
+        // Update the poses in the graph and handle loop closure edges
+        i = 0;
+        for (Vertex v : adjList.keySet()) {
+            ScanPoint scan = (ScanPoint) v;
+            scan.setPos(new Vector(adjustment.get(i++, 0), adjustment.get(i++, 0), adjustment.get(i++, 0)));
+
+            // TODO: Handle loop closure edges
+            for (Edge edge : adjList.get(v)) {
+                ScanEdge scanEdge = (ScanEdge) edge;
+                if (scanEdge.isLoopClosure()) {
+                    // Update any additional information specific to loop closure edges
+                    // For example, you might update the weight or perform other adjustments
+                    // based on the loop closure information
+                }
+            }
+        }
+    }
+
+    /**
+     * @return Matrix representing loop closure constraints (you need to implement this)
+     */
+    private SimpleMatrix computeLoopClosureConstraints() {
+        // Implement the logic to compute loop closure constraints
+        // This matrix should represent the relative transformation between loop closure nodes
+        // It may involve iterating through loop closure edges and extracting relevant information
+        // You may use a similar structure to the poses matrix
+        // For simplicity, this example assumes a 3D pose with translation only
+        int numPoses = adjList.size();
+        int poseDim = 3;
+        SimpleMatrix loopClosureConstraints = new SimpleMatrix(numPoses * poseDim, 1);
+
+        // Populate loopClosureConstraints matrix with relevant information
+
+        return loopClosureConstraints;
+    }
+}
+
+/**
+ * @brief struct to hold the rotation and translation between two scans
+ */
+class MatchedScanTransform{
+    public SimpleMatrix rotation;
+    public SimpleMatrix translation;
+
+    public ScanPoint scan;
+
+    /**
+     * @brief constructor
+     * @param rotation the rotation between the two scans
+     * @param translation the translation between the two scans
+     * @param scan the scan that was matched
+     */
+    MatchedScanTransform(SimpleMatrix rotation, SimpleMatrix translation, ScanPoint scan){
+        this.rotation = rotation;
+        this.translation = translation;
+        this.scan = scan;
     }
 }

src/ScanGraph/ScanMatcher.java

@@ -149,6 +149,7 @@ public class ScanMatcher{
                 tempScan.getPoints().set(i, new Vector((float) newPointMatrix.get(0), (float) newPointMatrix.get(1)));
             }
         }
+        newScan.UpdatePose(rotationMatrix, translationVector);
         return tempScan;
     }
 

src/ScanGraph/ScanPoint.java

@@ -2,6 +2,7 @@ package ScanGraph;
 
 import Graph.Vertex;
 import Vector.Vector;
+import org.ejml.simple.SimpleMatrix;
 
 import java.util.ArrayList;
 
@@ -25,7 +26,7 @@ public class ScanPoint extends Vertex{
     public ScanPoint(ScanPoint other){
         super();
         this.position = new Vector(other.getPos().x, other.getPos().y);
-        this.orientation = other.getOrientation();
+        this.orientation = other.getAngle();
         this.scan = new ArrayList<>(other.getPoints());
     }
 
@@ -36,7 +37,11 @@ public class ScanPoint extends Vertex{
         return position;
     }
 
-    public float getOrientation(){
+    public void setPos(Vector pos){
+        this.position = pos;
+    }
+
+    public float getAngle(){
         return this.orientation;
     }
 
@@ -44,4 +49,20 @@ public class ScanPoint extends Vertex{
         return this.scan;
     }
 
+    /**
+     * @brief Update the pose of the scan point
+     * @param rotation The rotation matrix to apply to the scan point
+     * @param translation The translation matrix to apply to the scan point
+     */
+    public void UpdatePose(SimpleMatrix rotation, SimpleMatrix translation){
+        SimpleMatrix pose = new SimpleMatrix(3,1);
+        pose.set(0,0, this.position.x);
+        pose.set(1,0, this.position.y);
+        pose.set(2,0, this.orientation);
+        SimpleMatrix newPose = translation.plus(rotation.mult(pose));
+        this.position.x = (float)newPose.get(0,0);
+        this.position.y = (float)newPose.get(1,0);
+        this.orientation = (float)newPose.get(2,0);
+    }
+
 }

src/Vector/Vector.java

@@ -24,6 +24,17 @@ public class Vector {
         this.z = z;
     }
 
+    public Vector(double x, double y){
+        this.x = (float)x;
+        this.y = (float)y;
+    }
+
+    public Vector(double x, double y, double z){
+        this.x = (float)x;
+        this.y = (float)y;
+        this.z = (float)z;
+    }
+
     public Vector(SimpleMatrix matrix){
         // initialize x,y if matrix is 2x1 and x,y,z if matrix is 3x1
         if(matrix.getNumRows() == 2){