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 void PushResultsToUIThreadSafe(LaserFeaturesThreadData result)
{
// consider - change to double buffered
lock (sharedData)
{
Array.Copy(result.points, sharedData.points, result.points.Length);
Array.Copy(result.colors, sharedData.colors, result.colors.Length);
sharedData.elapsedMs = result.elapsedMs;
sharedData.consumed = false;
}
}
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;
}