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

Further tweaks to RANSAC algorithm and line of best fit

Browse Source
This commit is contained in:
Quinn
2023-05-03 16:22:38 -05:00
parent 62e7232435
commit bda601d326
8 changed files with 232 additions and 122 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/Car.java
View File

@@ -38,11 +38,13 @@ public class Car{
}
//draw the car and its views
public void drawCar(PointGraph g){
public void drawCar(PointGraph map, boolean SLAMIsHidden){
proc.stroke(255);
proc.ellipse(pose.x, pose.y, carWidth, carLength);
this.updateScan(g);
this.slam.drawFeatures(proc);
this.updateScan(map);
if(!SLAMIsHidden){
this.slam.drawFeatures(proc);
}
}
//With all the views that the car has, get their point list

6
src/Processing.java
View File

@@ -13,6 +13,7 @@ public class Processing extends PApplet {
PointGraph map = new PointGraph();
boolean mapIsHidden = false;
boolean SLAMIsHidden = false;
public static void main(String[] args) {
PApplet.main("Processing");
@@ -36,7 +37,7 @@ public class Processing extends PApplet {
if(!mapIsHidden){
map.draw(processing);
}
car.drawCar(map);
car.drawCar(map, SLAMIsHidden);
strokeWeight(2);
stroke(255);
//car.drive(new int[] {0, 0});
@@ -95,6 +96,9 @@ public class Processing extends PApplet {
if(key == 'h'){
mapIsHidden = !mapIsHidden;
}
if(key == 'j'){
SLAMIsHidden = !SLAMIsHidden;
}
}
public void mousePressed(){

37
src/SLAM.java
View File

@@ -11,7 +11,7 @@ import static processing.core.PApplet.*;
public class SLAM{
ArrayList<Line> lines = new ArrayList<>();
ArrayList<Vector> unassociatedPoints = new ArrayList<>();
ShortTermMem unassociatedPoints = new ShortTermMem();
private static PApplet proc;
SLAM(PApplet processing){
@@ -27,7 +27,7 @@ public class SLAM{
*/
private List<Vector> randomSampleInAngleRange(ArrayList<Vector> set, int subSampleSize, float minAngle, float maxAngle){
// create an arraylist with all points within the angle range fro mthe given set
// create an arraylist with all points within the angle range from the given set
ArrayList<Vector> pointsInAngleRange = new ArrayList<>();
for(Vector point : set){
if(minAngle <= point.z && point.z <= maxAngle){
@@ -54,6 +54,7 @@ public class SLAM{
private void extractFeature(List<Vector> randomSample, float maxRange, int consensus){
// get a line of best fit for this list.
Line bestFit = new Line(randomSample);
// check that there are enough points in the sample that are less than the maxRange away to form a consensus
int count = 0;
ArrayList<Vector> newRandomSample = new ArrayList<>();
for (Vector v : randomSample) {
@@ -73,17 +74,35 @@ public class SLAM{
}
}
private void fitToPreviousReadings(List<Vector> sample, float maxRange){
// keep track of points that were succesffully associated so they can be removed from the sample at the end
ArrayList<Vector> pointsToRemove = new ArrayList<>();
// try to associate points from the smaple with pre-existing lines
for(Vector v: sample){
for(Line l : lines){
if(l.getDistance(v) < maxRange){
l.refitLine(v);
pointsToRemove.add(v);
}
}
}
for(Vector v : pointsToRemove){
sample.remove(v);
}
}
/**
* @param view a laser scan view
*/
public void RANSAC(View view){
unassociatedPoints.addAll(view.getPoints());
unassociatedPoints.addScan(view.getPos(), view.getPoints());
float degreeRange = radians(10); // range to randomly sample readings within
int numSampleReadings = 10; // number of readings to randomly sample
float degreeRange = radians(5); // range to randomly sample readings within
int numSampleReadings = 15; // number of readings to randomly sample
int consensus = 7; // the number of points that need to lie near a line for it to be considered valid.
float maxRange = 5; // the maximum distance a point can be away from the line for it to count as a consensus
int consensus = 10; // the number of points that need to lie near a line for it to be considered valid.
float maxRange = 10; // the maximum distance a point can be away from the line for it to count as a consensus
// this for loop determines the maximum number of trials we're willing to do.
for(int j = 0; j < 20; j++) {
@@ -96,9 +115,11 @@ public class SLAM{
float randomAngle = (float) (2*PI*(random()) - 0.5);
// get a random sub sample of newPoints within the index range of a given size
List<Vector> randomSample = this.randomSampleInAngleRange(this.unassociatedPoints, numSampleReadings, randomAngle-degreeRange, randomAngle+degreeRange);
List<Vector> randomSample = this.randomSampleInAngleRange(this.unassociatedPoints.getPoints(), numSampleReadings, randomAngle-degreeRange, randomAngle+degreeRange);
if(randomSample.size() >= numSampleReadings){
// try to associate points from the sample with previously made lines
fitToPreviousReadings(randomSample, maxRange);
// check if the sub sample forms a valid line and remove the randomSample points if it does.
extractFeature(randomSample, maxRange, consensus);
}

59
src/ShortTermMem.java Normal file
View File

@@ -0,0 +1,59 @@
import java.util.ArrayList;
import Vector.Vector;
public class ShortTermMem {
ArrayList<ArrayList<Vector>> scans = new ArrayList<>();
ArrayList<Vector> scanPositions = new ArrayList<>();
ArrayList<Long> scanTimes = new ArrayList<>();
private int size = 0;
public void addScan(Vector scanPosition, ArrayList<Vector> scan){
size += scan.size();
scans.add(scan);
scanPositions.add(scanPosition);
scanTimes.add(System.currentTimeMillis());
purgeScans();
}
public ArrayList<Vector> getPoints(){
ArrayList<Vector> points = new ArrayList<>();
for(ArrayList<Vector> pointList : this.scans){
points.addAll(pointList);
}
return points;
}
public void remove(Vector point){
for(ArrayList<Vector> pointList : this.scans){
int listSize = pointList.size();
pointList.remove(point);
if(listSize - pointList.size() != 0){
size--;
break;
}
}
}
private void purgeScans(){
long currentTime = System.currentTimeMillis();
int i = scanTimes.size();
// loop through the list backwards and remove all scans that are over second old
// we loop backwards to avoid removal conflicts
while(i > 0){
i--;
long dt = currentTime - scanTimes.get(i);
if(dt < 1000){
continue;
}
size -= scans.get(i).size();
scanTimes.remove(i);
scanPositions.remove(i);
scans.remove(i);
}
}
public int size(){
return this.size;
}
}

28
src/Vector/Line.java
View File

@@ -3,6 +3,7 @@ package Vector;
import Vector.Vector;
import processing.core.PApplet;
import java.util.ArrayList;
import java.util.List;
import static processing.core.PApplet.*;
@@ -13,6 +14,8 @@ public class Line implements LineInterface{
// store the starting position of the line
protected Vector position = new Vector(0,0);
protected ArrayList<Vector> points = new ArrayList<>();
public Line(Vector startPosition, Vector endPosition){
direction = endPosition.sub(startPosition);
position = startPosition;
@@ -26,8 +29,16 @@ public class Line implements LineInterface{
bestFit(points);
}
public void refitLine(Vector newPoint){
// add the new point to our list
this.points.add(newPoint);
// rerun the bestFit algorithm with the new point
bestFit(new ArrayList<>());
}
// least squares line of best fit algorithm
private void bestFit(List<Vector> points){
public void bestFit(List<Vector> fitPoints){
this.points.addAll(fitPoints);
// get the mean of all the points
Vector mean = new Vector();
for(Vector point : points){
@@ -48,7 +59,7 @@ public class Line implements LineInterface{
length += dist;
}
length = 2.5f*length/points.size();
length = 2f*length/points.size();
// if the direction is perfectly vertical create a line to represent that.
if(direction.y == 0){
this.direction = new Vector(0, 1);
@@ -86,12 +97,15 @@ public class Line implements LineInterface{
return this.position.add(this.direction);
}
/**
* @param point
* @return the smallest distance from the point to this line
*/
public float getDistance(Vector point){
return (point.sub(position).cross(direction)).mag() / direction.mag();
float line_dist = direction.mag();
if(line_dist == 0) return this.position.sub(point).mag();
Vector l2 = this.endPoint();
float t = ((point.x - position.x) * (l2.x - position.x) + (point.y - position.y) * (l2.y - position.y) + (point.z - position.z) * (l2.z - position.z)) / line_dist;
t = constrain(t, 0, 1);
Vector closestPoint = new Vector(position.x + t * (l2.x - position.x), position.y + t * (l2.y - position.y), position.z + t * (l2.z - position.z));
return closestPoint.sub(point).mag();
}
public void draw(PApplet proc){

6
src/Vector/Vector.java
View File

@@ -1,5 +1,7 @@
package Vector;
import processing.core.PApplet;
import static java.lang.Math.*;
import static processing.core.PApplet.cos;
import static processing.core.PApplet.sin;
@@ -90,4 +92,8 @@ public class Vector {
float currentAngle = this.angle();
return new Vector(cos(currentAngle + angle), sin(currentAngle + angle)).mul(distance);
}
public void draw(PApplet proc){
proc.circle(this.x, this.y, 8);
}
}

3
src/View.java
View File

@@ -35,7 +35,8 @@ public class View {
for (Ray ray : rays) {
ray.castRay(map);
if(ray.hasCollided()){
ray.drawRay(proc);
ray.getPoint().draw(proc);
// ray.drawRay(proc);
}
}
}