/// <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); } } }