Started converting the view class over to ScanPoints.

This commit is contained in:
Quinn
2023-11-24 18:06:35 -05:00
parent 0c59839dfa
commit a0173b1053
5 changed files with 122 additions and 122 deletions

View File

@@ -1,7 +1,6 @@
import Vector.*;
import processing.core.*;
import java.lang.reflect.Array;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
@@ -96,7 +95,7 @@ public class SLAM{
* @param view a laser scan view
*/
public void RANSAC(View view){
unassociatedPoints.addScan(view.getPos(), view.getPoints());
unassociatedPoints.addScan(view.getPos(), view.getScan().getPoints());
float degreeRange = radians(5); // range to randomly sample readings within
int numSampleReadings = 15; // number of readings to randomly sample

View File

@@ -100,8 +100,8 @@ class CorrespondenceMatrix{
correspondenceMatrix.add(new ArrayList<Float>());
// go through all of the points in the new scan and find the closest point in the old scan
for (int newPointIndex = 0; newPointIndex < newScan.getScan().size(); newPointIndex++) {
Vector newPoint = newScan.getScan().get(newPointIndex);
for (int newPointIndex = 0; newPointIndex < newScan.getPoints().size(); newPointIndex++) {
Vector newPoint = newScan.getPoints().get(newPointIndex);
// if the new point is null, then skip it
if (newPoint == null) {
continue;
@@ -109,8 +109,8 @@ class CorrespondenceMatrix{
// find the closest point in the old scan
float closestDistance = Float.MAX_VALUE;
int closestIndex = -1;
for (int j = 0; j < referenceScan.getScan().size(); j++) {
Vector oldPoint = referenceScan.getScan().get(j);
for (int j = 0; j < referenceScan.getPoints().size(); j++) {
Vector oldPoint = referenceScan.getPoints().get(j);
// if the old point is null, then skip it
if (oldPoint == null) {
continue;
@@ -161,7 +161,7 @@ class ScanMatcher{
private SimpleMatrix averageScanPosition(ScanPoint scan){
Vector averagePosition = new Vector(0, 0);
int invalidPoints = 0;
for (Vector point : scan.getScan()) {
for (Vector point : scan.getPoints()) {
if (point != null) {
averagePosition = averagePosition.add(point);
}
@@ -169,7 +169,7 @@ class ScanMatcher{
invalidPoints++;
}
}
return new SimpleMatrix(averagePosition.div(scan.getScan().size() - invalidPoints).toArray());
return new SimpleMatrix(averagePosition.div(scan.getPoints().size() - invalidPoints).toArray());
}
/**
@@ -190,8 +190,8 @@ class ScanMatcher{
for (int i = 0; i < correspondenceMatrix.getOldPointIndices().size(); i++) {
int oldIndex = correspondenceMatrix.getOldPointIndices().get(i);
int newIndex = correspondenceMatrix.getNewPointIndices().get(i);
Vector oldPoint = referenceScan.getScan().get(oldIndex);
Vector newPoint = newScan.getScan().get(newIndex);
Vector oldPoint = referenceScan.getPoints().get(oldIndex);
Vector newPoint = newScan.getPoints().get(newIndex);
if (oldPoint != null && newPoint != null) {
Vector oldPointCentered = oldPoint.sub(referenceScanAveragePosition);
Vector newPointCentered = newPoint.sub(newScanAveragePosition);
@@ -232,12 +232,12 @@ class ScanMatcher{
public ScanPoint applyRotationAndTranslationMatrices(ScanPoint newScan){
// apply the rotation matrix and translation vector to the new scan
for (int i = 0; i < newScan.getScan().size(); i++) {
Vector point = newScan.getScan().get(i);
for (int i = 0; i < newScan.getPoints().size(); i++) {
Vector point = newScan.getPoints().get(i);
if (point != null) {
SimpleMatrix pointMatrix = new SimpleMatrix(point.toArray());
SimpleMatrix newPointMatrix = rotationMatrix.mult(pointMatrix).plus(translationVector);
newScan.getScan().set(i, new Vector((float) newPointMatrix.get(0), (float) newPointMatrix.get(1)));
newScan.getPoints().set(i, new Vector((float) newPointMatrix.get(0), (float) newPointMatrix.get(1)));
}
}
return newScan;

View File

@@ -8,10 +8,10 @@ import java.util.ArrayList;
public class ScanPoint extends Vertex{
private Vector position;
private Vector orientation;
private float orientation;
private ArrayList<Vector> scan;
ScanPoint(Vector scanPosition, Vector orientation, ArrayList<Vector> scan) {
public ScanPoint(Vector scanPosition, float orientation, ArrayList<Vector> scan) {
super();
this.position = scanPosition;
this.orientation = orientation;
@@ -25,11 +25,11 @@ public class ScanPoint extends Vertex{
return position;
}
public Vector getOrientation(){
public float getOrientation(){
return this.orientation;
}
public ArrayList<Vector> getScan(){
public ArrayList<Vector> getPoints(){
return this.scan;
}

View File

@@ -2,6 +2,7 @@ import Graph.PointGraph;
import Vector.Vector;
import processing.core.*;
import java.util.ArrayList;
import ScanGraph.ScanPoint;
public class View {
Vector pose;
@@ -81,7 +82,7 @@ public class View {
}
//gets the point that each ray has collided with
public ArrayList<Vector> getPoints() {
public ScanPoint getScan() {
ArrayList<Vector> points = new ArrayList<>();
for (Ray ray : rays) {
@@ -92,7 +93,7 @@ public class View {
points.add(point);
}
}
return points;
return new ScanPoint(this.pose,this.angle, points);
}
/**