/// <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);
}
public void TestCumulativeDistribution()
{
// Standard normal.
var sampler = new ZigguratGaussianSampler(0.0, 1.0);
GaussianDistributionTestUtils.TestDistribution(sampler, 0.0, 1.0);
// Non-zero mean tests.
sampler = new ZigguratGaussianSampler(10.0, 1.0);
GaussianDistributionTestUtils.TestDistribution(sampler, 10.0, 1.0);
sampler = new ZigguratGaussianSampler(-100.0, 1.0);
GaussianDistributionTestUtils.TestDistribution(sampler, -100.0, 1.0);
// Non-1.0 standard deviations
sampler = new ZigguratGaussianSampler(0.0, 0.2);
GaussianDistributionTestUtils.TestDistribution(sampler, 0.0, 0.2);
sampler = new ZigguratGaussianSampler(0.0, 5.0);
GaussianDistributionTestUtils.TestDistribution(sampler, 0.0, 5.0);
// Non-zero mean and non-1.0 standard deviation.
sampler = new ZigguratGaussianSampler(10.0, 2.0);
GaussianDistributionTestUtils.TestDistribution(sampler, 10.0, 2.0);
sampler = new ZigguratGaussianSampler(-10.0, 3.0);
GaussianDistributionTestUtils.TestDistribution(sampler, -10.0, 3.0);
}
/// <summary>
/// Genetic mutation for auxiliary argument data.
/// </summary>
public void MutateAuxArgs(double[] auxArgs, XorShiftRandom rng, ZigguratGaussianSampler gaussianSampler, double connectionWeightRange)
{
// Mutate center.
// Add gaussian ditribution sample and clamp result to +-connectionWeightRange.
double tmp = auxArgs[0] + gaussianSampler.NextSample(0, _auxArgsMutationSigmaCenter);
if (tmp < -connectionWeightRange)
{
auxArgs[0] = -connectionWeightRange;
}
else if (tmp > connectionWeightRange)
{
auxArgs[0] = connectionWeightRange;
}
else
{
auxArgs[0] = tmp;
}
// Mutate radius.
// Add gaussian ditribution sample and clamp result to [0,1]
tmp = auxArgs[1] + gaussianSampler.NextSample(0, _auxArgsMutationSigmaRadius);
if (tmp < 0.0)
{
auxArgs[1] = 0.0;
}
else if (tmp > 1.0)
{
auxArgs[1] = 1.0;
}
else
{
auxArgs[1] = tmp;
}
}
/// <summary>
/// Genetic mutation for auxiliary argument data.
/// </summary>
public void MutateAuxArgs(double[] auxArgs, FastRandom rng,
ZigguratGaussianSampler gaussianSampler,
double connectionWeightRange)
{
throw new SharpNeatException("MutateAuxArgs() called on activation" +
"function that does not use auxiliary arguments.");
}
public FunctionBenchmarks()
{
// Create some random gaussian values as the inputs to the activation functions.
ZigguratGaussianSampler gaussian = new ZigguratGaussianSampler(0);
for (int i = 0; i < _x.Length; i++)
{
_x[i] = gaussian.NextDouble(0, 2.0);
}
}
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();
}
}
public void EstimateMedianOfGaussianDistribution()
{
var gaussianRng = new ZigguratGaussianSampler();
var testData = Enumerable.Range(0, 1000).Select(i => Math.Abs((int)gaussianRng.NextSample(500, 250))).ToList();
var actualMedian = FrugalQuantile.ShuffledEstimate(testData, 1, 2, FrugalQuantile.LinearStepAdjuster);
var msg = $"Estimated median of numbers following a Gaussian distribution is {actualMedian}, should be near 500";
Debug.WriteLine(msg);
Assert.IsTrue(actualMedian >= 450 && actualMedian <= 550, msg);
}
/// <summary> Default constructor. </summary>
public ActivationFunctions()
{
// Create some random Gaussian values as the inputs to the activation functions.
var gaussian = new ZigguratGaussianSampler(0);
for (var i = 0; i < _x.Length; i++)
{
_x[i] = gaussian.NextDouble(0, 2.0);
_f[i] = (float)gaussian.NextDouble(0, 2.0);
}
}
//нормальное распределение(зиккурат-алогритм)
public static double ZigguratNormalDistibution()
{
ZigguratGaussianSampler zig = new ZigguratGaussianSampler();
Random rnd = new Random();
var res = zig.NextSample();
if (Math.Abs(res) > 1)
{
return(rnd.NextDouble());
}
return(Math.Abs(res));
}
/// <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);
}
}
}
/// <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();
}
}
}
}
/// <summary>
/// Genetic mutation for auxiliary argument data.
/// </summary>
public void MutateAuxArgs(double[] auxArgs, XorShiftRandom rng, ZigguratGaussianSampler gaussianSampler, double connectionWeightRange)
{
// Mutate center.
// Add gaussian ditribution sample and clamp result to +-connectionWeightRange.
double tmp = auxArgs[0] + gaussianSampler.NextSample(0, _auxArgsMutationSigmaCenter);
if(tmp < -connectionWeightRange) {
auxArgs[0] = -connectionWeightRange;
}
else if(tmp > connectionWeightRange) {
auxArgs[0] = connectionWeightRange;
}
else {
auxArgs[0] = tmp;
}
// Mutate radius.
// Add gaussian ditribution sample and clamp result to [0,1]
tmp = auxArgs[1] + gaussianSampler.NextSample(0, _auxArgsMutationSigmaRadius);
if(tmp < 0.0) {
auxArgs[1] = 0.0;
}
else if(tmp > 1.0) {
auxArgs[1] = 1.0;
}
else {
auxArgs[1] = tmp;
}
}
/// <summary>
/// ReSeed random number generator using new seed
/// </summary>
/// <param name="seed">Array of ulong to re-initialize</param>
public void ReSeed(ulong[] seed)
{
_ziggurt = new ZigguratGaussianSampler((int)seed[0]);
}
/// <summary>
/// Genetic mutation for auxiliary argument data.
/// </summary>
public void MutateAuxArgs(double[] auxArgs, XorShiftRandom rng, ZigguratGaussianSampler gaussianSampler, double connectionWeightRange)
{
throw new SharpNeatException("MutateAuxArgs() called on activation function that does not use auxiliary arguments.");
}
public void SimpleStats()
{
var sampler = new ZigguratGaussianSampler(0.0, 1.0);
GaussianDistributionTestUtils.TestSimpleStats(sampler);
}
public void TestCumulativeDistribution(double mean, double stdDev)
{
var sampler = new ZigguratGaussianSampler(mean, stdDev);
GaussianDistributionTestUtils.TestDistribution(sampler, mean, stdDev);
}
public static void NewGaussSeed(int seed)
{
gaussianSampler = new ZigguratGaussianSampler(seed);
gaussianSamplerInitialized = true;
}
