public void CreateRay() { stereoModel model = new stereoModel(); evidenceRay ray = model.createRay(20,10,5,0,255,255,255); Assert.IsNotNull(ray.vertices); Assert.IsNotNull(ray.vertices[0]); Assert.IsNotNull(ray.vertices[1]); }
/// <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; }
public void CreateObservation() { float baseline = 120; int image_width = 320; int image_height = 240; float FOV_degrees = 68; int no_of_stereo_features = 10; float[] stereo_features = new float[no_of_stereo_features*4]; byte[] stereo_features_colour = new byte[no_of_stereo_features*3]; bool translate = false; for (int i = 0; i < no_of_stereo_features; i++) { stereo_features[i*4] = 1; stereo_features[i*4+1] = i * image_width / no_of_stereo_features; stereo_features[i*4+2] = image_height/2; stereo_features[i*4+3] = 1; stereo_features_colour[i*3] = 200; stereo_features_colour[i*3+1] = 200; stereo_features_colour[i*3+2] = 200; } for (int rotation_degrees = 0; rotation_degrees < 360; rotation_degrees += 90) { stereoModel model = new stereoModel(); pos3D observer = new pos3D(0,0,0); observer = observer.rotate(rotation_degrees/180.0f*(float)Math.PI,0,0); List<evidenceRay> rays = model.createObservation( observer, baseline, image_width, image_height, FOV_degrees, stereo_features, stereo_features_colour, translate); float tx = float.MaxValue; float ty = float.MaxValue; float bx = float.MinValue; float by = float.MinValue; for (int i = 0; i < no_of_stereo_features; i++) { //float pan_degrees = rays[i].pan_angle * 180 / (float)Math.PI; //Console.WriteLine(pan_degrees.ToString()); for (int j = 0; j < rays[i].vertices.Length; j++) { Console.WriteLine("Vertex " + j.ToString()); Console.WriteLine("xyz: " + rays[i].vertices[j].x.ToString() + " " + rays[i].vertices[j].y.ToString() + " " + rays[i].vertices[j].z.ToString()); if (rays[i].vertices[j].x < tx) tx = rays[i].vertices[j].x; if (rays[i].vertices[j].x > bx) bx = rays[i].vertices[j].x; if (rays[i].vertices[j].y < ty) ty = rays[i].vertices[j].y; if (rays[i].vertices[j].y > by) by = rays[i].vertices[j].y; } } int img_width = 640; Bitmap bmp = new Bitmap(img_width, img_width, System.Drawing.Imaging.PixelFormat.Format24bppRgb); byte[] img = new byte[img_width * img_width * 3]; for (int i = 0; i < img.Length; i++) img[i] = 255; for (int i = 0; i < no_of_stereo_features; i++) { int x0 = (int)((rays[i].vertices[0].x - tx) * img_width / (bx - tx)); int y0 = (int)((rays[i].vertices[0].y - ty) * img_width / (by - ty)); int x1 = (int)((rays[i].vertices[1].x - tx) * img_width / (bx - tx)); int y1 = (int)((rays[i].vertices[1].y - ty) * img_width / (by - ty)); drawing.drawLine(img, img_width, img_width, x0,y0,x1,y1,0,0,0,0,false); } BitmapArrayConversions.updatebitmap_unsafe(img, bmp); bmp.Save("dpslam_tests_createobservation_" + rotation_degrees.ToString() + ".bmp", System.Drawing.Imaging.ImageFormat.Bmp); Console.WriteLine("dpslam_tests_createobservation_" + rotation_degrees.ToString() + ".bmp"); } }