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