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

First semi-successful test of feature extraction. I think the algorithm for line of best fit needs some work. Specifically, ifnding the starting and end point for the line.

Browse Source
This commit is contained in:
Quinn
2023-05-02 23:51:59 -05:00
parent 2f46605837
commit 7298f80d36
4 changed files with 176 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
src/SLAM.java
View File

@@ -1,14 +1,17 @@
import Vector.*;
import processing.core.*;
import java.lang.reflect.Array;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import static java.lang.Math.random;
import static processing.core.PApplet.*;
public class SLAM{
ArrayList<Line> lines = new ArrayList<>();
ArrayList<Vector> unassociatedPoints = new ArrayList<>();
private static PApplet proc;
SLAM(PApplet processing){
@@ -17,36 +20,38 @@ public class SLAM{
/**
* @param set the set to take a sub sample of
* @param indexRange the range within to take the sub sample
* @param subSampleSize the size of the sub sample
* @return A random subset of the set within an indexRange and of size: subSampleSize
* @param minAngle the minimum angle allowed in the subset
* @param maxAngle the maximum angle allowed in the subset
* @return A random subset of the set within the angle range
*/
private List<Vector> randomSample(ArrayList<Vector> set, int indexRange, int subSampleSize){
// select a random laser data reading
int randomIdx = (int) proc.random(set.size() - 1); // index of starter reading
Vector point = set.get(randomIdx); // point of starter reading
private List<Vector> randomSampleInAngleRange(ArrayList<Vector> set, int subSampleSize, float minAngle, float maxAngle){
// get a random sample of size numSampleReadings within degreeRange degrees of this laser reading.
List<Vector> subSample;
int rangeStart = randomIdx - indexRange >= 0 ? randomIdx - indexRange : 0;
int rangeEnd = randomIdx + indexRange < set.size() ? randomIdx + indexRange : set.size()-1;
subSample = set.subList(rangeStart, rangeEnd); // get the sub-sample
Collections.shuffle(subSample); // shuffle the list
List<Vector> randomSample = subSample.subList(0, rangeEnd-rangeStart); // get our random sample
if (!randomSample.contains(point)) {
randomSample.add(point);
// create an arraylist with all points within the angle range fro mthe given set
ArrayList<Vector> pointsInAngleRange = new ArrayList<>();
for(Vector point : set){
if(minAngle <= point.z && point.z <= maxAngle){
pointsInAngleRange.add(point);
}
}
return randomSample;
// shuffle the list to randomize it
Collections.shuffle(pointsInAngleRange);
// if the list is too small, just return the whole list
if(pointsInAngleRange.size() < subSampleSize){
return pointsInAngleRange;
}
// return a subSample of the list
return pointsInAngleRange.subList(0, subSampleSize);
}
/**
* @param originalList the list which the randomSample of points originated from
* @param randomSample a random subsampling of points from the originalList
* @param maxRange the maximum distance away from the line of best fit of the subSample of points for a given point's consensus to count.
* @param consensus the number of points that have to give their consensus for the line of best fit to count as a valid feature.
*/
private void extractFeature(ArrayList<Vector> originalList, List<Vector> randomSample, float maxRange, int consensus){
private void extractFeature(List<Vector> randomSample, float maxRange, int consensus){
// get a line of best fit for this list.
Line bestFit = new Line(randomSample);
int count = 0;
@@ -63,41 +68,47 @@ public class SLAM{
lines.add(bestFit);
// remove the associated readings from the total available readings.
for (Vector v : newRandomSample) {
originalList.remove(v);
this.unassociatedPoints.remove(v);
}
}
}
/**
* @param newPoints a new scan of points to perform feature detection on
* @param raysPerDegree How many degrees apart are each ray that was cast
* @param view a laser scan view
*/
public void RANSAC(ArrayList<Vector> newPoints, float raysPerDegree){
float degreeRange = radians(10/2); // range to randomly sample readings within
int indexRange = (int) (degreeRange / raysPerDegree);
public void RANSAC(View view){
unassociatedPoints.addAll(view.getPoints());
float degreeRange = radians(25/2); // range to randomly sample readings within
int numSampleReadings = 10; // number of readings to randomly sample
// constrain numSampleReadings so that it cant be higher than possible
if(numSampleReadings >= 2 * indexRange){
numSampleReadings = 2 * indexRange;
}
int consensus = 6; // 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
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
// this for loop determines the maximum number of trials we're willing to do.
for(int j = 0; j < 20; j++) {
// if there aren't enough points left in the set to form a consensus, we're done.
if(newPoints.size() < consensus){
if(this.unassociatedPoints.size() < maxRange){
break;
}
// get a random angle between -PI and PI
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.randomSample(newPoints, indexRange, numSampleReadings);
List<Vector> randomSample = this.randomSampleInAngleRange(this.unassociatedPoints, numSampleReadings, randomAngle-degreeRange, randomAngle+degreeRange);
// check if the sub sample forms a valid line and remove the randomSample points if it does.
extractFeature(newPoints, randomSample, maxRange, consensus);
extractFeature(randomSample, maxRange, consensus);
}
}
public void drawFeatures(PApplet proc){
for(Line line : lines){
line.draw(proc);
}
}
}