public void SampleNormalDistribution()
{
int image_width = 640;
robot rob = new robot(1);
motionModel mm = new motionModel(rob, rob.LocalGrid, 1);
byte[] img_rays = new byte[image_width*image_width*3];
Bitmap bmp = new Bitmap(image_width, image_width, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
int[] hist = new int[20];
int max = 1;
int max_index = 0;
for (int sample = 0; sample < 2000; sample++)
{
float v = mm.sample_normal_distribution(20);
int index = (hist.Length/2) + ((int)v/(hist.Length/2));
if ((index > -1) && (index < hist.Length))
{
hist[index]++;
if (hist[index] > max)
{
max = hist[index];
max_index = index;
}
}
}
max += 5;
for (int x = 0; x < image_width; x++)
{
int index = x * hist.Length / image_width;
drawing.drawLine(img_rays, image_width, image_width, x,image_width-1, x, image_width-1-(hist[index] * image_width / max), 255,255,255,0,false);
}
BitmapArrayConversions.updatebitmap_unsafe(img_rays, bmp);
bmp.Save("motionmodel_tests_SampleNormalDistribution.bmp", System.Drawing.Imaging.ImageFormat.Bmp);
Assert.IsTrue(max_index == hist.Length/2, "Peak of normal distribution is offset");
}
/// <summary>
/// initialise with the given number of stereo cameras
/// </summary>
/// <param name="no_of_stereo_cameras">the number of stereo cameras on the robot (not the total number of cameras)</param>
/// <param name="rays_per_stereo_camera">the number of rays which will be thrown from each stereo camera per time step</param>
private void init(
int no_of_stereo_cameras,
int rays_per_stereo_camera)
{
this.no_of_stereo_cameras = no_of_stereo_cameras;
// head and shoulders
head = new stereoHead(no_of_stereo_cameras);
// sensor model used for mapping
inverseSensorModel = new stereoModel();
// set the number of stereo features to be detected and inserted into the grid
// on each time step. This figure should be large enough to get reasonable
// detail, but not so large that the mapping consumes a huge amount of
// processing resource
inverseSensorModel.no_of_stereo_features = rays_per_stereo_camera;
// add local occupancy grid
createLocalGrid();
// create a motion model for each possible grid
motion = new motionModel(this, LocalGrid, 100);
// zero encoder positions
prev_left_wheel_encoder = 0;
prev_right_wheel_encoder = 0;
}
