/// <summary> /// calculate the position of the robots head and cameras for this pose /// </summary> /// <param name="rob">robot object</param> /// <param name="head_location">location of the centre of the head</param> /// <param name="camera_centre_location">location of the centre of each stereo camera</param> /// <param name="left_camera_location">location of the left camera within each stereo camera</param> /// <param name="right_camera_location">location of the right camera within each stereo camera</param> protected void calculateCameraPositions( robot rob, ref pos3D head_location, ref pos3D[] camera_centre_location, ref pos3D[] left_camera_location, ref pos3D[] right_camera_location) { // calculate the position of the centre of the head relative to // the centre of rotation of the robots body pos3D head_centroid = new pos3D(-(rob.BodyWidth_mm / 2) + rob.head.x, -(rob.BodyLength_mm / 2) + rob.head.y, rob.head.z); // location of the centre of the head on the grid map // adjusted for the robot pose and the head pan and tilt angle. // Note that the positions and orientations of individual cameras // on the head have already been accounted for within stereoModel.createObservation pos3D head_locn = head_centroid.rotate(rob.head.pan + pan, rob.head.tilt, 0); head_locn = head_locn.translate(x, y, 0); head_location.copyFrom(head_locn); for (int cam = 0; cam < rob.head.no_of_stereo_cameras; cam++) { // calculate the position of the centre of the stereo camera // (baseline centre point) pos3D camera_centre_locn = new pos3D(rob.head.calibration[cam].positionOrientation.x, rob.head.calibration[cam].positionOrientation.y, rob.head.calibration[cam].positionOrientation.y); camera_centre_locn = camera_centre_locn.rotate(rob.head.calibration[cam].positionOrientation.pan + rob.head.pan + pan, rob.head.calibration[cam].positionOrientation.tilt, rob.head.calibration[cam].positionOrientation.roll); camera_centre_location[cam] = camera_centre_locn.translate(head_location.x, head_location.y, head_location.z); // where are the left and right cameras? // we need to know this for the origins of the vacancy models float half_baseline_length = rob.head.calibration[cam].baseline / 2; pos3D left_camera_locn = new pos3D(-half_baseline_length, 0, 0); left_camera_locn = left_camera_locn.rotate(rob.head.calibration[cam].positionOrientation.pan + rob.head.pan + pan, rob.head.calibration[cam].positionOrientation.tilt, rob.head.calibration[cam].positionOrientation.roll); pos3D right_camera_locn = new pos3D(-left_camera_locn.x, -left_camera_locn.y, -left_camera_locn.z); left_camera_location[cam] = left_camera_locn.translate(camera_centre_location[cam].x, camera_centre_location[cam].y, camera_centre_location[cam].z); right_camera_location[cam] = right_camera_locn.translate(camera_centre_location[cam].x, camera_centre_location[cam].y, camera_centre_location[cam].z); right_camera_location[cam].pan = left_camera_location[cam].pan; } }
public void InsertRays() { int no_of_stereo_features = 2000; int image_width = 640; int image_height = 480; int no_of_stereo_cameras = 1; int localisationRadius_mm = 16000; int maxMappingRange_mm = 16000; int cellSize_mm = 32; int dimension_cells = 16000 / cellSize_mm; int dimension_cells_vertical = dimension_cells/2; float vacancyWeighting = 0.5f; float FOV_horizontal = 78 * (float)Math.PI / 180.0f; // create a grid Console.WriteLine("Creating grid"); occupancygridSimple grid = new occupancygridSimple( dimension_cells, dimension_cells_vertical, cellSize_mm, localisationRadius_mm, maxMappingRange_mm, vacancyWeighting); Assert.AreNotEqual(grid, null, "object occupancygridSimple was not created"); Console.WriteLine("Creating sensor models"); stereoModel inverseSensorModel = new stereoModel(); inverseSensorModel.FOV_horizontal = FOV_horizontal; inverseSensorModel.FOV_vertical = FOV_horizontal * image_height / image_width; inverseSensorModel.createLookupTable(cellSize_mm, image_width, image_height); //Assert.AreNotEqual(0, inverseSensorModel.ray_model.probability[1][5], "Ray model probabilities not updated"); // observer parameters int pan_angle_degrees = 0; pos3D observer = new pos3D(0,0,0); observer.pan = pan_angle_degrees * (float)Math.PI / 180.0f; float stereo_camera_baseline_mm = 100; pos3D left_camera_location = new pos3D(stereo_camera_baseline_mm*0.5f,0,0); pos3D right_camera_location = new pos3D(-stereo_camera_baseline_mm*0.5f,0,0); left_camera_location = left_camera_location.rotate(observer.pan, observer.tilt, observer.roll); right_camera_location = right_camera_location.rotate(observer.pan, observer.tilt, observer.roll); left_camera_location = left_camera_location.translate(observer.x, observer.y, observer.z); right_camera_location = right_camera_location.translate(observer.x, observer.y, observer.z); float FOV_degrees = 78; float[] stereo_features = new float[no_of_stereo_features * 3]; byte[,] stereo_features_colour = new byte[no_of_stereo_features, 3]; float[] stereo_features_uncertainties = new float[no_of_stereo_features]; // create some stereo disparities within the field of view Console.WriteLine("Adding disparities"); //MersenneTwister rnd = new MersenneTwister(0); Random rnd = new Random(0); for (int correspondence = 0; correspondence < no_of_stereo_features; correspondence++) { float x = rnd.Next(image_width-1); float y = rnd.Next(image_height/50) + (image_height/2); float disparity = 7; if ((x < image_width/5) || (x > image_width * 4/5)) { disparity = 7; //15; } byte colour_red = (byte)rnd.Next(255); byte colour_green = (byte)rnd.Next(255); byte colour_blue = (byte)rnd.Next(255); stereo_features[correspondence*3] = x; stereo_features[(correspondence*3)+1] = y; stereo_features[(correspondence*3)+2] = disparity; stereo_features_colour[correspondence, 0] = colour_red; stereo_features_colour[correspondence, 1] = colour_green; stereo_features_colour[correspondence, 2] = colour_blue; stereo_features_uncertainties[correspondence] = 0; } // create an observation as a set of rays from the stereo correspondence results List<evidenceRay>[] stereo_rays = new List<evidenceRay>[no_of_stereo_cameras]; for (int cam = 0; cam < no_of_stereo_cameras; cam++) { Console.WriteLine("Creating rays"); stereo_rays[cam] = inverseSensorModel.createObservation( observer, stereo_camera_baseline_mm, image_width, image_height, FOV_degrees, stereo_features, stereo_features_colour, stereo_features_uncertainties, true); // insert rays into the grid Console.WriteLine("Throwing rays"); for (int ray = 0; ray < stereo_rays[cam].Count; ray++) { grid.Insert(stereo_rays[cam][ray], inverseSensorModel.ray_model, left_camera_location, right_camera_location, false); } } // save the result as an image Console.WriteLine("Saving grid"); int debug_img_width = 640; int debug_img_height = 480; byte[] debug_img = new byte[debug_img_width * debug_img_height * 3]; Bitmap bmp = new Bitmap(debug_img_width, debug_img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb); grid.Show(debug_img, debug_img_width, debug_img_height, false, false); BitmapArrayConversions.updatebitmap_unsafe(debug_img, bmp); bmp.Save("tests_occupancygrid_simple_InsertRays_overhead.jpg", System.Drawing.Imaging.ImageFormat.Jpeg); grid.ShowFront(debug_img, debug_img_width, debug_img_height, true); BitmapArrayConversions.updatebitmap_unsafe(debug_img, bmp); bmp.Save("tests_occupancygrid_simple_InsertRays_front.jpg", System.Drawing.Imaging.ImageFormat.Jpeg); // side view of the probabilities float max_prob = -1; float min_prob = 1; float[] probs = new float[dimension_cells/2]; float[] mean_colour = new float[3]; for (int y = dimension_cells / 2; y < dimension_cells; y++) { float p = grid.GetProbability(dimension_cells / 2, y, mean_colour); probs[y - (dimension_cells / 2)] = p; if (p != occupancygridSimple.NO_OCCUPANCY_EVIDENCE) { if (p < min_prob) min_prob = p; if (p > max_prob) max_prob = p; } } for (int i = 0; i < debug_img.Length; i++) debug_img[i] = 255; int prev_x = -1; int prev_y = debug_img_height / 2; for (int i = 0; i < probs.Length; i++) { if (probs[i] != occupancygridSimple.NO_OCCUPANCY_EVIDENCE) { int x = i * (debug_img_width - 1) / probs.Length; int y = debug_img_height - 1 - (int)((probs[i] - min_prob) / (max_prob - min_prob) * (debug_img_height - 1)); int n = ((y * debug_img_width) + x) * 3; if (prev_x > -1) { int r = 255; int g = 0; int b = 0; if (probs[i] > 0.5f) { r = 0; g = 255; b = 0; } drawing.drawLine(debug_img, debug_img_width, debug_img_height, prev_x, prev_y, x, y, r, g, b, 0, false); } prev_x = x; prev_y = y; } } int y_zero = debug_img_height - 1 - (int)((0.5f-min_prob) / (max_prob - min_prob) * (debug_img_height - 1)); drawing.drawLine(debug_img, debug_img_width, debug_img_height, 0, y_zero, debug_img_width - 1, y_zero, 0, 0, 0, 0, false); BitmapArrayConversions.updatebitmap_unsafe(debug_img, bmp); bmp.Save("tests_occupancygrid_simple_InsertRays_probs.jpg", System.Drawing.Imaging.ImageFormat.Jpeg); }
/// <summary> /// Calculate the position of the robots head and cameras for this pose /// the positions returned are relative to the robots centre of rotation /// </summary> /// <param name="body_width_mm">width of the robot body in millimetres</param> /// <param name="body_length_mm">length of the robot body in millimetres</param> /// <param name="body_centre_of_rotation_x">x centre of rotation of the robot, relative to the top left corner</param> /// <param name="body_centre_of_rotation_y">y centre of rotation of the robot, relative to the top left corner</param> /// <param name="body_centre_of_rotation_z">z centre of rotation of the robot, relative to the top left corner</param> /// <param name="head_centroid_x">head centroid x position in millimetres relative to the top left corner of the body</param> /// <param name="head_centroid_y">head centroid y position in millimetres relative to the top left corner of the body</param> /// <param name="head_centroid_z">head centroid z position in millimetres relative to the top left corner of the body</param> /// <param name="head_pan">head pan angle in radians</param> /// <param name="head_tilt">head tilt angle in radians</param> /// <param name="head_roll">head roll angle in radians</param> /// <param name="baseline_mm">stereo camera baseline in millimetres</param> /// <param name="stereo_camera_position_x">stereo camera x position in millimetres relative to the head centroid</param> /// <param name="stereo_camera_position_y">stereo camera y position in millimetres relative to the head centroid</param> /// <param name="stereo_camera_position_z">stereo camera z position in millimetres relative to the head centroid</param> /// <param name="stereo_camera_pan">stereo camera pan in radians relative to the head</param> /// <param name="stereo_camera_tilt">stereo camera tilt in radians relative to the head</param> /// <param name="stereo_camera_roll">stereo camera roll in radians relative to the head</param> /// <param name="head_location">returned location/orientation of the robot head</param> /// <param name="camera_centre_location">returned stereo camera centre position/orientation</param> /// <param name="left_camera_location">returned left camera position/orientation</param> /// <param name="right_camera_location">returned right camera position/orientation</param> public static void calculateCameraPositions( float body_width_mm, float body_length_mm, float body_centre_of_rotation_x, float body_centre_of_rotation_y, float body_centre_of_rotation_z, float head_centroid_x, float head_centroid_y, float head_centroid_z, float head_pan, float head_tilt, float head_roll, float baseline_mm, float stereo_camera_position_x, float stereo_camera_position_y, float stereo_camera_position_z, float stereo_camera_pan, float stereo_camera_tilt, float stereo_camera_roll, ref pos3D head_location, ref pos3D camera_centre_location, ref pos3D left_camera_location, ref pos3D right_camera_location) { // calculate the position of the centre of the head relative to // the centre of rotation of the robots body pos3D head_centroid = new pos3D( -(body_width_mm * 0.5f) + (body_centre_of_rotation_x - (body_width_mm * 0.5f)) + head_centroid_x, -(body_length_mm * 0.5f) + (body_centre_of_rotation_y - (body_length_mm * 0.5f)) + head_centroid_y, head_centroid_z); // location of the centre of the head // adjusted for the robot pose and the head pan and tilt angle. // Note that the positions and orientations of individual cameras // on the head have already been accounted for within stereoModel.createObservation pos3D head_locn = head_centroid.rotate( head_pan, head_tilt, 0); head_location.copyFrom(head_locn); // calculate the position of the centre of the stereo camera // (baseline centre point) pos3D camera_centre_locn = new pos3D( stereo_camera_position_x, stereo_camera_position_y, stereo_camera_position_z); // rotate the stereo camera camera_centre_locn = camera_centre_locn.rotate( stereo_camera_pan + head_pan, stereo_camera_tilt + head_tilt, stereo_camera_roll + head_roll); // move the stereo camera relative to the head position camera_centre_location = camera_centre_locn.translate( head_location.x, head_location.y, head_location.z); // where are the left and right cameras? // we need to know this for the origins of the vacancy models float half_baseline_length = baseline_mm * 0.5f; pos3D left_camera_locn = new pos3D(-half_baseline_length, 0, 0); left_camera_locn = left_camera_locn.rotate(stereo_camera_pan + head_pan, stereo_camera_tilt + head_tilt, stereo_camera_roll + head_roll); pos3D right_camera_locn = new pos3D(-left_camera_locn.x, -left_camera_locn.y, -left_camera_locn.z); left_camera_location = left_camera_locn.translate(camera_centre_location.x, camera_centre_location.y, camera_centre_location.z); right_camera_location = right_camera_locn.translate(camera_centre_location.x, camera_centre_location.y, camera_centre_location.z); right_camera_location.pan = left_camera_location.pan; }
public void RotatePoint() { pos3D point = new pos3D(0, 100, 0); point.pan = DegreesToRadians(10); pos3D rotated = point.rotate(DegreesToRadians(30), 0, 0); int pan = (int)Math.Round(RadiansToDegrees(rotated.pan)); Assert.AreEqual(40, pan, "pan positive"); Assert.AreEqual(50, (int)Math.Round(rotated.x), "pan positive x"); Assert.AreEqual(87, (int)Math.Round(rotated.y), "pan positive y"); rotated = point.rotate(DegreesToRadians(-30), 0, 0); pan = (int)Math.Round(RadiansToDegrees(rotated.pan)); Assert.AreEqual(-20, pan, "pan negative"); Assert.AreEqual(-50, (int)Math.Round(rotated.x), "pan negative x"); Assert.AreEqual(87, (int)Math.Round(rotated.y), "pan negative y"); point.pan = 0; point.tilt = DegreesToRadians(5); pos3D tilted = point.rotate(0, DegreesToRadians(30), 0); int tilt = (int)Math.Round(RadiansToDegrees(tilted.tilt)); Assert.AreEqual(35, tilt, "tilt positive"); point.pan = 0; point.tilt = 0; point.roll = DegreesToRadians(2); pos3D rolled = point.rotate(0, 0, DegreesToRadians(-20)); int roll = (int)Math.Round(RadiansToDegrees(rolled.roll)); Assert.AreEqual(-18, roll, "roll negative"); //Console.WriteLine("x = " + rotated.x.ToString()); //Console.WriteLine("y = " + rotated.y.ToString()); }
public void PanTilt() { int pan_angle1 = -40; float pan1 = pan_angle1 * (float)Math.PI / 180.0f; int tilt_angle1 = 20; float tilt1 = tilt_angle1 * (float)Math.PI / 180.0f; pos3D pos1 = new pos3D(0, 50, 0); pos3D pos2 = pos1.rotate_old(pan1,tilt1,0); pos3D pos3 = pos1.rotate(pan1,tilt1,0); float dx = Math.Abs(pos2.x - pos3.x); float dy = Math.Abs(pos2.y - pos3.y); float dz = Math.Abs(pos2.z - pos3.z); Console.WriteLine("pos old: " + pos2.x.ToString() + ", " + pos2.y.ToString() + ", " + pos2.z.ToString()); Console.WriteLine("pos new: " + pos3.x.ToString() + ", " + pos3.y.ToString() + ", " + pos3.z.ToString()); Assert.Less(dx, 1); Assert.Less(dy, 1); Assert.Less(dz, 1); }
public void Roll() { int roll_angle1 = 20; float roll1 = roll_angle1 * (float)Math.PI / 180.0f; pos3D pos1 = new pos3D(50, 0, 0); pos3D pos2 = pos1.rotate_old(0,0,roll1); pos3D pos3 = pos1.rotate(0,0,roll1); float dx = Math.Abs(pos2.x - pos3.x); float dy = Math.Abs(pos2.y - pos3.y); float dz = Math.Abs(pos2.z - pos3.z); Console.WriteLine("pos old: " + pos2.x.ToString() + ", " + pos2.y.ToString() + ", " + pos2.z.ToString()); Console.WriteLine("pos new: " + pos3.x.ToString() + ", " + pos3.y.ToString() + ", " + pos3.z.ToString()); Assert.Less(dx, 1); Assert.Less(dy, 1); Assert.Less(dz, 1); }
/// <summary> /// Localisation /// </summary> /// <param name="body_width_mm">width of the robot body in millimetres</param> /// <param name="body_length_mm">length of the robot body in millimetres</param> /// <param name="body_centre_of_rotation_x">x centre of rotation of the robot, relative to the top left corner</param> /// <param name="body_centre_of_rotation_y">y centre of rotation of the robot, relative to the top left corner</param> /// <param name="body_centre_of_rotation_z">z centre of rotation of the robot, relative to the top left corner</param> /// <param name="head_centroid_x">head centroid x position in millimetres relative to the top left corner of the body</param> /// <param name="head_centroid_y">head centroid y position in millimetres relative to the top left corner of the body</param> /// <param name="head_centroid_z">head centroid z position in millimetres relative to the top left corner of the body</param> /// <param name="head_pan">head pan angle in radians</param> /// <param name="head_tilt">head tilt angle in radians</param> /// <param name="head_roll">head roll angle in radians</param> /// <param name="baseline_mm">stereo camera baseline in millimetres</param> /// <param name="stereo_camera_position_x">stereo camera x position in millimetres relative to the head centroid</param> /// <param name="stereo_camera_position_y">stereo camera y position in millimetres relative to the head centroid</param> /// <param name="stereo_camera_position_z">stereo camera z position in millimetres relative to the head centroid</param> /// <param name="stereo_camera_pan">stereo camera pan in radians relative to the head</param> /// <param name="stereo_camera_tilt">stereo camera tilt in radians relative to the head</param> /// <param name="stereo_camera_roll">stereo camera roll in radians relative to the head</param> /// <param name="image_width">image width for each stereo camera</param> /// <param name="image_height">image height for each stereo camera</param> /// <param name="FOV_degrees">field of view for each stereo camera in degrees</param> /// <param name="stereo_features">stereo features (disparities) for each stereo camera</param> /// <param name="stereo_features_colour">stereo feature colours for each stereo camera</param> /// <param name="stereo_features_uncertainties">stereo feature uncertainties (priors) for each stereo camera</param> /// <param name="sensormodel">sensor model for each stereo camera</param> /// <param name="left_camera_location">returned position and orientation of the left camera on each stereo camera</param> /// <param name="right_camera_location">returned position and orientation of the right camera on each stereo camera</param> /// <param name="no_of_samples">number of sample poses</param> /// <param name="sampling_radius_major_mm">major radius for samples, in the direction of robot movement</param> /// <param name="sampling_radius_minor_mm">minor radius for samples, perpendicular to the direction of robot movement</param> /// <param name="robot_pose">current estimated position and orientation of the robots centre of rotation, from which each stereo camera took its observations</param> /// <param name="max_orientation_variance">maximum variance in orientation in radians, used to create sample poses</param> /// <param name="max_tilt_variance">maximum variance in tilt angle in radians, used to create sample poses</param> /// <param name="max_roll_variance">maximum variance in roll angle in radians, used to create sample poses</param> /// <param name="poses">list of poses tried</param> /// <param name="pose_score">list of pose matching scores</param> /// <param name="pose_offset">offset of the best pose from the current one</param> /// <param name="rnd">random number generator</param> /// <param name="pose_offset">returned pose offset</param> /// <param name="img_poses">optional image within which to show poses</param> /// <param name="img_poses_width">width of the poses image</param> /// <param name="img_poses_height">height of the poses image</param> /// <returns>best localisation matching score</returns> public float Localise( float body_width_mm, float body_length_mm, float body_centre_of_rotation_x, float body_centre_of_rotation_y, float body_centre_of_rotation_z, float head_centroid_x, float head_centroid_y, float head_centroid_z, float head_pan, float head_tilt, float head_roll, float[] baseline_mm, float[] stereo_camera_position_x, float[] stereo_camera_position_y, float[] stereo_camera_position_z, float[] stereo_camera_pan, float[] stereo_camera_tilt, float[] stereo_camera_roll, int[] image_width, int[] image_height, float[] FOV_degrees, float[][] stereo_features, byte[][,] stereo_features_colour, float[][] stereo_features_uncertainties, stereoModel[][] sensormodel, ref pos3D[] left_camera_location, ref pos3D[] right_camera_location, int no_of_samples, float sampling_radius_major_mm, float sampling_radius_minor_mm, pos3D[] robot_pose, float max_orientation_variance, float max_tilt_variance, float max_roll_variance, List<pos3D> poses, List<float> pose_score, Random rnd, ref pos3D pose_offset, byte[] img_poses, int img_poses_width, int img_poses_height, float known_best_pose_x_mm, float known_best_pose_y_mm) { float best_matching_score = float.MinValue; poses.Clear(); pose_score.Clear(); gridCells.CreateMoireGrid( sampling_radius_major_mm, sampling_radius_minor_mm, no_of_samples, robot_pose[0].pan, robot_pose[0].tilt, robot_pose[0].roll, max_orientation_variance, max_tilt_variance, max_roll_variance, rnd, ref poses, null, null, 0, 0); // positions of the left and right camera relative to the robots centre of rotation pos3D head_location = new pos3D(0, 0, 0); left_camera_location = new pos3D[baseline_mm.Length]; right_camera_location = new pos3D[baseline_mm.Length]; pos3D[] camera_centre_location = new pos3D[baseline_mm.Length]; pos3D[] relative_left_cam = new pos3D[baseline_mm.Length]; pos3D[] relative_right_cam = new pos3D[baseline_mm.Length]; for (int cam = 0; cam < baseline_mm.Length; cam++) { occupancygridBase.calculateCameraPositions( body_width_mm, body_length_mm, body_centre_of_rotation_x, body_centre_of_rotation_y, body_centre_of_rotation_z, head_centroid_x, head_centroid_y, head_centroid_z, head_pan, head_tilt, head_roll, baseline_mm[cam], stereo_camera_position_x[cam], stereo_camera_position_y[cam], stereo_camera_position_z[cam], stereo_camera_pan[cam], stereo_camera_tilt[cam], stereo_camera_roll[cam], ref head_location, ref camera_centre_location[cam], ref relative_left_cam[cam], ref relative_right_cam[cam]); left_camera_location[cam] = relative_left_cam[cam].translate(robot_pose[cam].x, robot_pose[cam].y, robot_pose[cam].z); right_camera_location[cam] = relative_right_cam[cam].translate(robot_pose[cam].x, robot_pose[cam].y, robot_pose[cam].z); } pose_score.Clear(); for (int p = 0; p < poses.Count; p++) { pose_score.Add(0); } int no_of_zero_probabilities = 0; // try a number of random poses // we can do this in parallel Parallel.For(0, poses.Count, delegate(int i) //for (int i = 0; i < poses.Count; i++) { pos3D sample_pose = poses[i]; float matching_score = 0; for (int cam = 0; cam < baseline_mm.Length; cam++) { // update the position of the left camera for this pose pos3D sample_pose_left_cam = relative_left_cam[cam].add(sample_pose); sample_pose_left_cam.pan = 0; sample_pose_left_cam.tilt = 0; sample_pose_left_cam.roll = 0; sample_pose_left_cam = sample_pose_left_cam.rotate(sample_pose.pan, sample_pose.tilt, sample_pose.roll); sample_pose_left_cam.x += robot_pose[cam].x; sample_pose_left_cam.y += robot_pose[cam].y; sample_pose_left_cam.z += robot_pose[cam].z; // update the position of the right camera for this pose pos3D sample_pose_right_cam = relative_right_cam[cam].add(sample_pose); sample_pose_right_cam.pan = 0; sample_pose_right_cam.tilt = 0; sample_pose_right_cam.roll = 0; sample_pose_right_cam = sample_pose_right_cam.rotate(sample_pose.pan, sample_pose.tilt, sample_pose.roll); sample_pose_right_cam.x += robot_pose[cam].x; sample_pose_right_cam.y += robot_pose[cam].y; sample_pose_right_cam.z += robot_pose[cam].z; // centre position between the left and right cameras pos3D stereo_camera_centre = new pos3D( sample_pose_left_cam.x + ((sample_pose_right_cam.x - sample_pose_left_cam.x) * 0.5f), sample_pose_left_cam.y + ((sample_pose_right_cam.y - sample_pose_left_cam.y) * 0.5f), sample_pose_left_cam.z + ((sample_pose_right_cam.z - sample_pose_left_cam.z) * 0.5f)); stereo_camera_centre.pan = head_pan + stereo_camera_pan[cam] + sample_pose.pan; stereo_camera_centre.tilt = head_tilt + stereo_camera_tilt[cam] + sample_pose.tilt; stereo_camera_centre.roll = head_roll + stereo_camera_roll[cam] + sample_pose.roll; // create a set of stereo rays as observed from this pose List<evidenceRay> rays = sensormodel[cam][0].createObservation( stereo_camera_centre, baseline_mm[cam], image_width[cam], image_height[cam], FOV_degrees[cam], stereo_features[cam], stereo_features_colour[cam], stereo_features_uncertainties[cam], true); // insert rays into the occupancy grid for (int r = 0; r < rays.Count; r++) { float score = Insert(rays[r], sensormodel[cam], sample_pose_left_cam, sample_pose_right_cam, true); if (score != occupancygridBase.NO_OCCUPANCY_EVIDENCE) { if (matching_score == occupancygridBase.NO_OCCUPANCY_EVIDENCE) matching_score = 0; matching_score += score; } } } // add the pose to the list sample_pose.pan -= robot_pose[0].pan; sample_pose.tilt -= robot_pose[0].tilt; sample_pose.roll -= robot_pose[0].roll; if (Math.Abs(sample_pose.pan) > max_orientation_variance) { Console.WriteLine("Pose variance out of range"); } if (matching_score != occupancygridBase.NO_OCCUPANCY_EVIDENCE) { float prob = probabilities.LogOddsToProbability(matching_score); if (prob == 0) no_of_zero_probabilities++; pose_score[i] = prob; //pose_score[i] = matching_score; } } ); if (no_of_zero_probabilities == poses.Count) { Console.WriteLine("Localisation failure"); pose_offset = new pos3D(0, 0, 0); best_matching_score = occupancygridBase.NO_OCCUPANCY_EVIDENCE; } else { // locate the best possible pose pos3D best_robot_pose = new pos3D(0, 0, 0); gridCells.FindBestPose(poses, pose_score, ref best_robot_pose, sampling_radius_major_mm, img_poses, null, img_poses_width, img_poses_height, known_best_pose_x_mm, known_best_pose_y_mm); // rotate the best pose to the robot's current orientation // this becomes an offset, which may be used for course correction pose_offset = best_robot_pose.rotate(robot_pose[0].pan, robot_pose[0].tilt, robot_pose[0].roll); // orientation relative to the current heading pose_offset.pan = best_robot_pose.pan; pose_offset.tilt = best_robot_pose.tilt; pose_offset.roll = best_robot_pose.roll; // range checks if (Math.Abs(pose_offset.pan) > max_orientation_variance) Console.WriteLine("pose_offset pan out of range"); if (Math.Abs(pose_offset.tilt) > max_tilt_variance) Console.WriteLine("pose_offset tilt out of range"); if (Math.Abs(pose_offset.roll) > max_roll_variance) Console.WriteLine("pose_offset roll out of range"); } return (best_matching_score); }
/// <summary> /// Returns positions of grid cells corresponding to a moire grid /// produced by the interference of a pair of hexagonal grids (theta waves) /// </summary> /// <param name="sampling_radius_major_mm">radius of the major axis of the bounding ellipse</param> /// <param name="sampling_radius_minor_mm">radius of the minor axis of the bounding ellipse</param> /// <param name="first_grid_spacing">spacing of the first hexagonal grid (theta wavelength 1)</param> /// <param name="second_grid_spacing">spacing of the second hexagonal grid (theta wavelength 2)</param> /// <param name="first_grid_rotation_degrees">rotation of the first grid (theta field 1)</param> /// <param name="second_grid_rotation_degrees">rotation of the second grid (theta field 2)</param> /// <param name="dimension_x_cells">number of grid cells in the x axis</param> /// <param name="dimension_y_cells">number of grid cells in the y axis</param> /// <param name="img">image data</param> /// <param name="img_width">image width</param> /// <param name="img_height">image height</param> public static void ShowMoireGrid( float sampling_radius_major_mm, float sampling_radius_minor_mm, float first_grid_spacing, float second_grid_spacing, float first_grid_rotation_degrees, float second_grid_rotation_degrees, int dimension_x_cells, int dimension_y_cells, float scaling_factor, byte[] img, int img_width, int img_height) { float[,,] grid1 = new float[dimension_x_cells, dimension_y_cells, 2]; float[,,] grid2 = new float[dimension_x_cells, dimension_y_cells, 2]; float rotation = 0; float min_x = float.MaxValue; float max_x = float.MinValue; float min_y = float.MaxValue; float max_y = float.MinValue; float radius_sqr = sampling_radius_minor_mm * sampling_radius_minor_mm; for (int grd = 0; grd < 2; grd++) { float spacing = first_grid_spacing; if (grd == 1) { spacing = second_grid_spacing; } float half_grid_spacing = spacing / 2; float half_x = spacing * dimension_x_cells / 2; float half_y = spacing * dimension_y_cells / 2; if (grd == 0) { rotation = first_grid_rotation_degrees * (float)Math.PI / 180; } else { rotation = second_grid_rotation_degrees * (float)Math.PI / 180; } for (int cell_x = 0; cell_x < dimension_x_cells; cell_x++) { for (int cell_y = 0; cell_y < dimension_y_cells; cell_y++) { float x = (cell_x * spacing) - half_x; if (cell_y % 2 == 0) { x += half_grid_spacing; } float y = (cell_y * spacing) - half_y; x *= scaling_factor; y *= scaling_factor; pos3D p = new pos3D(x, y, 0); p = p.rotate(rotation, 0, 0); if (grd == 0) { grid1[cell_x, cell_y, 0] = p.x; grid1[cell_x, cell_y, 1] = p.y; } else { grid2[cell_x, cell_y, 0] = p.x; grid2[cell_x, cell_y, 1] = p.y; } if (p.x < min_x) { min_x = p.x; } if (p.y < min_y) { min_y = p.y; } if (p.x > max_x) { max_x = p.x; } if (p.y > max_y) { max_y = p.y; } } } } for (int i = (img_width * img_height * 3) - 1; i >= 0; i--) { img[i] = 0; } int radius = img_width / (dimension_x_cells * 350 / 100); if (radius < 2) { radius = 2; } int r, g, b; for (int grd = 0; grd < 2; grd++) { float[,,] grid = grid1; r = 5; g = -1; b = -1; if (grd == 1) { grid = grid2; r = -1; g = 5; b = -1; } for (int cell_x = 0; cell_x < dimension_x_cells; cell_x++) { for (int cell_y = 0; cell_y < dimension_y_cells; cell_y++) { int x = ((img_width - img_height) / 2) + (int)((grid[cell_x, cell_y, 0] - min_x) * img_height / (max_x - min_x)); int y = (int)((grid[cell_x, cell_y, 1] - min_y) * img_height / (max_y - min_y)); float dx = grid[cell_x, cell_y, 0]; float dy = (grid[cell_x, cell_y, 1] * sampling_radius_minor_mm) / sampling_radius_major_mm; float dist = dx * dx + dy * dy; if (dist <= radius_sqr) { drawing.drawSpotOverlay(img, img_width, img_height, x, y, radius, r, g, b); } } } for (int i = (img_width * img_height * 3) - 3; i >= 2; i -= 3) { if ((img[i + 2] > 0) && (img[i + 1] > 0)) { img[i + 2] = 255; img[i + 1] = 255; } } } }
/// <summary> /// Returns positions of grid cells corresponding to a moire grid /// produced by the interference of a pair of hexagonal grids (theta waves) /// </summary> /// <param name="first_grid_spacing">spacing of the first hexagonal grid (theta wavelength 1)</param> /// <param name="second_grid_spacing">spacing of the second hexagonal grid (theta wavelength 2)</param> /// <param name="phase_major_degrees">phase precession in the direction of motion in degrees</param> /// <param name="phase_minor_degrees">phase precession perpendicular to the direction of motion in degrees</param> /// <param name="first_grid_rotation_degrees">rotation of the first grid (theta field 1)</param> /// <param name="second_grid_rotation_degrees">rotation of the second grid (theta field 2)</param> /// <param name="dimension_x_cells">number of grid cells in the x axis</param> /// <param name="dimension_y_cells">number of grid cells in the y axis</param> /// <param name="scaling_factor">scaling factor (k)</param> /// <param name="cells">returned grid cell positions</param> public static void CreateMoireGrid( float sampling_radius_major_mm, float sampling_radius_minor_mm, float first_grid_spacing, float second_grid_spacing, float first_grid_rotation_degrees, float second_grid_rotation_degrees, int dimension_x_cells, int dimension_y_cells, float scaling_factor, ref List <pos3D> cells) { cells.Clear(); float scaling = 0; float orientation = 0; float dx, dy; // compute scaling and orientation of the grid MoireGrid( first_grid_spacing, second_grid_spacing, first_grid_rotation_degrees, second_grid_rotation_degrees, scaling_factor, ref scaling, ref orientation); float moire_grid_spacing = first_grid_spacing * scaling; float half_grid_spacing = moire_grid_spacing / 2; float radius_sqr = sampling_radius_minor_mm * sampling_radius_minor_mm; Console.WriteLine("moire_grid_spacing: " + moire_grid_spacing.ToString()); Console.WriteLine("radius_sqr: " + radius_sqr.ToString()); float half_x = dimension_x_cells * moire_grid_spacing / 2.0f; float half_y = dimension_y_cells * moire_grid_spacing / 2.0f; for (int cell_x = 0; cell_x < dimension_x_cells; cell_x++) { for (int cell_y = 0; cell_y < dimension_y_cells; cell_y++) { float x = (cell_x * moire_grid_spacing) - half_x; if (cell_y % 2 == 0) { x += half_grid_spacing; } float y = (cell_y * moire_grid_spacing) - half_y; pos3D grid_cell = new pos3D(x, y, 0); grid_cell = grid_cell.rotate(-orientation, 0, 0); dx = grid_cell.x; dy = (grid_cell.y * sampling_radius_minor_mm) / sampling_radius_major_mm; float dist = dx * dx + dy * dy; if (dist <= radius_sqr) { cells.Add(grid_cell); } } } }
/// <summary> /// Returns positions of grid cells corresponding to a moire grid /// produced by the interference of a pair of hexagonal grids (theta waves) /// </summary> /// <param name="sampling_radius_major_mm">radius of the major axis of the bounding ellipse</param> /// <param name="sampling_radius_minor_mm">radius of the minor axis of the bounding ellipse</param> /// <param name="first_grid_spacing">spacing of the first hexagonal grid (theta wavelength 1)</param> /// <param name="second_grid_spacing">spacing of the second hexagonal grid (theta wavelength 2)</param> /// <param name="first_grid_rotation_degrees">rotation of the first grid (theta field 1)</param> /// <param name="second_grid_rotation_degrees">rotation of the second grid (theta field 2)</param> /// <param name="dimension_x_cells">number of grid cells in the x axis</param> /// <param name="dimension_y_cells">number of grid cells in the y axis</param> /// <param name="img">image data</param> /// <param name="img_width">image width</param> /// <param name="img_height">image height</param> public static void ShowMoireGrid( float sampling_radius_major_mm, float sampling_radius_minor_mm, float first_grid_spacing, float second_grid_spacing, float first_grid_rotation_degrees, float second_grid_rotation_degrees, int dimension_x_cells, int dimension_y_cells, float scaling_factor, byte[] img, int img_width, int img_height) { float[,,] grid1 = new float[dimension_x_cells, dimension_y_cells, 2]; float[,,] grid2 = new float[dimension_x_cells, dimension_y_cells, 2]; float rotation = 0; float min_x = float.MaxValue; float max_x = float.MinValue; float min_y = float.MaxValue; float max_y = float.MinValue; float radius_sqr = sampling_radius_minor_mm*sampling_radius_minor_mm; for (int grd = 0; grd < 2; grd++) { float spacing = first_grid_spacing; if (grd == 1) spacing = second_grid_spacing; float half_grid_spacing = spacing/2; float half_x = spacing * dimension_x_cells / 2; float half_y = spacing * dimension_y_cells / 2; if (grd == 0) rotation = first_grid_rotation_degrees * (float)Math.PI / 180; else rotation = second_grid_rotation_degrees * (float)Math.PI / 180; for (int cell_x = 0; cell_x < dimension_x_cells; cell_x++) { for (int cell_y = 0; cell_y < dimension_y_cells; cell_y++) { float x = (cell_x * spacing) - half_x; if (cell_y % 2 == 0) x += half_grid_spacing; float y = (cell_y * spacing) - half_y; x *= scaling_factor; y *= scaling_factor; pos3D p = new pos3D(x,y,0); p = p.rotate(rotation, 0, 0); if (grd == 0) { grid1[cell_x, cell_y, 0] = p.x; grid1[cell_x, cell_y, 1] = p.y; } else { grid2[cell_x, cell_y, 0] = p.x; grid2[cell_x, cell_y, 1] = p.y; } if (p.x < min_x) min_x = p.x; if (p.y < min_y) min_y = p.y; if (p.x > max_x) max_x = p.x; if (p.y > max_y) max_y = p.y; } } } for (int i = (img_width * img_height * 3)-1; i >= 0; i--) img[i] = 0; int radius = img_width / (dimension_x_cells*350/100); if (radius < 2) radius = 2; int r,g,b; for (int grd = 0; grd < 2; grd++) { float[,,] grid = grid1; r = 5; g = -1; b = -1; if (grd == 1) { grid = grid2; r = -1; g = 5; b = -1; } for (int cell_x = 0; cell_x < dimension_x_cells; cell_x++) { for (int cell_y = 0; cell_y < dimension_y_cells; cell_y++) { int x = ((img_width-img_height)/2) + (int)((grid[cell_x, cell_y, 0] - min_x) * img_height / (max_x - min_x)); int y = (int)((grid[cell_x, cell_y, 1] - min_y) * img_height / (max_y - min_y)); float dx = grid[cell_x, cell_y, 0]; float dy = (grid[cell_x, cell_y, 1] * sampling_radius_minor_mm) / sampling_radius_major_mm; float dist = dx*dx + dy*dy; if (dist <= radius_sqr) { drawing.drawSpotOverlay(img, img_width, img_height, x,y,radius,r,g,b); } } } for (int i = (img_width * img_height * 3)-3; i >= 2; i-=3) { if ((img[i+2] > 0) && (img[i+1] > 0)) { img[i+2] = 255; img[i+1] = 255; } } } }
/// <summary> /// Returns positions of grid cells corresponding to a moire grid /// produced by the interference of a pair of hexagonal grids (theta waves) /// </summary> /// <param name="first_grid_spacing">spacing of the first hexagonal grid (theta wavelength 1)</param> /// <param name="second_grid_spacing">spacing of the second hexagonal grid (theta wavelength 2)</param> /// <param name="phase_major_degrees">phase precession in the direction of motion in degrees</param> /// <param name="phase_minor_degrees">phase precession perpendicular to the direction of motion in degrees</param> /// <param name="first_grid_rotation_degrees">rotation of the first grid (theta field 1)</param> /// <param name="second_grid_rotation_degrees">rotation of the second grid (theta field 2)</param> /// <param name="dimension_x_cells">number of grid cells in the x axis</param> /// <param name="dimension_y_cells">number of grid cells in the y axis</param> /// <param name="scaling_factor">scaling factor (k)</param> /// <param name="cells">returned grid cell positions</param> public static void CreateMoireGrid( float sampling_radius_major_mm, float sampling_radius_minor_mm, float first_grid_spacing, float second_grid_spacing, float first_grid_rotation_degrees, float second_grid_rotation_degrees, int dimension_x_cells, int dimension_y_cells, float scaling_factor, ref List<pos3D> cells) { cells.Clear(); float scaling = 0; float orientation = 0; float dx, dy; // compute scaling and orientation of the grid MoireGrid( first_grid_spacing, second_grid_spacing, first_grid_rotation_degrees, second_grid_rotation_degrees, scaling_factor, ref scaling, ref orientation); float moire_grid_spacing = first_grid_spacing * scaling; float half_grid_spacing = moire_grid_spacing/2; float radius_sqr = sampling_radius_minor_mm*sampling_radius_minor_mm; Console.WriteLine("moire_grid_spacing: " + moire_grid_spacing.ToString()); Console.WriteLine("radius_sqr: " + radius_sqr.ToString()); float half_x = dimension_x_cells * moire_grid_spacing / 2.0f; float half_y = dimension_y_cells * moire_grid_spacing / 2.0f; for (int cell_x = 0; cell_x < dimension_x_cells; cell_x++) { for (int cell_y = 0; cell_y < dimension_y_cells; cell_y++) { float x = (cell_x * moire_grid_spacing) - half_x; if (cell_y % 2 == 0) x += half_grid_spacing; float y = (cell_y * moire_grid_spacing) - half_y; pos3D grid_cell = new pos3D(x, y, 0); grid_cell = grid_cell.rotate(-orientation, 0, 0); dx = grid_cell.x; dy = (grid_cell.y * sampling_radius_minor_mm) / sampling_radius_major_mm; float dist = dx*dx + dy*dy; if (dist <= radius_sqr) { cells.Add(grid_cell); } } } }
/// <summary> /// Localisation /// </summary> /// <param name="body_width_mm">width of the robot body in millimetres</param> /// <param name="body_length_mm">length of the robot body in millimetres</param> /// <param name="body_centre_of_rotation_x">x centre of rotation of the robot, relative to the top left corner</param> /// <param name="body_centre_of_rotation_y">y centre of rotation of the robot, relative to the top left corner</param> /// <param name="body_centre_of_rotation_z">z centre of rotation of the robot, relative to the top left corner</param> /// <param name="head_centroid_x">head centroid x position in millimetres relative to the top left corner of the body</param> /// <param name="head_centroid_y">head centroid y position in millimetres relative to the top left corner of the body</param> /// <param name="head_centroid_z">head centroid z position in millimetres relative to the top left corner of the body</param> /// <param name="head_pan">head pan angle in radians</param> /// <param name="head_tilt">head tilt angle in radians</param> /// <param name="head_roll">head roll angle in radians</param> /// <param name="baseline_mm">stereo camera baseline in millimetres</param> /// <param name="stereo_camera_position_x">stereo camera x position in millimetres relative to the head centroid</param> /// <param name="stereo_camera_position_y">stereo camera y position in millimetres relative to the head centroid</param> /// <param name="stereo_camera_position_z">stereo camera z position in millimetres relative to the head centroid</param> /// <param name="stereo_camera_pan">stereo camera pan in radians relative to the head</param> /// <param name="stereo_camera_tilt">stereo camera tilt in radians relative to the head</param> /// <param name="stereo_camera_roll">stereo camera roll in radians relative to the head</param> /// <param name="image_width">image width for each stereo camera</param> /// <param name="image_height">image height for each stereo camera</param> /// <param name="FOV_degrees">field of view for each stereo camera in degrees</param> /// <param name="stereo_features">stereo features (disparities) for each stereo camera</param> /// <param name="stereo_features_colour">stereo feature colours for each stereo camera</param> /// <param name="stereo_features_uncertainties">stereo feature uncertainties (priors) for each stereo camera</param> /// <param name="sensormodel">sensor model for each stereo camera</param> /// <param name="left_camera_location">returned position and orientation of the left camera on each stereo camera</param> /// <param name="right_camera_location">returned position and orientation of the right camera on each stereo camera</param> /// <param name="no_of_samples">number of sample poses</param> /// <param name="sampling_radius_major_mm">major radius for samples, in the direction of robot movement</param> /// <param name="sampling_radius_minor_mm">minor radius for samples, perpendicular to the direction of robot movement</param> /// <param name="robot_pose">current estimated position and orientation of the robots centre of rotation, from which each stereo camera took its observations</param> /// <param name="max_orientation_variance">maximum variance in orientation in radians, used to create sample poses</param> /// <param name="max_tilt_variance">maximum variance in tilt angle in radians, used to create sample poses</param> /// <param name="max_roll_variance">maximum variance in roll angle in radians, used to create sample poses</param> /// <param name="poses">list of poses tried</param> /// <param name="pose_score">list of pose matching scores</param> /// <param name="pose_offset">offset of the best pose from the current one</param> /// <param name="rnd">random number generator</param> /// <param name="pose_offset">returned pose offset</param> /// <param name="img_poses">optional image within which to show poses</param> /// <param name="img_poses_width">width of the poses image</param> /// <param name="img_poses_height">height of the poses image</param> /// <returns>best localisation matching score</returns> public float Localise( float body_width_mm, float body_length_mm, float body_centre_of_rotation_x, float body_centre_of_rotation_y, float body_centre_of_rotation_z, float head_centroid_x, float head_centroid_y, float head_centroid_z, float head_pan, float head_tilt, float head_roll, float[] baseline_mm, float[] stereo_camera_position_x, float[] stereo_camera_position_y, float[] stereo_camera_position_z, float[] stereo_camera_pan, float[] stereo_camera_tilt, float[] stereo_camera_roll, int[] image_width, int[] image_height, float[] FOV_degrees, float[][] stereo_features, byte[][,] stereo_features_colour, float[][] stereo_features_uncertainties, stereoModel[][] sensormodel, ref pos3D[] left_camera_location, ref pos3D[] right_camera_location, int no_of_samples, float sampling_radius_major_mm, float sampling_radius_minor_mm, pos3D[] robot_pose, float max_orientation_variance, float max_tilt_variance, float max_roll_variance, List <pos3D> poses, List <float> pose_score, Random rnd, ref pos3D pose_offset, byte[] img_poses, int img_poses_width, int img_poses_height, float known_best_pose_x_mm, float known_best_pose_y_mm) { float best_matching_score = float.MinValue; poses.Clear(); pose_score.Clear(); gridCells.CreateMoireGrid( sampling_radius_major_mm, sampling_radius_minor_mm, no_of_samples, robot_pose[0].pan, robot_pose[0].tilt, robot_pose[0].roll, max_orientation_variance, max_tilt_variance, max_roll_variance, rnd, ref poses, null, null, 0, 0); // positions of the left and right camera relative to the robots centre of rotation pos3D head_location = new pos3D(0, 0, 0); left_camera_location = new pos3D[baseline_mm.Length]; right_camera_location = new pos3D[baseline_mm.Length]; pos3D[] camera_centre_location = new pos3D[baseline_mm.Length]; pos3D[] relative_left_cam = new pos3D[baseline_mm.Length]; pos3D[] relative_right_cam = new pos3D[baseline_mm.Length]; for (int cam = 0; cam < baseline_mm.Length; cam++) { occupancygridBase.calculateCameraPositions( body_width_mm, body_length_mm, body_centre_of_rotation_x, body_centre_of_rotation_y, body_centre_of_rotation_z, head_centroid_x, head_centroid_y, head_centroid_z, head_pan, head_tilt, head_roll, baseline_mm[cam], stereo_camera_position_x[cam], stereo_camera_position_y[cam], stereo_camera_position_z[cam], stereo_camera_pan[cam], stereo_camera_tilt[cam], stereo_camera_roll[cam], ref head_location, ref camera_centre_location[cam], ref relative_left_cam[cam], ref relative_right_cam[cam]); left_camera_location[cam] = relative_left_cam[cam].translate(robot_pose[cam].x, robot_pose[cam].y, robot_pose[cam].z); right_camera_location[cam] = relative_right_cam[cam].translate(robot_pose[cam].x, robot_pose[cam].y, robot_pose[cam].z); } pose_score.Clear(); for (int p = 0; p < poses.Count; p++) { pose_score.Add(0); } int no_of_zero_probabilities = 0; // try a number of random poses // we can do this in parallel Parallel.For(0, poses.Count, delegate(int i) //for (int i = 0; i < poses.Count; i++) { pos3D sample_pose = poses[i]; float matching_score = 0; for (int cam = 0; cam < baseline_mm.Length; cam++) { // update the position of the left camera for this pose pos3D sample_pose_left_cam = relative_left_cam[cam].add(sample_pose); sample_pose_left_cam.pan = 0; sample_pose_left_cam.tilt = 0; sample_pose_left_cam.roll = 0; sample_pose_left_cam = sample_pose_left_cam.rotate(sample_pose.pan, sample_pose.tilt, sample_pose.roll); sample_pose_left_cam.x += robot_pose[cam].x; sample_pose_left_cam.y += robot_pose[cam].y; sample_pose_left_cam.z += robot_pose[cam].z; // update the position of the right camera for this pose pos3D sample_pose_right_cam = relative_right_cam[cam].add(sample_pose); sample_pose_right_cam.pan = 0; sample_pose_right_cam.tilt = 0; sample_pose_right_cam.roll = 0; sample_pose_right_cam = sample_pose_right_cam.rotate(sample_pose.pan, sample_pose.tilt, sample_pose.roll); sample_pose_right_cam.x += robot_pose[cam].x; sample_pose_right_cam.y += robot_pose[cam].y; sample_pose_right_cam.z += robot_pose[cam].z; // centre position between the left and right cameras pos3D stereo_camera_centre = new pos3D( sample_pose_left_cam.x + ((sample_pose_right_cam.x - sample_pose_left_cam.x) * 0.5f), sample_pose_left_cam.y + ((sample_pose_right_cam.y - sample_pose_left_cam.y) * 0.5f), sample_pose_left_cam.z + ((sample_pose_right_cam.z - sample_pose_left_cam.z) * 0.5f)); stereo_camera_centre.pan = head_pan + stereo_camera_pan[cam] + sample_pose.pan; stereo_camera_centre.tilt = head_tilt + stereo_camera_tilt[cam] + sample_pose.tilt; stereo_camera_centre.roll = head_roll + stereo_camera_roll[cam] + sample_pose.roll; // create a set of stereo rays as observed from this pose List <evidenceRay> rays = sensormodel[cam][0].createObservation( stereo_camera_centre, baseline_mm[cam], image_width[cam], image_height[cam], FOV_degrees[cam], stereo_features[cam], stereo_features_colour[cam], stereo_features_uncertainties[cam], true); // insert rays into the occupancy grid for (int r = 0; r < rays.Count; r++) { float score = Insert(rays[r], sensormodel[cam], sample_pose_left_cam, sample_pose_right_cam, true); if (score != occupancygridBase.NO_OCCUPANCY_EVIDENCE) { if (matching_score == occupancygridBase.NO_OCCUPANCY_EVIDENCE) { matching_score = 0; } matching_score += score; } } } // add the pose to the list sample_pose.pan -= robot_pose[0].pan; sample_pose.tilt -= robot_pose[0].tilt; sample_pose.roll -= robot_pose[0].roll; if (Math.Abs(sample_pose.pan) > max_orientation_variance) { Console.WriteLine("Pose variance out of range"); } if (matching_score != occupancygridBase.NO_OCCUPANCY_EVIDENCE) { float prob = probabilities.LogOddsToProbability(matching_score); if (prob == 0) { no_of_zero_probabilities++; } pose_score[i] = prob; //pose_score[i] = matching_score; } }); if (no_of_zero_probabilities == poses.Count) { Console.WriteLine("Localisation failure"); pose_offset = new pos3D(0, 0, 0); best_matching_score = occupancygridBase.NO_OCCUPANCY_EVIDENCE; } else { // locate the best possible pose pos3D best_robot_pose = new pos3D(0, 0, 0); gridCells.FindBestPose(poses, pose_score, ref best_robot_pose, sampling_radius_major_mm, img_poses, null, img_poses_width, img_poses_height, known_best_pose_x_mm, known_best_pose_y_mm); // rotate the best pose to the robot's current orientation // this becomes an offset, which may be used for course correction pose_offset = best_robot_pose.rotate(robot_pose[0].pan, robot_pose[0].tilt, robot_pose[0].roll); // orientation relative to the current heading pose_offset.pan = best_robot_pose.pan; pose_offset.tilt = best_robot_pose.tilt; pose_offset.roll = best_robot_pose.roll; // range checks if (Math.Abs(pose_offset.pan) > max_orientation_variance) { Console.WriteLine("pose_offset pan out of range"); } if (Math.Abs(pose_offset.tilt) > max_tilt_variance) { Console.WriteLine("pose_offset tilt out of range"); } if (Math.Abs(pose_offset.roll) > max_roll_variance) { Console.WriteLine("pose_offset roll out of range"); } } return(best_matching_score); }