/// <summary>
/// Search for best robot pose in monto-carlo method
/// </summary>
/// <param name="cloud">Scan points cloud</param>
/// <param name="startPose">Search start position</param>
/// <param name="sigmaXY">XY coordinates standard deviation</param>
/// <param name="sigmaTheta">Theta standard deviation</param>
/// <param name="iterations">Number of search iterations</param>
/// <param name="distance">Best pose distance value (the lower the better)</param>
/// <returns>Best found pose</returns>
public Vector3 MonteCarloSearch(ScanCloud cloud, Vector3 startPose, float sigmaXY, float sigmaTheta, int iterations, out int distance)
{
// Use 3rd party library for fast normal distribution random number generator
var samplerXY = new ZigguratGaussianSampler(0.0f, sigmaXY);
var samplerTheta = new ZigguratGaussianSampler(0.0f, sigmaTheta);
Vector3 bestPose = startPose;
int currentDistance = CalculateDistance(cloud, startPose);
int bestDistance = currentDistance;
for (int counter = 0; counter < iterations; counter++)
{
// Create new random position
Vector3 currentpose = new Vector3()
{
X = startPose.X + samplerXY.Sample(),
Y = startPose.Y + samplerXY.Sample(),
Z = startPose.Z + samplerTheta.Sample()
};
// Calculate distance at that position
currentDistance = CalculateDistance(cloud, currentpose);
// Is it the best ?
if (currentDistance < bestDistance)
{
bestDistance = currentDistance;
bestPose = currentpose;
}
}
distance = bestDistance;
return(bestPose);
}
/// <summary>
/// Fill random number queues which are used in parallel search.
/// </summary>
/// <param name="index">Thread index</param>
private void FillRandomQueues(int index)
{
// Make sure there are enough random numbers in queue
// XY queue has to be twice as large as theta queue
while (randomQueuesXY[index].Count < SearchIterationsPerThread * 2)
{
randomQueuesXY[index].Enqueue(samplerXY.Sample());
}
while (randomQueuesTheta[index].Count < SearchIterationsPerThread)
{
randomQueuesTheta[index].Enqueue(samplerTheta.Sample());
}
}
public BenchmarksDouble()
{
// Create some random Gaussian values as the inputs to the activation functions.
var gaussian = new ZigguratGaussianSampler(0.0, 2.0, 0);
for (int i = 0; i < _x.Length; i++)
{
_x[i] = gaussian.Sample();
}
}
/// <summary>
/// Monte-carlo search job
/// </summary>
/// <param name="state">Context</param>
private void MonteCarloSearchJob(object state)
{
MonteCarloSearchContext context = (MonteCarloSearchContext)state;
Queue <float> randomXY = new Queue <float>();
Queue <float> randomTheta = new Queue <float>();
// Use 3rd party library for fast normal distribution random number generator
ZigguratGaussianSampler samplerXY = new ZigguratGaussianSampler(0.0f, SigmaXY);
ZigguratGaussianSampler samplerTheta = new ZigguratGaussianSampler(0.0f, SigmaTheta);
// Working loop
while (true)
{
// Make sure there are enough random numbers in queue
// XY queue has to be twice as large as theta queue
while (randomXY.Count < SearchIterationsPerThread * 2)
{
randomXY.Enqueue(samplerXY.Sample());
}
while (randomTheta.Count < SearchIterationsPerThread)
{
randomTheta.Enqueue(samplerTheta.Sample());
}
// Wait for job input
if (!context.InputQueue.IsEmpty || context.InputSignal.WaitOne(10))
{
if (context.InputQueue.TryDequeue(out MonteCarloSearchInput input))
{
// Find best position
Vector3 pose = MonteCarloSearch(input.cloud, input.startPose, randomXY, randomTheta, SearchIterationsPerThread, out int distance);
// Put the result to output queue
context.OutputQueue.Enqueue(new MonteCarloSearchResult()
{
pose = pose,
distance = distance
});
context.OutputSignal.Set();
}
}
}
}
