diff --git a/src/Graph/Edge.java b/src/Graph/Edge.java index df11d03..9045ed9 100644 --- a/src/Graph/Edge.java +++ b/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; } diff --git a/src/ScanGraph/ScanEdge.java b/src/ScanGraph/ScanEdge.java index d892f24..f3487e2 100644 --- a/src/ScanGraph/ScanEdge.java +++ b/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 { - - protected ScanPoint vStart; - protected ScanPoint vEnd; +public class ScanEdge extends Edge { 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(){ - return line.getPosition(); + // Getter and setter for loop closure flag + public boolean isLoopClosure() { + return isLoopClosure; } - 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); + public void setLoopClosure(boolean loopClosure) { + isLoopClosure = loopClosure; } } diff --git a/src/ScanGraph/ScanGraph.java b/src/ScanGraph/ScanGraph.java index 71c89cd..deabf9a 100644 --- a/src/ScanGraph/ScanGraph.java +++ b/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); - ScanEdge edge = new ScanEdge(this.lastPoint, vEnd); - adjList.get((Vertex) this.lastPoint).add(edge); + /** + * @brief Add a new scan to the graph + * @param newScan the new scan to add + */ + 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 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; + } +} \ No newline at end of file diff --git a/src/ScanGraph/ScanMatcher.java b/src/ScanGraph/ScanMatcher.java index 0bc5c46..ce53680 100644 --- a/src/ScanGraph/ScanMatcher.java +++ b/src/ScanGraph/ScanMatcher.java @@ -134,6 +134,7 @@ public class ScanMatcher{ tempScan.getPoints().set(i, new Vector((float) newPointMatrix.get(0), (float) newPointMatrix.get(1))); } } + newScan.UpdatePose(rotationMatrix, translationVector); return tempScan; } diff --git a/src/ScanGraph/ScanPoint.java b/src/ScanGraph/ScanPoint.java index a0210ca..e4b458c 100644 --- a/src/ScanGraph/ScanPoint.java +++ b/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); + } + } diff --git a/src/Vector/Vector.java b/src/Vector/Vector.java index cf20ddc..f364d45 100644 --- a/src/Vector/Vector.java +++ b/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){