private List<DBSCANCluster> AngularSegmentation(DBSCAN dbscan, Scan360 scan, float eps, int minPoints)
{
scan.EstimateLocalAngle();
List<DBSCANCluster> clusters=dbscan.Cluster(scan.readings, eps, minPoints, new AngleComparer(), new AngleCircularMetric());
foreach (DBSCANCluster c in clusters)
foreach (ScanPoint dp in c)
dp.ParallelCluster = c.Id;
return clusters;
}
public void PutScanThreadSafe(Vector3[] readings, bool[] invalid_data)
{
//to consider - circular buffer of scans and swap buffers instead of copy
//copy is ok for now
Scan360 scan = new Scan360(readings, invalid_data);
lock(waitingScansLock)
{
waitingScans.Enqueue(scan);
}
scanWaitEvent.Set();
}
public void FromScan360(Scan360 scan, LaserFeaturesSegmentationLevel level, Color[] featureColors, long elapsedMs)
{
for (int i = 0; i < scan.readings.Count; ++i)
{
points[i] = scan.readings[i].Point;
if (level == LaserFeaturesSegmentationLevel.Angle)
colors[i] = GetColor(scan.readings[i].ParallelCluster, featureColors);
else
colors[i] = GetColor(scan.readings[i].DistanceCluster, featureColors);
}
for (int i = scan.readings.Count; i < 360; ++i)
{
points[i] = scan.readings[0].Point;
colors[i] = Color.clear;
}
this.elapsedMs = elapsedMs;
}
private bool ProcessScan(DBSCAN dbscan, Scan360 scan, LaserFeaturesThreadData result)
{
if (scan.readings.Count < 3)
return false;
System.Diagnostics.Stopwatch stopwatch = System.Diagnostics.Stopwatch.StartNew();
for (int i = 0; i < scan.readings.Count; ++i)
scan.readings[i].ParallelCluster = scan.readings[i].DistanceCluster = 0;
List<DBSCANCluster> parallelClusters = AngularSegmentation(dbscan, scan, segmentation.angularEpsDeg * Constants.DEG2RAD, segmentation.angularPoints);
List<DBSCANCluster> distanceClusters = new List<DBSCANCluster>();
int lastClusterId = 0;
foreach (DBSCANCluster c in parallelClusters)
distanceClusters.AddRange(DistanceSegmentation(dbscan, c, ref lastClusterId, segmentation.distanceEpsCm/100.0f, segmentation.distancePoints));
stopwatch.Stop();
result.FromScan360(scan, plot.level, plot.colors, stopwatch.ElapsedMilliseconds);
return true;
}