static void sample_motion_data_thrun(particle_t* particle, const motion_data_t* motion, const motion_param_t *param) {
real_t rot12 = motion->delta_rot1*motion->delta_rot1;
real_t rot22 = motion->delta_rot2*motion->delta_rot2;
real_t trans2 = motion->delta_trans * motion->delta_trans;
real_t delta_rot1 = motion->delta_rot1 + random_gaussian(param->alpha1*rot12 + param->alpha2*trans2);
real_t delta_trans = motion->delta_trans + random_gaussian(param->alpha3*trans2 + param->alpha4*rot12 + param->alpha4*rot22);
real_t delta_rot2 = motion->delta_rot2 + random_gaussian(param->alpha1*rot22 + param->alpha2*trans2);
particle->pose.x += delta_trans * cos(particle->pose.th + delta_rot1);
particle->pose.y += delta_trans * sin(particle->pose.th + delta_rot1);
particle->pose.th += delta_rot1 + delta_rot2;
}
static void sample_particle_gausian(const map_t* map, particle_t *particle,
const real_t initial_sample_pose_x, const real_t initial_sample_pose_y, const real_t initial_sample_pose_phi,
const real_t initial_sample_std_xy, const real_t initial_sample_std_phi) {
while (TRUE) {
int x, y;
particle->pose.x = random_gaussian(initial_sample_std_xy) + initial_sample_pose_x;
particle->pose.y = random_gaussian(initial_sample_std_xy) + initial_sample_pose_y;
particle->pose.th = random_gaussian(initial_sample_std_phi) + initial_sample_pose_phi;
pose_to_cell(&particle->pose, map, &x, &y);
if (map_validate_index(map, x, y)) {
if (map_empty(map, x, y)) {
break;
}
}
}
}
static void initialize_params_noisy_uniform(void)
{
int i;
SW_INS_PTR ptr;
double sum,p;
for (i = 0; i < occ_switch_tab_size; i++) {
ptr = occ_switches[i];
if (ptr->fixed > 0) continue;
p = 1.0 / num_sw_vals[i];
sum = 0.0;
while (ptr != NULL) {
ptr->inside = random_gaussian(p, std_ratio * p);
if (ptr->inside < INIT_PROB_THRESHOLD)
ptr->inside = INIT_PROB_THRESHOLD;
sum += ptr->inside;
ptr = ptr->next;
}
ptr = occ_switches[i];
while (ptr != NULL) { /* normalize */
ptr->inside = ptr->inside / sum;
ptr = ptr->next;
}
}
}
/// Return a random multivariate gaussian point
VectorType random_gaussian(int arg_dim){
VectorType random_vector;
random_vector.setZero(arg_dim);
for(int ii=0; ii < random_vector.size(); ii++){
random_vector(ii) = random_gaussian();
}
return random_vector;
}
static void initialize_lambdas_noisy_uniform(void) {
int i;
SW_INS_PTR ptr;
for (i = 0; i < occ_switch_tab_size; i++) {
ptr = occ_switches[i];
if (ptr->fixed > 0) continue;
while (ptr != NULL) {
ptr->inside = random_gaussian(0, std_ratio);
ptr = ptr->next;
}
}
}
// Poisson distributor (JRP)
long long poisson(long long lambda) {
long long count;
double f,g;
long long d;
if (lambda<10) {
f = RngDouble();
d = 0;
while (f >= exp(-(double)lambda)) {
g = RngDouble();
f = f*g;
d++;
}
count = d;
} else {
count = (long long)((double)lambda + sqrt((double)lambda)*random_gaussian());
if (count<0) count = 0;
}
return(count);
}
tracktable::PointCartesian<dim> random_point_in_sphere(double sphere_radius=1)
{
typedef tracktable::PointCartesian<dim> point_t;
point_t result((tracktable::arithmetic::zero<point_t>()));
double squared_magnitude = 0;
for (int i = 0; i < dim; ++i)
{
// YOU ARE HERE
double rg = random_gaussian();
squared_magnitude += rg*rg;
result[i] = rg;
}
boost::geometry::divide_value(result, sqrt(squared_magnitude));
// now scale it down to somewhere within the sphere
boost::geometry::multiply_value(result, sphere_radius * pow(random_float(), 1.0 / dim));
return result;
}
void initialize_hyperparams(void) {
int i;
SW_INS_PTR ptr;
double p,r;
for (i = 0; i < occ_switch_tab_size; i++) {
ptr = occ_switches[i];
while (ptr != NULL) {
ptr->smooth = ptr->smooth_prolog;
ptr = ptr->next;
}
}
for (i = 0; i < occ_switch_tab_size; i++) {
ptr = occ_switches[i];
if (ptr->fixed_h > 0) {
while (ptr != NULL) {
ptr->inside_h = ptr->smooth + 1.0;
ptr->total_expect = 0.0;
ptr = ptr->next;
}
} else {
p = 1.0 / num_sw_vals[i];
while (ptr != NULL) {
r = random_gaussian(0.0, std_ratio * p);
ptr->inside_h =
(ptr->smooth + 1.0 < EPS) ? EPS : ptr->smooth + 1.0;
ptr->inside_h *= (1.0 + fabs(r));
ptr->smooth = ptr->inside_h - 1.0;
ptr->total_expect = 0.0;
ptr = ptr->next;
}
}
}
}
/// Draws a random point from the n-sphere
VectorType random_n_sphere(int arg_dim){
return random_radius(arg_dim) *
random_gaussian(arg_dim).normalized();
}
GAULFUNC boolean random_test(void)
{
unsigned int i, j, k; /* Loop variables. */
double r; /* Pseudo-random number. */
long bins[NUM_BINS]; /* Bin. */
double sum=0,sumsq=0; /* Stats. */
int numtrue=0, numfalse=0; /* Count booleans. */
unsigned int rchi=100; /* Number of bins in chisq test. */
unsigned int nchi=1000; /* Number of samples in chisq test. */
double chisq; /* Chisq error. */
double elimit = 2*sqrt((double)rchi); /* Chisq error limit. */
double nchi_rchi = (double)nchi / (double)rchi; /* Speed calculation. */
FILE *rfile=NULL; /* Handle for file of random integers. */
random_init();
printf("Testing random numbers.\n");
/*
* Uniform Distribution.
*/
printf("Uniform distribution. Mean should be about 0.5.\n");
for (i=0;i<NUM_BINS;i++) bins[i] = 0;
for (i=0;i<NUM_SAMPLES;i++)
{
r = random_unit_uniform();
if (r >= 0.0 && r < 1.0)
{
bins[(int)(r*NUM_BINS)]++;
sum += r;
sumsq += SQU(r);
}
else
{
printf("Number generated out of range 0.0<=r<1.0.\n");
}
}
printf("Mean = %f\n", sum / NUM_SAMPLES);
printf("Standard deviation = %f\n", (sumsq - sum*sum/NUM_SAMPLES)/NUM_SAMPLES);
for (i=0;i<NUM_BINS;i++)
printf("%5.3f %ld\n", i/(double)NUM_BINS, bins[i]);
/*
* Gaussian Distribution.
*/
printf("Gaussian distribution. Mean should be about 0.45. Standard deviation should be about 0.05.\n");
sum=0;
sumsq=0;
for (i=0;i<NUM_BINS;i++) bins[i] = 0;
for (i=0;i<NUM_SAMPLES;i++)
{
r = random_gaussian(0.45,0.05);
if (r >= 0.0 && r < 1.0)
{
bins[(int)(r*NUM_BINS)]++;
sum += r;
sumsq += SQU(r);
}
else
{
printf("Number generated out of range 0.0<=r<1.0.\n");
}
}
printf("Mean = %f\n", sum / NUM_SAMPLES);
printf("Standard deviation = %f\n", (sumsq - sum*sum/NUM_SAMPLES)/NUM_SAMPLES);
for (i=0;i<NUM_BINS;i++)
printf("%5.3f %ld\n", i/(double)NUM_BINS, bins[i]);
/*
* Unit Gaussian Distribution.
*/
printf("Gaussian distribution. Mean should be about 0.0. Standard deviation should be about 1.0.\n");
sum=0;
sumsq=0;
for (i=0;i<NUM_BINS;i++) bins[i] = 0;
for (i=0;i<NUM_SAMPLES;i++)
{
r = random_unit_gaussian();
if (r >= -5.0 && r < 5.0)
{
bins[(int)((r+5.0)*NUM_BINS)/10]++;
sum += r;
sumsq += SQU(r);
}
else
{
printf("Number generated out of range -5.0<=r<5.0.\n");
}
}
printf("Mean = %f\n", sum / NUM_SAMPLES);
printf("Standard deviation = %f\n", (sumsq - sum*sum/NUM_SAMPLES)/NUM_SAMPLES);
for (i=0;i<NUM_BINS;i++)
printf("%5.3f %ld\n", -5.0+10*i/(double)NUM_BINS, bins[i]);
/*
* Random Boolean.
*/
printf("Random Booleans. Two counts should be approximately equal.\n");
for (i=0;i<NUM_SAMPLES;i++)
{
if ( random_boolean() )
numtrue++;
else
numfalse++;
}
printf("TRUE/FALSE = %d/%d\n", numtrue, numfalse);
/*
* Random int.
*/
printf("Random Integers. The distribution should be approximately uniform.\n");
for (i=0;i<NUM_BINS;i++) bins[i] = 0;
for (i=0;i<NUM_SAMPLES;i++)
bins[random_int(NUM_BINS)]++;
for (i=0;i<NUM_BINS;i++)
printf("%u %ld\n", i, bins[i]);
/*
* Chi squared test. This is the standard basic test for randomness of a PRNG.
* We would expect any moethod to fail about about one out of ten runs.
* The error is r*t/N - N and should be within 2*sqrt(r) of r.
*/
printf("Chi Squared Test of Random Integers. We would expect a couple of failures.\n");
if (rchi>NUM_BINS) die("Internal error: too few bins.");
for (j=0;j<NUM_CHISQ;j++)
{
printf("Run %u. chisq should be within %f of %u.\n", j, elimit, rchi);
for(k=0; k<10; k++)
{
memset(bins, 0, rchi*sizeof(long));
chisq = 0.0;
for(i=0; i<nchi; i++)
bins[random_int(rchi)]++;
for(i=0; i<rchi; i++)
chisq += SQU((double)bins[i] - nchi_rchi);
chisq /= nchi_rchi;
printf("chisq = %f - %s\n", chisq, fabs(chisq - rchi)>elimit?"FAILED":"PASSED");
}
}
printf("Creating file (\"randtest.dat\") of 5000 random integer numbers for external analysis.\n");
rfile = fopen("randtest.dat", "w");
for (i=0; i<5000; i++)
{
r = (double) random_rand();
fprintf(rfile, "%f %f\n",
/*i, r,*/
(double)i/(double)5000, r/(double)RANDOM_RAND_MAX);
}
fclose(rfile);
return TRUE;
}
double random_gaussian_wrapper(double *mean, double *stddev)
{
return random_gaussian(*mean, *stddev);
}
