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

Laying the foundation for a good graph.

Browse Source
This commit is contained in:
Quinn
2023-12-11 18:55:23 -05:00
parent f7fd6fc6a8
commit 2d55388f24
6 changed files with 193 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/Graph/Edge.java
View File

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

58
src/ScanGraph/ScanEdge.java
View File

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

140
src/ScanGraph/ScanGraph.java
View File

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

1
src/ScanGraph/ScanMatcher.java
View File

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

25
src/ScanGraph/ScanPoint.java
View File

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

11
src/Vector/Vector.java
View File

@@ -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){