private void ScanThreadMain()
{
Queue<Scan360> scans = new Queue<Scan360>();
Queue<Scan360> temp = new Queue<Scan360>();
DBSCAN dbscan = new DBSCAN(360);
LaserFeaturesThreadData resultData = new LaserFeaturesThreadData();
while (scanWaitEvent.WaitOne())
{
if (!Run)
break;
lock(waitingScansLock)
{
temp = waitingScans;
waitingScans = scans;
scans = temp;
}
if (scans.Count > 1)
print("Features - more then one waiting scan, does this ever happen?");
while (scans.Count > 0)
{
if(ProcessScan(dbscan, scans.Dequeue(), resultData))
PushResultsToUIThreadSafe(resultData);
}
}
print("Features - scan thread finished");
}
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;
}
private List<DBSCANCluster> DistanceSegmentation(DBSCAN dbscan, DBSCANCluster c, ref int clusterOffset, float eps, int minPoints)
{
float angle = ScanPoint.MeanCircularAngle0Pi(c);
float cos = Mathf.Cos(angle);
float sin = Mathf.Sin(angle);
foreach (ScanPoint p in c)
p.Distance = p.Point.x * cos + p.Point.z * sin;
List<DBSCANCluster> clusters=dbscan.Cluster(c, eps, minPoints, new DistanceComparer(), new DistanceMetric());
if (clusters.Count == 0)
return new List<DBSCANCluster>();
foreach (DBSCANCluster dc in clusters)
foreach (ScanPoint p in dc)
p.DistanceCluster = dc.Id + clusterOffset;
clusterOffset += clusters[clusters.Count - 1].Id;
return clusters;
}
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;
}