/// <summary>
/// Constructor
/// </summary>
/// <param name="physicalMapSize">Physical map size in meters</param>
/// <param name="holeMapSize">Hole map size in pixels</param>
/// <param name="obstacleMapSize">Obstacle map size in pixels</param>
/// <param name="startPose">Start pose</param>
public SLAM(float physicalMapSize, int holeMapSize, int obstacleMapSize, Vector3 startPose, int numSearchThreads)
{
PhysicalMapSize = physicalMapSize;
HoleMap = new HoleMap(holeMapSize, physicalMapSize);
ObstacleMap = new ObstacleMap(obstacleMapSize, physicalMapSize);
noHitMap = new bool[obstacleMapSize, obstacleMapSize];
this.startPose = startPose;
Reset();
// Create parallel search threads
searchThreads = new Thread[numSearchThreads];
searchContexts = new MonteCarloSearchContext[numSearchThreads];
searchResultSignals = new AutoResetEvent[numSearchThreads];
for (int i = 0; i < numSearchThreads; i++)
{
searchContexts[i] = new MonteCarloSearchContext()
{
InputSignal = new AutoResetEvent(false),
OutputSignal = new AutoResetEvent(false),
InputQueue = new ConcurrentQueue <MonteCarloSearchInput>(),
OutputQueue = new ConcurrentQueue <MonteCarloSearchResult>()
};
searchResultSignals[i] = searchContexts[i].OutputSignal;
searchThreads[i] = new Thread(new ParameterizedThreadStart(MonteCarloSearchJob));
searchThreads[i].Start(searchContexts[i]);
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="physicalMapSize">Physical map size in meters</param>
/// <param name="holeMapSize">Hole map size in pixels</param>
/// <param name="obstacleMapSize">Obstacle map size in pixels</param>
/// <param name="startPose">Start pose</param>
/// <param name="sigmaXY">Sigma XY in meters</param>
/// <param name="sigmaTheta">Sigma theta in radians</param>
/// <param name="iterationsPerThread">Search iterations per thread</param>
/// <param name="numSearchThreads">Number of search threads (1 or less for no threading)</param>
public CoreSLAMProcessor(float physicalMapSize, int holeMapSize, int obstacleMapSize, Vector3 startPose,
float sigmaXY, float sigmaTheta, int iterationsPerThread, int numSearchThreads)
{
// Set properties
PhysicalMapSize = physicalMapSize;
this.startPose = startPose;
SigmaXY = sigmaXY;
SigmaTheta = sigmaTheta;
SearchIterationsPerThread = iterationsPerThread;
NumSearchThreads = numSearchThreads;
// Create maps
HoleMap = new HoleMap(holeMapSize, physicalMapSize);
ObstacleMap = new ObstacleMap(obstacleMapSize, physicalMapSize);
noHitMap = new bool[obstacleMapSize, obstacleMapSize];
// Use 3rd party library for fast normal distribution random number generator
samplerXY = new ZigguratGaussianSampler(0.0f, sigmaXY);
samplerTheta = new ZigguratGaussianSampler(0.0f, sigmaTheta);
// Reset everything
Reset();
// Threaded mode or not ?
if (numSearchThreads <= 0)
{
worker = null;
}
else
{
worker = new ParallelWorker(NumSearchThreads, "CoreSLAM search");
randomQueuesXY = new Queue <float> [NumSearchThreads];
randomQueuesTheta = new Queue <float> [NumSearchThreads];
// Create random number queues and fill them up
for (int i = 0; i < NumSearchThreads; i++)
{
randomQueuesXY[i] = new Queue <float>();
randomQueuesTheta[i] = new Queue <float>();
FillRandomQueues(i);
}
}
}
