/// <summary> /// insert a stereo ray into the grid /// </summary> /// <param name="ray">stereo ray</param> /// <param name="sensormodel">sensor model for each grid level</param> /// <param name="left_camera_location">left stereo camera position and orientation</param> /// <param name="right_camera_location">right stereo camera position and orientation</param> /// <param name="localiseOnly">whether we are mapping or localising</param> /// <returns>localisation matching score</returns> public float Insert( evidenceRay ray, stereoModel[] sensormodel, pos3D left_camera_location, pos3D right_camera_location, bool localiseOnly) { float matchingScore = occupancygridBase.NO_OCCUPANCY_EVIDENCE; switch (grid_type) { case TYPE_SIMPLE: { for (int grid_level = 0; grid_level < grid.Length; grid_level++) { rayModelLookup sensormodel_lookup = sensormodel[grid_level].ray_model; occupancygridSimple grd = (occupancygridSimple)grid[grid_level]; float score = grd.Insert(ray, sensormodel_lookup, left_camera_location, right_camera_location, localiseOnly); if (score != occupancygridBase.NO_OCCUPANCY_EVIDENCE) { if (matchingScore == occupancygridBase.NO_OCCUPANCY_EVIDENCE) { matchingScore = 0; } matchingScore += grid_weight[grid_level] * score; } } break; } } return(matchingScore); }
/// <summary> /// insert a stereo ray into the grid /// </summary> /// <param name="ray">stereo ray</param> /// <param name="origin">pose from which this observation was made</param> /// <param name="sensormodel">sensor model for each grid level</param> /// <param name="left_camera_location">left stereo camera position and orientation</param> /// <param name="right_camera_location">right stereo camera position and orientation</param> /// <param name="localiseOnly">whether we are mapping or localising</param> /// <returns>localisation matching score</returns> public float Insert( evidenceRay ray, particlePose origin, stereoModel[] sensormodel, pos3D left_camera_location, pos3D right_camera_location, bool localiseOnly) { float matchingScore = 0; switch (grid_type) { case TYPE_MULTI_HYPOTHESIS: { for (int grid_level = 0; grid_level < grid.Length; grid_level++) { rayModelLookup sensormodel_lookup = sensormodel[grid_level].ray_model; occupancygridMultiHypothesis grd = (occupancygridMultiHypothesis)grid[grid_level]; matchingScore += grid_weight[grid_level] * grd.Insert(ray, origin, sensormodel_lookup, left_camera_location, right_camera_location, localiseOnly); } break; } } return(matchingScore); }
/// <summary> /// update the robot's map /// </summary> /// <param name="state">robot state</param> private static void Update(ThreadMappingState state) { // object used for taking benchmark timings stopwatch clock = new stopwatch(); clock.Start(); // update all current poses with the observed rays state.motion.LocalGrid = state.grid; state.motion.AddObservation(state.stereo_rays, false); clock.Stop(); state.benchmark_observation_update = clock.time_elapsed_mS; // what's the relative position of the robot inside the grid ? pos3D relative_position = new pos3D(state.pose.x - state.grid.x, state.pose.y - state.grid.y, 0); relative_position.pan = state.pose.pan - state.grid.pan; clock.Start(); // garbage collect dead occupancy hypotheses state.grid.GarbageCollect(); clock.Stop(); state.benchmark_garbage_collection = clock.time_elapsed_mS; }
public void show(Byte[] img, int img_width, int img_height, int scale) { if (viewpoints.Count > 0) { // find the centre of the path pos3D centre = pathCentre(); // insert the viewpoints int prev_x = 0; int prev_y = 0; for (int v = 0; v < viewpoints.Count; v++) { viewpoint view = (viewpoint)viewpoints[v]; float x = view.odometry_position.x - centre.x; float y = view.odometry_position.y - centre.y; int screen_x = (int)(x * scale / 10000.0f * img_width / 640) + (img_width / 2); int screen_y = (int)(y * scale / 10000.0f * img_height / 480) + (img_height / 2); if (v > 0) { util.drawLine(img, img_width, img_height, screen_x, screen_y, prev_x, prev_y, 0, 255, 0, v * 4 / viewpoints.Count, false); } prev_x = screen_x; prev_y = screen_y; } } }
/// <summary> /// update the robot's map /// </summary> /// <param name="state">robot state</param> private static void Update(ThreadMappingState state) { // object used for taking benchmark timings stopwatch clock = new stopwatch(); clock.Start(); // update all current poses with the observed rays state.motion.LocalGrid = state.grid; state.motion.AddObservation(state.stereo_rays, false); clock.Stop(); state.benchmark_observation_update = clock.time_elapsed_mS; // what's the relative position of the robot inside the grid ? pos3D relative_position = new pos3D(state.pose.x - state.grid.x, state.pose.y - state.grid.y, 0); relative_position.pan = state.pose.pan - state.grid.pan; clock.Start(); // garbage collect dead occupancy hypotheses state.grid.GarbageCollect(); clock.Stop(); state.benchmark_garbage_collection = clock.time_elapsed_mS; }
public void SubtractPoints() { pos3D[] point = new pos3D[3]; for (int i = 0; i < 2; i++) point[i] = new pos3D(0,0,0); point[0].x = 10; point[0].y = 20; point[0].z = 30; point[0].pan = 1.4f; point[0].tilt = 0.01f; point[0].roll = 0.002f; point[1].x = 40; point[1].y = 50; point[1].z = 60; point[1].pan = 2.1f; point[1].tilt = 0.03f; point[1].roll = 0.001f; point[2] = point[0].subtract(point[1]); Assert.AreEqual(10 - 40, point[2].x, "x"); Assert.AreEqual(20 - 50, point[2].y, "y"); Assert.AreEqual(30 - 60, point[2].z, "z"); Assert.AreEqual(1.4f - 2.1f, point[2].pan, "pan"); Assert.AreEqual(0.01f - 0.03f, point[2].tilt, "tilt"); Assert.AreEqual(0.002f - 0.001f, point[2].roll, "roll"); }
/// <summary> /// translate and rotate the ray by the given values /// </summary> /// <param name="r">object containing the desired translation and rotation</param> public void translateRotate(pos3D r) { //take a note of where the ray was observed from //in egocentric coordinates. This will help to //restrict localisation searching observedFrom = new pos3D(r.x, r.y, r.z); observedFrom.pan = r.pan; observedFrom.tilt = r.tilt; //centre = centre.rotate(r.pan, r.tilt, r.roll); for (int v = 0; v < 2; v++) { vertices[v] = vertices[v].rotate(r.pan, r.tilt, r.roll); } //store the XY plane pan angle, which may later be used //to reduce the searching during localisation pan_angle = vertices[0].new_pan_angle; pan_index = (int)(pan_angle * pan_steps / (2 * 3.1415927f)); start_dist = vertices[0].dist_xy; length = vertices[1].dist_xy - start_dist; //centre = centre.translate(r.x, r.y, r.z); for (int v = 0; v < 2; v++) { vertices[v] = vertices[v].translate(r.x, r.y, r.z); } }
public void AddPoints() { pos3D[] point = new pos3D[3]; for (int i = 0; i < 2; i++) point[i] = new pos3D(0,0,0); point[0].x = 10; point[0].y = 20; point[0].z = 30; point[0].pan = 1.4f; point[0].tilt = 0.01f; point[0].roll = 0.002f; point[1].x = 40; point[1].y = 50; point[1].z = 60; point[1].pan = 2.1f; point[1].tilt = 0.03f; point[1].roll = 0.001f; point[2] = point[0].add(point[1]); Assert.AreEqual(10 + 40, point[2].x, "x"); Assert.AreEqual(20 + 50, point[2].y, "y"); Assert.AreEqual(30 + 60, point[2].z, "z"); Assert.AreEqual(1.4f + 2.1f, point[2].pan, "pan"); Assert.AreEqual(0.01f + 0.03f, point[2].tilt, "tilt"); Assert.AreEqual(0.002f + 0.001f, point[2].roll, "roll"); }
public pos3D subtract(pos3D other) { pos3D sum = new pos3D(x - other.x, y - other.y, z - other.z); sum.pan = pan - other.pan; sum.tilt = tilt - other.tilt; sum.roll = roll - other.roll; return (sum); }
public pos3D add(pos3D other) { pos3D sum = new pos3D(x + other.x, y + other.y, z + other.z); sum.pan = pan + other.pan; sum.tilt = tilt + other.tilt; sum.roll = roll + other.roll; return (sum); }
/// <summary> /// sets the position and orientation of the robots head /// </summary> /// <param name="p"> /// A <see cref="pos3D"/> /// </param> public void SetHeadPositionOrientation(pos3D p) { head_centroid_x = p.x; head_centroid_y = p.y; head_centroid_z = p.z; head_pan = p.pan; head_tilt = p.tilt; head_roll = p.roll; }
public pos3D add(pos3D other) { pos3D sum = new pos3D(x + other.x, y + other.y, z + other.z); sum.pan = pan + other.pan; sum.tilt = tilt + other.tilt; sum.roll = roll + other.roll; return(sum); }
public pos3D Subtract(pos3D p) { pos3D new_p = new pos3D(x - p.x, y - p.y, z - p.z); new_p.pan = pan - p.pan; new_p.tilt = tilt - p.tilt; new_p.roll = roll - p.roll; return(new_p); }
/// <summary> /// copy the position from another /// </summary> /// <param name="other"></param> /// <returns></returns> public void copyFrom(pos3D other) { x = other.x; y = other.y; z = other.z; pan = other.pan; tilt = other.tilt; roll = other.roll; }
/// <summary> /// return a translated version of the point /// </summary> /// <param name="x"></param> /// <param name="y"></param> /// <param name="z"></param> /// <returns></returns> public pos3D translate(float x, float y, float z) { pos3D result = new pos3D(this.x + x, this.y + y, this.z + z); result.pan = pan; result.tilt = tilt; result.roll = roll; return(result); }
public pos3D subtract(pos3D other) { pos3D sum = new pos3D(x - other.x, y - other.y, z - other.z); sum.pan = pan - other.pan; sum.tilt = tilt - other.tilt; sum.roll = roll - other.roll; return(sum); }
public pos3D rotate_old(float pan, float tilt, float roll) { float hyp; pos3D rotated = new pos3D(x,y,z); // roll //if (roll != 0) { hyp = (float)Math.Sqrt((rotated.x * rotated.x) + (rotated.z * rotated.z)); if (hyp > 0) { float roll_angle = (float)Math.Acos(rotated.z / hyp); if (rotated.x < 0) roll_angle = (float)(Math.PI * 2) - roll_angle; float new_roll_angle = roll + roll_angle; rotated.x = hyp * (float)Math.Sin(new_roll_angle); rotated.z = hyp * (float)Math.Cos(new_roll_angle); } } if (tilt != 0) { // tilt hyp = (float)Math.Sqrt((rotated.y * rotated.y) + (rotated.z * rotated.z)); if (hyp > 0) { float tilt_angle = (float)Math.Acos(rotated.y / hyp); if (rotated.z < 0) tilt_angle = (float)(Math.PI * 2) - tilt_angle; float new_tilt_angle = tilt + tilt_angle; rotated.y = hyp * (float)Math.Sin(new_tilt_angle); rotated.z = hyp * (float)Math.Cos(new_tilt_angle); } } //if (pan != 0) { // pan hyp = (float)Math.Sqrt((rotated.x * rotated.x) + (rotated.y * rotated.y)); if (hyp > 0) { float pan_angle = (float)Math.Acos(rotated.y / hyp); if (rotated.x < 0) pan_angle = (float)(Math.PI * 2) - pan_angle; rotated.new_pan_angle = pan - pan_angle; rotated.dist_xy = hyp; rotated.x = hyp * (float)Math.Sin(rotated.new_pan_angle); rotated.y = hyp * (float)Math.Cos(rotated.new_pan_angle); } } rotated.pan = this.pan + pan; rotated.tilt = this.tilt + tilt; rotated.roll = this.roll + roll; return (rotated); }
/// <summary> /// localise and return offset values /// </summary> /// <param name="stereo_features">disparities for each stereo camera (x,y,disparity)</param> /// <param name="stereo_features_colour">colour for each disparity</param> /// <param name="stereo_features_uncertainties">uncertainty for each disparity</param> /// <param name="debug_mapping_filename">optional filename used for saving debugging info</param> /// <param name="known_offset_x_mm">ideal x offset, if known</param> /// <param name="known_offset_y_mm">ideal y offset, if known</param> /// <param name="offset_x_mm">returned x offset</param> /// <param name="offset_y_mm">returned y offset</param> /// <param name="offset_z_mm">returned z offset</param> /// <param name="offset_pan_radians">returned pan</param> /// <param name="offset_tilt_radians">returned tilt</param> /// <param name="offset_roll_radians">returned roll</param> /// <returns>true if the localisation was valid</returns> public bool Localise( float[][] stereo_features, byte[][,] stereo_features_colour, float[][] stereo_features_uncertainties, string debug_mapping_filename, float known_offset_x_mm, float known_offset_y_mm, ref float offset_x_mm, ref float offset_y_mm, ref float offset_z_mm, ref float offset_pan_radians, ref float offset_tilt_radians, ref float offset_roll_radians) { int overall_img_width = 640; int overall_img_height = 480; bool valid_localisation = true; pos3D pose_offset = new pos3D(0, 0, 0); bool buffer_transition = false; float matching_score = buffer.Localise( robot_geometry, stereo_features, stereo_features_colour, stereo_features_uncertainties, rnd, ref pose_offset, ref buffer_transition, debug_mapping_filename, known_offset_x_mm, known_offset_y_mm, overall_map_filename, ref overall_map_img, overall_img_width, overall_img_height, overall_map_dimension_mm, overall_map_centre_x_mm, overall_map_centre_y_mm); if (matching_score == occupancygridBase.NO_OCCUPANCY_EVIDENCE) { valid_localisation = false; } offset_x_mm = pose_offset.x; offset_y_mm = pose_offset.y; offset_z_mm = pose_offset.z; offset_pan_radians = pose_offset.pan; offset_tilt_radians = pose_offset.tilt; offset_roll_radians = pose_offset.roll; return(valid_localisation); }
public void testLocalisation(robot rob, int localisation_track_index, int localisation_test_index, occupancygridMultiResolution grid, int ray_thickness, int no_of_trial_poses, bool pruneSurvey, int survey_diameter_mm, int randomSeed, int pruneThreshold, float survey_angular_offset, float momentum, ref float error_x, ref float error_y, ref float error_pan, pos3D estimatedPos) { robotPath pathLocalisation = getLocalisationTrack(localisation_track_index); robotPath pathMap = getMappingTrack(localisation_track_index); viewpoint view_localisation = pathLocalisation.getViewpoint(localisation_test_index); viewpoint view_map = pathMap.getViewpoint(localisation_test_index); float max_score = 0; // position estimate from odometry, which positions the robot within the grid // as an initial estimate from which to search pos3D local_odometry_position = view_map.odometry_position.subtract(pathMap.pathCentre()); ArrayList survey_results = rob.sensorModelLocalisation.surveyXYP(view_localisation, grid, survey_diameter_mm, survey_diameter_mm, no_of_trial_poses, ray_thickness, pruneSurvey, randomSeed, pruneThreshold, survey_angular_offset, local_odometry_position, momentum, ref max_score); float peak_x = 0; float peak_y = 0; float peak_pan = 0; rob.sensorModelLocalisation.SurveyPeak(survey_results, ref peak_x, ref peak_y); //float[] peak = rob.sensorModel.surveyPan(view_map, view_localisation, separation_tollerance, ray_thickness, peak_x, peak_y); float[] peak = rob.sensorModelLocalisation.surveyPan(view_localisation, grid, ray_thickness, peak_x, peak_y); peak_pan = rob.sensorModelLocalisation.SurveyPeakPan(peak); float half_track_length = 1000; float dx = view_localisation.odometry_position.x - view_map.odometry_position.x; float dy = view_localisation.odometry_position.y - (view_map.odometry_position.y - half_track_length); float dp = view_localisation.odometry_position.pan - view_map.odometry_position.pan; error_x = dx + peak_x; error_y = dy + peak_y; error_pan = dp + peak_pan; error_pan = error_pan / (float)Math.PI * 180; estimatedPos.x = view_map.odometry_position.x - peak_x; estimatedPos.y = view_map.odometry_position.y - peak_y; estimatedPos.pan = view_map.odometry_position.pan - peak_pan; }
/// <summary> /// rotates this position, using Y forward convention /// </summary> /// <param name="pan">pan angle in radians</param> /// <param name="tilt">tilt angle in radians</param> /// <param name="roll">roll angle in radians</param> /// <returns>rotated position</returns> public pos3D rotate(float pan, float tilt, float roll) { float x2 = x; float y2 = y; float z2 = z; float x3 = x; float y3 = y; float z3 = z; // Rotation about the y axis if (roll != 0) { float roll2 = roll + (float)Math.PI; x3 = (float)((Math.Cos(roll2) * x2) + (Math.Sin(roll2) * z2)); z3 = (float)(-(Math.Sin(roll) * x2) + (Math.Cos(roll) * z2)); x2 = x3; z2 = z3; } // Rotatation about the x axis if (tilt != 0) { float tilt2 = tilt; z3 = (float)((Math.Cos(tilt2) * y2) - (Math.Sin(tilt2) * z2)); y3 = (float)((Math.Sin(tilt2) * y2) + (Math.Cos(tilt2) * z2)); y2 = y3; z2 = z3; } // Rotation about the z axis: if (pan != 0) { float pan2 = pan + (float)Math.PI; x3 = (float)((Math.Cos(pan2) * x2) - (Math.Sin(pan2) * y2)); y3 = (float)((Math.Sin(pan) * x2) + (Math.Cos(pan) * y2)); } pos3D rotated = new pos3D(x3, y3, z3); rotated.pan = this.pan + pan; rotated.tilt = this.tilt + tilt; rotated.roll = this.roll + roll; return(rotated); }
public void CreateStereoCameras( int no_of_stereo_cameras, float cam_baseline_mm, float dist_from_centre_of_tilt_mm, int cam_image_width, int cam_image_height, float cam_FOV_degrees, float head_diameter_mm, float default_head_orientation_degrees) { this.head_diameter_mm = head_diameter_mm; pose = new pos3D[no_of_stereo_cameras]; for (int i = 0; i < no_of_stereo_cameras; i++) { pose[i] = new pos3D(0, 0, 0); } baseline_mm = new float[no_of_stereo_cameras]; image_width = new int[no_of_stereo_cameras]; image_height = new int[no_of_stereo_cameras]; FOV_degrees = new float[no_of_stereo_cameras]; stereo_camera_position_x = new float[no_of_stereo_cameras]; stereo_camera_position_y = new float[no_of_stereo_cameras]; stereo_camera_position_z = new float[no_of_stereo_cameras]; stereo_camera_pan = new float[no_of_stereo_cameras]; stereo_camera_tilt = new float[no_of_stereo_cameras]; stereo_camera_roll = new float[no_of_stereo_cameras]; left_camera_location = new pos3D[no_of_stereo_cameras]; right_camera_location = new pos3D[no_of_stereo_cameras]; for (int cam = 0; cam < no_of_stereo_cameras; cam++) { float cam_orientation = cam * (float)Math.PI * 2 / no_of_stereo_cameras; cam_orientation += default_head_orientation_degrees * (float)Math.PI / 180.0f; stereo_camera_position_x[cam] = head_diameter_mm * 0.5f * (float)Math.Sin(cam_orientation); stereo_camera_position_y[cam] = head_diameter_mm * 0.5f * (float)Math.Cos(cam_orientation); stereo_camera_position_z[cam] = dist_from_centre_of_tilt_mm; stereo_camera_pan[cam] = cam_orientation; baseline_mm[cam] = cam_baseline_mm; image_width[cam] = cam_image_width; image_height[cam] = cam_image_height; FOV_degrees[cam] = cam_FOV_degrees; } }
// find the centre of the path public pos3D pathCentre() { float centre_x = 0; float centre_y = 0; for (int v = 0; v < viewpoints.Count; v++) { viewpoint view = (viewpoint)viewpoints[v]; centre_x += view.odometry_position.x; centre_y += view.odometry_position.y; } centre_x /= viewpoints.Count; centre_y /= viewpoints.Count; pos3D centre = new pos3D(centre_x, centre_y, 0); return(centre); }
/// <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 scanMatching[] scanmatch; // simple scan matching /// <summary> /// constructor /// </summary> /// <param name="no_of_stereo_cameras"> /// A <see cref="System.Int32"/> /// The number of stereo cameras /// </param> public stereoHead(int no_of_stereo_cameras) : base(0, 0, 0) { this.no_of_stereo_cameras = no_of_stereo_cameras; // create feature lists features = new stereoFeatures[no_of_stereo_cameras]; // store filenames of the images for each camera imageFilename = new String[no_of_stereo_cameras * 2]; // create the cameras cameraPosition = new pos3D[no_of_stereo_cameras]; // calibration data calibration = new calibrationStereo[no_of_stereo_cameras]; // create objects for each stereo camera for (int cam = 0; cam < no_of_stereo_cameras; cam++) { calibration[cam] = new calibrationStereo(); cameraPosition[cam] = new pos3D(0, 0, 0); features[cam] = null; // new stereoFeatures(); } // sensor models sensormodel = new rayModelLookup[no_of_stereo_cameras]; //scan matching scanmatch = new scanMatching[no_of_stereo_cameras]; /* * if (no_of_cameras == 4) initQuadCam(); * if (no_of_cameras == 2) initDualCam(); * if (no_of_cameras == 1) initSingleStereoCamera(false); */ }
public scanMatching[] scanmatch; // simple scan matching /// <summary> /// constructor /// </summary> /// <param name="no_of_stereo_cameras"> /// A <see cref="System.Int32"/> /// The number of stereo cameras /// </param> public stereoHead(int no_of_stereo_cameras) : base(0,0,0) { this.no_of_stereo_cameras = no_of_stereo_cameras; // create feature lists features = new stereoFeatures[no_of_stereo_cameras]; // store filenames of the images for each camera imageFilename = new String[no_of_stereo_cameras * 2]; // create the cameras cameraPosition = new pos3D[no_of_stereo_cameras]; // calibration data calibration = new calibrationStereo[no_of_stereo_cameras]; // create objects for each stereo camera for (int cam = 0; cam < no_of_stereo_cameras; cam++) { calibration[cam] = new calibrationStereo(); cameraPosition[cam] = new pos3D(0, 0, 0); features[cam] = null; // new stereoFeatures(); } // sensor models sensormodel = new rayModelLookup[no_of_stereo_cameras]; //scan matching scanmatch = new scanMatching[no_of_stereo_cameras]; /* if (no_of_cameras == 4) initQuadCam(); if (no_of_cameras == 2) initDualCam(); if (no_of_cameras == 1) initSingleStereoCamera(false); */ }
/// <summary> /// Mapping /// </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="robot_pose">current estimated position and orientation of the robots centre of rotation</param> protected void Map( 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, int stereo_camera_index, 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, pos3D robot_pose) { Parallel.For(0, 2, delegate(int i) { pos3D left_camera_location = null; pos3D right_camera_location = null; buffer[i].Map( 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, stereo_camera_index, baseline_mm, stereo_camera_position_x, stereo_camera_position_y, stereo_camera_position_z, stereo_camera_pan, stereo_camera_tilt, stereo_camera_roll, image_width, image_height, FOV_degrees, stereo_features, stereo_features_colour, stereo_features_uncertainties, sensormodel, ref left_camera_location, ref right_camera_location, robot_pose); }); }
/// <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, for each stereo camera observation</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="buffer_transition">have we transitioned to the next grid buffer?</param> /// <param name="overall_map_filename">filename to save an overall map of the path</param> /// <param name="overall_map_img">overall map image data</param> /// <param name="overall_map_dimension_mm">dimension of the overall map in millimetres</param> /// <param name="overall_map_centre_x_mm">centre x position of the overall map in millimetres</param> /// <param name="overall_map_centre_y_mm">centre x position of the overall map in millimetres</param> /// <returns>best localisation matching score</returns> protected 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, ref bool buffer_transition, string debug_mapping_filename, float known_best_pose_x_mm, float known_best_pose_y_mm, string overall_map_filename, ref byte[] overall_map_img, int overall_img_width, int overall_img_height, int overall_map_dimension_mm, int overall_map_centre_x_mm, int overall_map_centre_y_mm) { bool update_map = false; // move to the next grid buffer_transition = MoveToNextLocalGrid( ref current_grid_index, ref current_disparity_index, robot_pose[0], buffer, ref current_buffer_index, grid_centres, ref update_map, debug_mapping_filename, overall_map_filename, ref overall_map_img, overall_img_width, overall_img_height, overall_map_dimension_mm, overall_map_centre_x_mm, overall_map_centre_y_mm); // create the map if necessary if (update_map) { UpdateMap( 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, baseline_mm, stereo_camera_position_x, stereo_camera_position_y, stereo_camera_position_z, stereo_camera_pan, stereo_camera_tilt, stereo_camera_roll, image_width, image_height, FOV_degrees, sensormodel); } int img_poses_width = 640; int img_poses_height = 480; byte[] img_poses = null; if ((debug_mapping_filename != null) && (debug_mapping_filename != "")) { img_poses = new byte[img_poses_width * img_poses_height * 3]; } // localise within the currently active grid float matching_score = buffer[current_buffer_index].Localise( 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, stereo_camera_position_x, stereo_camera_position_y, stereo_camera_position_z, stereo_camera_pan, stereo_camera_tilt, stereo_camera_roll, image_width, image_height, FOV_degrees, stereo_features, stereo_features_colour, stereo_features_uncertainties, sensormodel, ref left_camera_location, ref right_camera_location, no_of_samples, sampling_radius_major_mm, sampling_radius_minor_mm, robot_pose, max_orientation_variance, max_tilt_variance, max_roll_variance, poses, pose_score, rnd, ref pose_offset, img_poses, img_poses_width, img_poses_height, known_best_pose_x_mm, known_best_pose_y_mm); if (matching_score != occupancygridBase.NO_OCCUPANCY_EVIDENCE) { // add this to the list of localisations localisations.Add(robot_pose[0].x + pose_offset.x); localisations.Add(robot_pose[0].y + pose_offset.y); localisations.Add(robot_pose[0].z + pose_offset.z); localisations.Add(pose_offset.pan); localisations.Add(matching_score); if ((debug_mapping_filename != null) && (debug_mapping_filename != "")) { string[] str = debug_mapping_filename.Split('.'); string poses_filename = str[0] + "_gridcells.gif"; Bitmap poses_bmp = new Bitmap(img_poses_width, img_poses_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb); BitmapArrayConversions.updatebitmap_unsafe(img_poses, poses_bmp); poses_bmp.Save(poses_filename, System.Drawing.Imaging.ImageFormat.Gif); } } return(matching_score); }
/// <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()); }
/// <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> /// Show a diagram of the robot in this pose /// This is useful for checking that the positions of cameras have /// been calculated correctly /// </summary> /// <param name="robot">robot object</param> /// <param name="img">image as a byte array</param> /// <param name="width">width of the image</param> /// <param name="height">height of the image</param> /// <param name="clearBackground">clear the background before drawing</param> /// <param name="min_x_mm">top left x coordinate</param> /// <param name="min_y_mm">top left y coordinate</param> /// <param name="max_x_mm">bottom right x coordinate</param> /// <param name="max_y_mm">bottom right y coordinate</param> /// <param name="showFieldOfView">whether to show the field of view of each camera</param> public void Show(robot rob, byte[] img, int width, int height, bool clearBackground, int min_x_mm, int min_y_mm, int max_x_mm, int max_y_mm, int line_width, bool showFieldOfView) { if (clearBackground) { for (int i = 0; i < width * height * 3; i++) { img[i] = 255; } } // get the positions of the head and cameras for this pose pos3D head_location = new pos3D(0, 0, 0); pos3D[] camera_centre_location = new pos3D[rob.head.no_of_stereo_cameras]; pos3D[] left_camera_location = new pos3D[rob.head.no_of_stereo_cameras]; pos3D[] right_camera_location = new pos3D[rob.head.no_of_stereo_cameras]; calculateCameraPositions(rob, ref head_location, ref camera_centre_location, ref left_camera_location, ref right_camera_location); int w = max_x_mm - min_x_mm; int h = max_y_mm - min_y_mm; // draw the body int xx = (int)((x - min_x_mm) * width / w); int yy = (int)((y - min_y_mm) * height / h); int wdth = (int)(rob.BodyWidth_mm * width / w); int hght = (int)(rob.BodyLength_mm * height / h); drawing.drawBox(img, width, height, xx, yy, wdth, hght, pan, 0, 255, 0, line_width); // draw the head xx = (int)((head_location.x - min_x_mm) * width / w); yy = (int)((head_location.y - min_y_mm) * height / h); int radius = (int)(rob.HeadSize_mm * width / w); drawing.drawBox(img, width, height, xx, yy, radius, radius, head_location.pan, 0, 255, 0, line_width); // draw the cameras for (int cam = 0; cam < rob.head.no_of_stereo_cameras; cam++) { // draw the left camera int xx1 = (int)((left_camera_location[cam].x - min_x_mm) * width / w); int yy1 = (int)((left_camera_location[cam].y - min_y_mm) * height / h); wdth = (int)((rob.head.calibration[cam].baseline / 4) * width / w); hght = (int)((rob.head.calibration[cam].baseline / 12) * height / h); if (hght < 1) { hght = 1; } drawing.drawBox(img, width, height, xx1, yy1, wdth, hght, left_camera_location[cam].pan + (float)(Math.PI / 2), 0, 255, 0, line_width); // draw the right camera int xx2 = (int)((right_camera_location[cam].x - min_x_mm) * width / w); int yy2 = (int)((right_camera_location[cam].y - min_y_mm) * height / h); drawing.drawBox(img, width, height, xx2, yy2, wdth, hght, right_camera_location[cam].pan + (float)(Math.PI / 2), 0, 255, 0, line_width); if (showFieldOfView) { float half_FOV = rob.head.calibration[cam].leftcam.camera_FOV_degrees * (float)Math.PI / 360.0f; int xx_ray = xx1 + (int)(width * Math.Sin(left_camera_location[cam].pan + half_FOV)); int yy_ray = yy1 + (int)(width * Math.Cos(left_camera_location[cam].pan + half_FOV)); drawing.drawLine(img, width, height, xx1, yy1, xx_ray, yy_ray, 200, 200, 255, 0, false); xx_ray = xx1 + (int)(width * Math.Sin(left_camera_location[cam].pan - half_FOV)); yy_ray = yy1 + (int)(width * Math.Cos(left_camera_location[cam].pan - half_FOV)); drawing.drawLine(img, width, height, xx1, yy1, xx_ray, yy_ray, 200, 200, 255, 0, false); xx_ray = xx2 + (int)(width * Math.Sin(right_camera_location[cam].pan + half_FOV)); yy_ray = yy2 + (int)(width * Math.Cos(right_camera_location[cam].pan + half_FOV)); drawing.drawLine(img, width, height, xx2, yy2, xx_ray, yy_ray, 200, 200, 255, 0, false); xx_ray = xx2 + (int)(width * Math.Sin(right_camera_location[cam].pan - half_FOV)); yy_ray = yy2 + (int)(width * Math.Cos(right_camera_location[cam].pan - half_FOV)); drawing.drawLine(img, width, height, xx2, yy2, xx_ray, yy_ray, 200, 200, 255, 0, false); } } }
/// <summary> /// insert a stereo ray into the grid /// </summary> /// <param name="ray">stereo ray</param> /// <param name="sensormodel">sensor model for each grid level</param> /// <param name="left_camera_location">left stereo camera position and orientation</param> /// <param name="right_camera_location">right stereo camera position and orientation</param> /// <param name="localiseOnly">whether we are mapping or localising</param> /// <returns>localisation matching score</returns> public float Insert( evidenceRay ray, stereoModel[] sensormodel, pos3D left_camera_location, pos3D right_camera_location, bool localiseOnly) { float matchingScore = occupancygridBase.NO_OCCUPANCY_EVIDENCE; switch (grid_type) { case TYPE_SIMPLE: { for (int grid_level = 0; grid_level < grid.Length; grid_level++) { rayModelLookup sensormodel_lookup = sensormodel[grid_level].ray_model; occupancygridSimple grd = (occupancygridSimple)grid[grid_level]; float score = grd.Insert(ray, sensormodel_lookup, left_camera_location, right_camera_location, localiseOnly); if (score != occupancygridBase.NO_OCCUPANCY_EVIDENCE) { if (matchingScore == occupancygridBase.NO_OCCUPANCY_EVIDENCE) matchingScore = 0; matchingScore += grid_weight[grid_level] * score; } } break; } } return (matchingScore); }
/// <summary> /// Mapping /// </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 grid level</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="robot_pose">current estimated position and orientation of the robots centre of rotation</param> public void Map( 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, int stereo_camera_index, 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, pos3D robot_pose) { // positions of the left and right camera relative to the robots centre of rotation pos3D head_location = new pos3D(0,0,0); pos3D camera_centre_location = null; pos3D relative_left_cam = null; pos3D relative_right_cam = null; 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, stereo_camera_position_x, stereo_camera_position_y, stereo_camera_position_z, stereo_camera_pan, stereo_camera_tilt, stereo_camera_roll, ref head_location, ref camera_centre_location, ref relative_left_cam, ref relative_right_cam); left_camera_location = relative_left_cam.translate(robot_pose.x, robot_pose.y, robot_pose.z); right_camera_location = relative_right_cam.translate(robot_pose.x, robot_pose.y, robot_pose.z); pos3D stereo_camera_centre = new pos3D(0, 0, 0); // update the grid // centre position between the left and right cameras stereo_camera_centre.x = left_camera_location.x + ((right_camera_location.x - left_camera_location.x) * 0.5f); stereo_camera_centre.y = left_camera_location.y + ((right_camera_location.y - left_camera_location.y) * 0.5f); stereo_camera_centre.z = left_camera_location.z + ((right_camera_location.z - left_camera_location.z) * 0.5f); stereo_camera_centre.pan = robot_pose.pan + head_pan + stereo_camera_pan; stereo_camera_centre.tilt = robot_pose.tilt + head_tilt + stereo_camera_tilt; stereo_camera_centre.roll = robot_pose.roll + head_roll + stereo_camera_roll; // create a set of stereo rays as observed from this pose List<evidenceRay> rays = sensormodel[stereo_camera_index][0].createObservation( stereo_camera_centre, baseline_mm, image_width, image_height, FOV_degrees, stereo_features, stereo_features_colour, stereo_features_uncertainties, true); // insert rays into the occupancy grid for (int r = 0; r < rays.Count; r++) { Insert(rays[r], sensormodel[stereo_camera_index], left_camera_location, right_camera_location, false); } }
/// <summary> /// returns a score indicating how closely matched the two viewpoints are /// </summary> /// <param name="other"></param> /// <param name="intersects"></param> /// <returns></returns> public float matchingScore(viewpoint other, float separation_tollerance, int ray_thickness, ArrayList intersects) { float separation; float score = 0; pos3D intersectPos = new pos3D(0, 0, 0); if (ray_thickness < 1) ray_thickness = 1; for (int cam1 = 0; cam1 < rays.Length; cam1++) { for (int ry1 = 0; ry1 < rays[cam1].Count; ry1++) { evidenceRay ray1 = (evidenceRay)rays[cam1][ry1]; int pan_index1 = ray1.pan_index - 1; int pan_index2 = ray1.pan_index; int pan_index3 = ray1.pan_index + 1; if (pan_index1 < 0) pan_index1 = evidenceRay.pan_steps - 1; if (pan_index3 >= evidenceRay.pan_steps) pan_index3 = 0; int cam2 = cam1; //for (int cam2 = 0; cam2 < other.rays.Length; cam2++) { for (int ry2 = 0; ry2 < other.rays[cam2].Count; ry2++) { evidenceRay ray2 = (evidenceRay)other.rays[cam2][ry2]; int pan_index4 = ray2.pan_index; if ((pan_index1 == pan_index4) || (pan_index2 == pan_index4) || (pan_index3 == pan_index4)) { //do these rays intersect separation = 999; if (stereoModel.raysOverlap(ray1, ray2, intersectPos, separation_tollerance, ray_thickness, ref separation)) { float p1 = ray1.probability(intersectPos.x, intersectPos.y); float p2 = ray2.probability(intersectPos.x, intersectPos.y); float prob = (p1 * p2) + ((1.0f - p1) * (1.0f - p2)); if (intersects != null) { // add the intersection to the list intersects.Add(intersectPos); intersectPos = new pos3D(0, 0, 0); } //increment the matching score score += 1.0f / (1.0f + ((1.0f - prob) * separation * separation)); //score += 1.0f / (1.0f + (separation * separation) + (ray2.length * ray1.length / 2)); //score += 1.0f / (1.0f + (separation * separation)); } } } } } } return (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="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); } } } }
// find the centre of the path public pos3D pathCentre() { float centre_x = 0; float centre_y = 0; for (int v = 0; v < viewpoints.Count; v++) { viewpoint view = (viewpoint)viewpoints[v]; centre_x += view.odometry_position.x; centre_y += view.odometry_position.y; } centre_x /= viewpoints.Count; centre_y /= viewpoints.Count; pos3D centre = new pos3D(centre_x, centre_y, 0); return (centre); }
public static void CreateMoireGrid( float sampling_radius_major_mm, float sampling_radius_minor_mm, int no_of_poses, float pan, float tilt, float roll, float max_orientation_variance, float max_tilt_variance, float max_roll_variance, Random rnd, ref List <pos3D> cells, byte[] img_basic, byte[] img_moire, int img_width, int img_height) { float first_grid_spacing = sampling_radius_minor_mm / (float)Math.Sqrt(no_of_poses); float second_grid_spacing = first_grid_spacing * 1.1f; float first_grid_rotation_degrees = 0; float second_grid_rotation_degrees = 10; float scaling_factor = 0.3f; int dimension_x_cells = 50; int dimension_y_cells = 50; int cells_percent = 0; cells.Clear(); int tries = 0; while (((cells_percent < 90) || (cells_percent > 110)) && (tries < 10)) { CreateMoireGrid( sampling_radius_major_mm, sampling_radius_minor_mm, first_grid_spacing, second_grid_spacing, first_grid_rotation_degrees, second_grid_rotation_degrees, dimension_x_cells, dimension_y_cells, scaling_factor, ref cells); cells_percent = cells.Count * 100 / no_of_poses; if (cells_percent < 90) { scaling_factor *= 0.9f; } if (cells_percent > 110) { scaling_factor *= 1.1f; } tries++; Console.WriteLine("Cells = " + cells.Count.ToString()); } for (int i = cells.Count - 1; i >= 0; i--) { pos3D sample_pose = cells[i]; if (i % 2 == 0) { sample_pose.pan = pan + ((float)rnd.NextDouble() * max_orientation_variance); } else { sample_pose.pan = pan - ((float)rnd.NextDouble() * max_orientation_variance); } sample_pose.tilt = tilt + (((float)rnd.NextDouble() - 0.5f) * 2 * max_tilt_variance); sample_pose.roll = roll + (((float)rnd.NextDouble() - 0.5f) * 2 * max_roll_variance); } // create an image showing the results if (img_basic != null) { ShowMoireGrid( sampling_radius_major_mm, sampling_radius_minor_mm, first_grid_spacing, second_grid_spacing, first_grid_rotation_degrees, second_grid_rotation_degrees, dimension_x_cells * 2, dimension_y_cells * 2, scaling_factor, img_moire, img_width, img_height); float max_radius = sampling_radius_major_mm * 0.025f; for (int i = img_basic.Length - 1; i >= 0; i--) { img_basic[i] = 0; } int tx = -(int)(sampling_radius_major_mm); int ty = -(int)(sampling_radius_major_mm); int bx = (int)(sampling_radius_major_mm); int by = (int)(sampling_radius_major_mm); for (int i = cells.Count - 1; i >= 0; i--) { pos3D p = cells[i]; int x = ((img_width - img_height) / 2) + (int)((p.x - tx) * img_height / (sampling_radius_major_mm * 2)); int y = (int)((p.y - ty) * img_height / (sampling_radius_major_mm * 2)); int radius = (int)(max_radius * img_width / (sampling_radius_major_mm * 2)); int r = (int)(((p.pan - pan) - (-max_orientation_variance)) * 255 / (max_orientation_variance * 2)); int g = 255 - r; int b = 0; drawing.drawSpot(img_basic, img_width, img_height, x, y, radius, r, g, b); } } }
/// <summary> /// given a set of poses and their matching scores return the best pose /// </summary> /// <param name="poses">list of poses which have been evaluated</param> /// <param name="scores">localisation matching score for each pose</param> /// <param name="best_pose">returned best pose</param> /// <param name="sampling_radius_major_mm">major axis length</param> /// <param name="img">optional image data</param> /// <param name="img_width">image width</param> /// <param name="img_height">image height</param> static public void FindBestPose( List <pos3D> poses, List <float> scores, ref pos3D best_pose, float sampling_radius_major_mm, byte[] img_poses, byte[] img_graph, int img_width, int img_height, float known_best_pose_x, float known_best_pose_y) { float peak_radius = sampling_radius_major_mm * 0.5f; float max_score = float.MinValue; float min_score = float.MaxValue; float peak_x = 0; float peak_y = 0; float peak_z = 0; for (int i = 0; i < scores.Count; i++) { if (scores[i] < min_score) { min_score = scores[i]; } if (scores[i] > max_score) { max_score = scores[i]; peak_x = poses[i].x; peak_y = poses[i].y; peak_z = poses[i].z; } } float score_range = max_score - min_score; float minimum_score = min_score + (score_range * 0.5f); float minimum_score2 = min_score + (score_range * 0.8f); //float minimum_score = 0.98f; //float minimum_score2 = 0.99f; if (best_pose == null) { best_pose = new pos3D(0, 0, 0); } best_pose.x = 0; best_pose.y = 0; best_pose.z = 0; best_pose.pan = 0; best_pose.tilt = 0; best_pose.roll = 0; float hits = 0; if (score_range > 0) { for (int i = 0; i < poses.Count; i++) { if (scores[i] > minimum_score2) { float dx = poses[i].x - peak_x; float dy = poses[i].y - peak_y; float dz = poses[i].z - peak_z; //if (Math.Abs(dx) < peak_radius) { //if (Math.Abs(dy) < peak_radius) { //if (Math.Abs(dz) < peak_radius) { float score = (scores[i] - min_score) / score_range; score *= score; best_pose.x += poses[i].x * score; best_pose.y += poses[i].y * score; best_pose.z += poses[i].z * score; best_pose.pan += poses[i].pan * score; best_pose.tilt += poses[i].tilt * score; best_pose.roll += poses[i].roll * score; hits += score; } } } } } } if (hits > 0) { best_pose.x /= hits; best_pose.y /= hits; best_pose.z /= hits; best_pose.pan /= hits; best_pose.tilt /= hits; best_pose.roll /= hits; } float grad = 1; if (Math.Abs(best_pose.x) < 0.0001f) { grad = best_pose.y / best_pose.x; } float d1 = (float)Math.Sqrt(best_pose.x * best_pose.x + best_pose.y * best_pose.y); if (d1 < 0.001f) { d1 = 0.001f; } float origin_x = best_pose.x * sampling_radius_major_mm * 2 / d1; //sampling_radius_major_mm*best_pose.x/Math.Abs(best_pose.x); float origin_y = best_pose.y * sampling_radius_major_mm * 2 / d1; List <float> points = new List <float>(); float points_min_distance = float.MaxValue; float points_max_distance = float.MinValue; float dxx = best_pose.y; float dyy = best_pose.x; float dist_to_best_pose = (float)Math.Sqrt(dxx * dxx + dyy * dyy); float max_score2 = 0; for (int i = 0; i < poses.Count; i++) { if (scores[i] > minimum_score) { float ix = 0; float iy = 0; geometry.intersection( 0, 0, best_pose.x, best_pose.y, poses[i].x - dxx, poses[i].y - dyy, poses[i].x, poses[i].y, ref ix, ref iy); float dxx2 = ix - origin_x; float dyy2 = iy - origin_y; float dist = (float)Math.Sqrt(dxx2 * dxx2 + dyy2 * dyy2); float score = scores[i]; points.Add(dist); points.Add(score); if (score > max_score2) { max_score2 = score; } if (dist < points_min_distance) { points_min_distance = dist; } if (dist > points_max_distance) { points_max_distance = dist; } } } // create an image showing the results if ((img_poses != null) && (score_range > 0)) { float max_radius = img_width / 50; // sampling_radius_major_mm * 0.1f; for (int i = img_poses.Length - 1; i >= 0; i--) { img_poses[i] = 0; if (img_graph != null) { img_graph[i] = 255; } } int tx = -(int)(sampling_radius_major_mm); int ty = -(int)(sampling_radius_major_mm); int bx = (int)(sampling_radius_major_mm); int by = (int)(sampling_radius_major_mm); int origin_x2 = (int)((origin_x - tx) * img_width / (sampling_radius_major_mm * 2)); int origin_y2 = img_height - 1 - (int)((origin_y - ty) * img_height / (sampling_radius_major_mm * 2)); int origin_x3 = (int)((0 - tx) * img_width / (sampling_radius_major_mm * 2)); int origin_y3 = img_height - 1 - (int)((0 - ty) * img_height / (sampling_radius_major_mm * 2)); drawing.drawLine(img_poses, img_width, img_height, origin_x3, origin_y3, origin_x2, origin_y2, 255, 255, 0, 1, false); for (int i = 0; i < poses.Count; i++) { pos3D p = poses[i]; float score = scores[i]; int x = (int)((p.x - tx) * img_width / (sampling_radius_major_mm * 2)); int y = img_height - 1 - (int)((p.y - ty) * img_height / (sampling_radius_major_mm * 2)); int radius = (int)((score - min_score) * max_radius / score_range); byte intensity_r = (byte)((score - min_score) * 255 / score_range); byte intensity_g = intensity_r; byte intensity_b = intensity_r; if (score < minimum_score) { intensity_r = 0; intensity_g = 0; } if ((score >= minimum_score) && (score < max_score - ((max_score - minimum_score) * 0.5f))) { intensity_g = 0; } drawing.drawSpot(img_poses, img_width, img_height, x, y, radius, intensity_r, intensity_g, intensity_b); } int best_x = (int)((best_pose.x - tx) * img_width / (sampling_radius_major_mm * 2)); int best_y = img_height - 1 - (int)((best_pose.y - ty) * img_height / (sampling_radius_major_mm * 2)); drawing.drawCross(img_poses, img_width, img_height, best_x, best_y, (int)max_radius, 255, 0, 0, 1); int known_best_x = (int)((known_best_pose_x - tx) * img_width / (sampling_radius_major_mm * 2)); int known_best_y = img_height - 1 - (int)((known_best_pose_y - ty) * img_height / (sampling_radius_major_mm * 2)); drawing.drawCross(img_poses, img_width, img_height, known_best_x, known_best_y, (int)max_radius, 0, 255, 0, 1); if (img_graph != null) { // draw the graph for (int i = 0; i < points.Count; i += 2) { int graph_x = (int)((points[i] - points_min_distance) * (img_width - 1) / (points_max_distance - points_min_distance)); int graph_y = img_height - 1 - ((int)(points[i + 1] * (img_height - 1) / (max_score2 * 1.1f))); drawing.drawCross(img_graph, img_width, img_height, graph_x, graph_y, 3, 0, 0, 0, 0); } } } }
public void EvidenceRayRotation() { int debug_img_width = 640; int debug_img_height = 480; byte[] debug_img = new byte[debug_img_width * debug_img_height * 3]; for (int i = (debug_img_width * debug_img_height * 3)-1; i >= 0; i--) debug_img[i] = 255; Bitmap bmp = new Bitmap(debug_img_width, debug_img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb); int cellSize_mm = 32; int image_width = 320; int image_height = 240; Console.WriteLine("Creating sensor models"); stereoModel inverseSensorModel = new stereoModel(); inverseSensorModel.createLookupTable(cellSize_mm, image_width, image_height); // create a ray float FOV_horizontal = 78 * (float)Math.PI / 180.0f; inverseSensorModel.FOV_horizontal = FOV_horizontal; inverseSensorModel.FOV_vertical = FOV_horizontal * image_height / image_width; evidenceRay ray = inverseSensorModel.createRay( image_width/2, image_height/2, 4, 0, 255, 255, 255); Assert.AreNotEqual(null, ray, "No ray was created"); Assert.AreNotEqual(null, ray.vertices, "No ray vertices were created"); pos3D[] start_vertices = (pos3D[])ray.vertices.Clone(); Console.WriteLine("x,y,z: " + start_vertices[0].x.ToString() + ", " + start_vertices[0].y.ToString() + ", " + start_vertices[0].z.ToString()); for (int i = 0; i < ray.vertices.Length; i++) { int j = i + 1; if (j == ray.vertices.Length) j = 0; int x0 = (debug_img_width/2) + (int)ray.vertices[i].x/50; int y0 = (debug_img_height/2) + (int)ray.vertices[i].y/50; int x1 = (debug_img_width/2) + (int)ray.vertices[j].x/50; int y1 = (debug_img_height/2) + (int)ray.vertices[j].y/50; drawing.drawLine(debug_img, debug_img_width, debug_img_height, x0,y0,x1,y1,0,255,0,0,false); } float angle_degrees = 30; float angle_radians = angle_degrees / 180.0f * (float)Math.PI; pos3D rotation = new pos3D(0, 0, 0); rotation.pan = angle_degrees; ray.translateRotate(rotation); Console.WriteLine("x,y,z: " + ray.vertices[0].x.ToString() + ", " + ray.vertices[0].y.ToString() + ", " + ray.vertices[0].z.ToString()); for (int i = 0; i < ray.vertices.Length; i++) { int j = i + 1; if (j == ray.vertices.Length) j = 0; int x0 = (debug_img_width/2) + (int)ray.vertices[i].x/50; int y0 = (debug_img_height/2) + (int)ray.vertices[i].y/50; int x1 = (debug_img_width/2) + (int)ray.vertices[j].x/50; int y1 = (debug_img_height/2) + (int)ray.vertices[j].y/50; drawing.drawLine(debug_img, debug_img_width, debug_img_height, x0,y0,x1,y1,255,0,0,0,false); } BitmapArrayConversions.updatebitmap_unsafe(debug_img, bmp); bmp.Save("tests_occupancygrid_simple_EvidenceRayRotation.jpg", System.Drawing.Imaging.ImageFormat.Jpeg); }
/// <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 a score indicating how closely matched the two viewpoints are /// </summary> /// <param name="other"></param> /// <param name="intersects"></param> /// <returns></returns> public float matchingScore(viewpoint other, float separation_tollerance, int ray_thickness, ArrayList intersects) { float separation; float score = 0; pos3D intersectPos = new pos3D(0, 0, 0); if (ray_thickness < 1) { ray_thickness = 1; } for (int cam1 = 0; cam1 < rays.Length; cam1++) { for (int ry1 = 0; ry1 < rays[cam1].Count; ry1++) { evidenceRay ray1 = (evidenceRay)rays[cam1][ry1]; int pan_index1 = ray1.pan_index - 1; int pan_index2 = ray1.pan_index; int pan_index3 = ray1.pan_index + 1; if (pan_index1 < 0) { pan_index1 = evidenceRay.pan_steps - 1; } if (pan_index3 >= evidenceRay.pan_steps) { pan_index3 = 0; } int cam2 = cam1; //for (int cam2 = 0; cam2 < other.rays.Length; cam2++) { for (int ry2 = 0; ry2 < other.rays[cam2].Count; ry2++) { evidenceRay ray2 = (evidenceRay)other.rays[cam2][ry2]; int pan_index4 = ray2.pan_index; if ((pan_index1 == pan_index4) || (pan_index2 == pan_index4) || (pan_index3 == pan_index4)) { //do these rays intersect separation = 999; if (stereoModel.raysOverlap(ray1, ray2, intersectPos, separation_tollerance, ray_thickness, ref separation)) { float p1 = ray1.probability(intersectPos.x, intersectPos.y); float p2 = ray2.probability(intersectPos.x, intersectPos.y); float prob = (p1 * p2) + ((1.0f - p1) * (1.0f - p2)); if (intersects != null) { // add the intersection to the list intersects.Add(intersectPos); intersectPos = new pos3D(0, 0, 0); } //increment the matching score score += 1.0f / (1.0f + ((1.0f - prob) * separation * separation)); //score += 1.0f / (1.0f + (separation * separation) + (ray2.length * ray1.length / 2)); //score += 1.0f / (1.0f + (separation * separation)); } } } } } } return(score); }
/// <summary> /// localise and return offset values /// </summary> /// <param name="stereo_features">disparities for each stereo camera (x,y,disparity)</param> /// <param name="stereo_features_colour">colour for each disparity</param> /// <param name="stereo_features_uncertainties">uncertainty for each disparity</param> /// <param name="debug_mapping_filename">optional filename used for saving debugging info</param> /// <param name="known_offset_x_mm">ideal x offset, if known</param> /// <param name="known_offset_y_mm">ideal y offset, if known</param> /// <param name="offset_x_mm">returned x offset</param> /// <param name="offset_y_mm">returned y offset</param> /// <param name="offset_z_mm">returned z offset</param> /// <param name="offset_pan_radians">returned pan</param> /// <param name="offset_tilt_radians">returned tilt</param> /// <param name="offset_roll_radians">returned roll</param> /// <returns>true if the localisation was valid</returns> public bool Localise( float[][] stereo_features, byte[][,] stereo_features_colour, float[][] stereo_features_uncertainties, string debug_mapping_filename, float known_offset_x_mm, float known_offset_y_mm, ref float offset_x_mm, ref float offset_y_mm, ref float offset_z_mm, ref float offset_pan_radians, ref float offset_tilt_radians, ref float offset_roll_radians) { int overall_img_width = 640; int overall_img_height = 480; bool valid_localisation = true; pos3D pose_offset = new pos3D(0,0,0); bool buffer_transition = false; float matching_score = buffer.Localise( robot_geometry, stereo_features, stereo_features_colour, stereo_features_uncertainties, rnd, ref pose_offset, ref buffer_transition, debug_mapping_filename, known_offset_x_mm, known_offset_y_mm, overall_map_filename, ref overall_map_img, overall_img_width, overall_img_height, overall_map_dimension_mm, overall_map_centre_x_mm, overall_map_centre_y_mm); if (matching_score == occupancygridBase.NO_OCCUPANCY_EVIDENCE) valid_localisation = false; offset_x_mm = pose_offset.x; offset_y_mm = pose_offset.y; offset_z_mm = pose_offset.z; offset_pan_radians = pose_offset.pan; offset_tilt_radians = pose_offset.tilt; offset_roll_radians = pose_offset.roll; return(valid_localisation); }
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); }
/// <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 pos3D rotate_old(float pan, float tilt, float roll) { float hyp; pos3D rotated = new pos3D(x, y, z); // roll //if (roll != 0) { hyp = (float)Math.Sqrt((rotated.x * rotated.x) + (rotated.z * rotated.z)); if (hyp > 0) { float roll_angle = (float)Math.Acos(rotated.z / hyp); if (rotated.x < 0) { roll_angle = (float)(Math.PI * 2) - roll_angle; } float new_roll_angle = roll + roll_angle; rotated.x = hyp * (float)Math.Sin(new_roll_angle); rotated.z = hyp * (float)Math.Cos(new_roll_angle); } } if (tilt != 0) { // tilt hyp = (float)Math.Sqrt((rotated.y * rotated.y) + (rotated.z * rotated.z)); if (hyp > 0) { float tilt_angle = (float)Math.Acos(rotated.y / hyp); if (rotated.z < 0) { tilt_angle = (float)(Math.PI * 2) - tilt_angle; } float new_tilt_angle = tilt + tilt_angle; rotated.y = hyp * (float)Math.Sin(new_tilt_angle); rotated.z = hyp * (float)Math.Cos(new_tilt_angle); } } //if (pan != 0) { // pan hyp = (float)Math.Sqrt((rotated.x * rotated.x) + (rotated.y * rotated.y)); if (hyp > 0) { float pan_angle = (float)Math.Acos(rotated.y / hyp); if (rotated.x < 0) { pan_angle = (float)(Math.PI * 2) - pan_angle; } rotated.new_pan_angle = pan - pan_angle; rotated.dist_xy = hyp; rotated.x = hyp * (float)Math.Sin(rotated.new_pan_angle); rotated.y = hyp * (float)Math.Cos(rotated.new_pan_angle); } } rotated.pan = this.pan + pan; rotated.tilt = this.tilt + tilt; rotated.roll = this.roll + roll; return(rotated); }
/// <summary> /// removes low probability poses /// Note that a maturity threshold is used, so that poses which may initially /// be unfairly judged as improbable have time to prove their worth /// </summary> private void Prune() { float max_score = float.MinValue; best_path = null; // sort poses by score for (int i = 0; i < Poses.Count - 1; i++) { particlePath p1 = Poses[i]; // keep track of the bounding region within which the path tree occurs if (p1.current_pose.x < min_tree_x) { min_tree_x = p1.current_pose.x; } if (p1.current_pose.x > max_tree_x) { max_tree_x = p1.current_pose.x; } if (p1.current_pose.y < min_tree_y) { min_tree_y = p1.current_pose.y; } if (p1.current_pose.y > max_tree_y) { max_tree_y = p1.current_pose.y; } max_score = p1.total_score; particlePath winner = null; int winner_index = 0; for (int j = i + 1; j < Poses.Count; j++) { particlePath p2 = Poses[i]; float sc = p2.total_score; if ((sc > max_score) || ((max_score == 0) && (sc != 0))) { max_score = sc; winner = p2; winner_index = j; } } if (winner != null) { Poses[i] = winner; Poses[winner_index] = p1; } } // the best path is at the top best_path = Poses[0]; // It's culling season int cull_index = (100 - cull_threshold) * Poses.Count / 100; if (cull_index > Poses.Count - 2) { cull_index = Poses.Count - 2; } for (int i = Poses.Count - 1; i > cull_index; i--) { particlePath path = Poses[i]; if (path.path.Count >= pose_maturation) { // remove mapping hypotheses for this path path.Remove(LocalGrid); // now remove the path itself Poses.RemoveAt(i); } } // garbage collect any dead paths (R.I.P.) List <particlePath> possible_roots = new List <particlePath>(); // stores paths where all branches have coinverged to a single possibility for (int i = ActivePoses.Count - 1; i >= 0; i--) { particlePath path = ActivePoses[i]; if ((!path.Enabled) || ((path.total_children == 0) && (path.branch_pose == null) && (path.current_pose.time_step < time_step - pose_maturation - 5))) { ActivePoses.RemoveAt(i); } else { // record any fully collapsed paths if (!path.Collapsed) { if (path.branch_pose == null) { possible_roots.Add(path); } else { if (path.branch_pose.path.Collapsed) { possible_roots.Add(path); } } } } } if (possible_roots.Count == 1) { // collapse tha psth particlePath path = possible_roots[0]; if (path.branch_pose != null) { particlePath previous_path = path.branch_pose.path; previous_path.Distill(LocalGrid); path.branch_pose.parent = null; path.branch_pose = null; } path.Collapsed = true; // take note of the time step. This is for use with the display functions only root_time_step = path.current_pose.time_step; } if (best_path != null) { // update the current robot position with the best available pose if (current_robot_pose == null) { current_robot_pose = new pos3D(best_path.current_pose.x, best_path.current_pose.y, 0); } current_robot_pose.pan = best_path.current_pose.pan; current_robot_path_score = best_path.total_score; // generate new poses from the ones which have survived culling int new_poses_required = survey_trial_poses; // -Poses.Count; int max = Poses.Count; int n = 0, added = 0; while ((max > 0) && (n < new_poses_required * 4) && (added < new_poses_required)) { // identify a potential parent at random, // from one of the surviving paths int random_parent_index = rnd.Next(max - 1); particlePath parent_path = Poses[random_parent_index]; // only mature parents can have children if (parent_path.path.Count >= pose_maturation) { // generate a child branching from the parent path particlePath child_path = new particlePath(parent_path, path_ID, rob.LocalGridDimension); createNewPose(child_path); added++; } n++; } // like salmon, after parents spawn they die if (added > 0) { for (int i = max - 1; i >= 0; i--) { Poses.RemoveAt(i); } } // if the robot has rotated through a large angle since // the previous time step then reset the scan matching estimate //if (Math.Abs(best_path.current_pose.pan - rob.pan) > rob.ScanMatchingMaxPanAngleChange * Math.PI / 180) rob.ScanMatchingPanAngleEstimate = scanMatching.NOT_MATCHED; } PosesEvaluated = false; }
/// <summary> /// returns a set of evidence rays based upon the given observation /// </summary> /// <param name="observer">observer position and orientation</param> /// <param name="baseline">baseline distance</param> /// <param name="image_width">camera image width</param> /// <param name="image_height">camera image height</param> /// <param name="FOV_degrees">horizontal camera field of view in degrees</param> /// <param name="stereo_features">stereo features, as groups of three figures</param> /// <param name="stereo_features_colour">colour of each stereo feature</param> /// <param name="stereo_features_uncertainties">uncertainty value associated with each stereo feature</param> /// <param name="translate">translate the ray to be relative to the observer position, otherwise it's relative to (0,0,0)</param> /// <returns>list of evidence rays, centred at (0, 0, 0)</returns> public List<evidenceRay> createObservation( pos3D observer, float baseline, int image_width, int image_height, float FOV_degrees, float[] stereo_features, byte[,] stereo_features_colour, float[] stereo_features_uncertainties, bool translate) { List<evidenceRay> result = new List<evidenceRay>(); // get essential data for this stereo camera FOV_horizontal = FOV_degrees * (float)Math.PI / 180.0f; FOV_vertical = FOV_horizontal * image_height / image_width; // calc uncertainty in angle (+/- half a pixel) float angular_uncertainty = FOV_horizontal / (image_width * 2); //sigma = 100 * (float)Math.Tan(angular_uncertainty); // 100 is the factor used in RaysIntersection sigma = 1.0f / (image_width * 1) * FOV_horizontal; // some head geometry pos3D headOrientation = observer; pos3D cameraOrientation = observer; if (!translate) cameraOrientation = new pos3D(0, 0, 0); cameraOrientation.pan = headOrientation.pan; cameraOrientation.tilt = headOrientation.tilt; cameraOrientation.roll = headOrientation.roll; if (stereo_features != null) // if there are stereo features associated with this camera { int f2 = 0; for (int f = 0; f < stereo_features.Length; f += 3) { // get the parameters of the feature float image_x = stereo_features[f]; float image_y = stereo_features[f + 1]; float disparity = stereo_features[f + 2]; // create a ray evidenceRay ray = createRay( image_x, image_y, disparity, 1 + stereo_features_uncertainties[f2], stereo_features_colour[f2, 0], stereo_features_colour[f2, 1], stereo_features_colour[f2, 2]); if (ray != null) { // convert from camera-centric coordinates to head coordinates ray.translateRotate(cameraOrientation); // add to the result result.Add(ray); } f2++; } } return (result); }
/// <summary> /// parse an xml node to extract geometry parameters /// </summary> /// <param name="xnod"></param> /// <param name="level"></param> public void LoadFromXml( XmlNode xnod, int level, ref int cameraIndex) { XmlNode xnodWorking; if (xnod.Name == "StereoCamera") { cameraIndex++; } if (xnod.Name == "BodyWidthMillimetres") { body_width_mm = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "BodyLengthMillimetres") { body_length_mm = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "BodyHeightMillimetres") { body_height_mm = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "CentreOfRotationX") { body_centre_of_rotation_x = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "CentreOfRotationY") { body_centre_of_rotation_y = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "CentreOfRotationZ") { body_centre_of_rotation_z = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "HeadCentreX") { head_centroid_x = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "HeadCentreY") { head_centroid_y = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "HeadCentreZ") { head_centroid_z = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "HeadDiameterMillimetres") { head_diameter_mm = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "MaxOrientationVarianceDegrees") { max_orientation_variance = Convert.ToSingle(xnod.InnerText) / 180.0f * (float)Math.PI; } if (xnod.Name == "MaxTiltVarianceDegrees") { max_tilt_variance = Convert.ToSingle(xnod.InnerText) / 180.0f * (float)Math.PI; } if (xnod.Name == "MaxRollVarianceDegrees") { max_roll_variance = Convert.ToSingle(xnod.InnerText) / 180.0f * (float)Math.PI; } if (xnod.Name == "NoOfStereoCameras") { cameraIndex = -1; int no_of_stereo_cameras = Convert.ToInt32(xnod.InnerText); baseline_mm = new float[no_of_stereo_cameras]; pose = new pos3D[no_of_stereo_cameras]; poses = new List <pos3D>(); stereo_camera_position_x = new float[no_of_stereo_cameras]; stereo_camera_position_y = new float[no_of_stereo_cameras]; stereo_camera_position_z = new float[no_of_stereo_cameras]; stereo_camera_pan = new float[no_of_stereo_cameras]; stereo_camera_tilt = new float[no_of_stereo_cameras]; stereo_camera_roll = new float[no_of_stereo_cameras]; image_width = new int[no_of_stereo_cameras]; image_height = new int[no_of_stereo_cameras]; FOV_degrees = new float[no_of_stereo_cameras]; left_camera_location = new pos3D[no_of_stereo_cameras]; right_camera_location = new pos3D[no_of_stereo_cameras]; for (int i = 0; i < no_of_stereo_cameras; i++) { left_camera_location[i] = new pos3D(0, 0, 0); right_camera_location[i] = new pos3D(0, 0, 0); } } if (xnod.Name == "BaselineMillimetres") { baseline_mm[cameraIndex] = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "PositionMillimetres") { string[] str = xnod.InnerText.Split(' '); stereo_camera_position_x[cameraIndex] = Convert.ToSingle(str[0]); stereo_camera_position_y[cameraIndex] = Convert.ToSingle(str[1]); stereo_camera_position_z[cameraIndex] = Convert.ToSingle(str[2]); } if (xnod.Name == "OrientationDegrees") { string[] str = xnod.InnerText.Split(' '); stereo_camera_pan[cameraIndex] = Convert.ToSingle(str[0]) / 180.0f * (float)Math.PI; stereo_camera_tilt[cameraIndex] = Convert.ToSingle(str[1]) / 180.0f * (float)Math.PI; stereo_camera_roll[cameraIndex] = Convert.ToSingle(str[2]) / 180.0f * (float)Math.PI; } if (xnod.Name == "ImageDimensions") { string[] str = xnod.InnerText.Split(' '); image_width[cameraIndex] = Convert.ToInt32(str[0]); image_height[cameraIndex] = Convert.ToInt32(str[1]); } if (xnod.Name == "FOVDegrees") { FOV_degrees[cameraIndex] = Convert.ToSingle(xnod.InnerText); } // call recursively on all children of the current node if (xnod.HasChildNodes) { xnodWorking = xnod.FirstChild; while (xnodWorking != null) { LoadFromXml(xnodWorking, level + 1, ref cameraIndex); xnodWorking = xnodWorking.NextSibling; } } }
/// <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; } }
/// <summary> /// add an observation taken from this pose /// </summary> /// <param name="stereo_rays">list of ray objects in this observation</param> /// <param name="rob">robot object</param> /// <param name="LocalGrid">occupancy grid into which to insert the observation</param> /// <param name="localiseOnly">if true does not add any mapping particles (pure localisation)</param> /// <returns>localisation matching score</returns> public float AddObservation(List <evidenceRay>[] stereo_rays, robot rob, occupancygridMultiHypothesis LocalGrid, bool localiseOnly) { // clear the localisation score float localisation_score = occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE; // get the positions of the head and cameras for this pose pos3D head_location = new pos3D(0, 0, 0); pos3D[] camera_centre_location = new pos3D[rob.head.no_of_stereo_cameras]; pos3D[] left_camera_location = new pos3D[rob.head.no_of_stereo_cameras]; pos3D[] right_camera_location = new pos3D[rob.head.no_of_stereo_cameras]; calculateCameraPositions(rob, ref head_location, ref camera_centre_location, ref left_camera_location, ref right_camera_location); // itterate for each stereo camera int cam = stereo_rays.Length - 1; while (cam >= 0) { // itterate through each ray int r = stereo_rays[cam].Count - 1; while (r >= 0) { // observed ray. Note that this is in an egocentric // coordinate frame relative to the head of the robot evidenceRay ray = stereo_rays[cam][r]; // translate and rotate this ray appropriately for the pose evidenceRay trial_ray = ray.trialPose(camera_centre_location[cam].pan, camera_centre_location[cam].x, camera_centre_location[cam].y); // update the grid cells for this ray and update the // localisation score accordingly float score = LocalGrid.Insert(trial_ray, this, rob.head.sensormodel[cam], left_camera_location[cam], right_camera_location[cam], localiseOnly); if (score != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE) { if (localisation_score != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE) { localisation_score += score; } else { localisation_score = score; } } r--; } cam--; } return(localisation_score); }
/// <summary> /// moves to the next grid in the sequence, if necessary /// </summary> /// <param name="current_grid_index">index of the current local grid</param> /// <param name="current_disparity_index">index of teh current disparities set within the disparities file</param> /// <param name="robot_pose">the current robot pose</param> /// <param name="buffer">buffer containing two metagrids</param> /// <param name="current_buffer_index">index of the currently active grid within the buffer (0 or 1)</param> /// <param name="grid_centres">list of grid centre positions (x,y,z)</param> /// <param name="update_map">returns whether the map should be updated</param> /// <param name="debug_mapping_filename">filename to save debugging images</param> /// <param name="overall_map_filename">filename to save an overall map of the path</param> /// <param name="overall_map_img">overall map image data</param> /// <param name="overall_map_dimension_mm">dimension of the overall map in millimetres</param> /// <param name="overall_map_centre_x_mm">centre x position of the overall map in millimetres</param> /// <param name="overall_map_centre_y_mm">centre x position of the overall map in millimetres</param> /// <returns>true if we have transitioned from one grid to the next</returns> public static bool MoveToNextLocalGrid( ref int current_grid_index, ref int current_disparity_index, pos3D robot_pose, metagrid[] buffer, ref int current_buffer_index, List<float> grid_centres, ref bool update_map, string debug_mapping_filename, string overall_map_filename, ref byte[] overall_map_img, int overall_img_width, int overall_img_height, int overall_map_dimension_mm, int overall_map_centre_x_mm, int overall_map_centre_y_mm) { bool buffer_transition = false; update_map = false; // if this is the first time that localisation // has been called since loading the path // then update the map if ((current_grid_index == 0) && (current_disparity_index == 0)) { update_map = true; float grid_centre_x_mm = grid_centres[current_grid_index * 3]; float grid_centre_y_mm = grid_centres[(current_grid_index * 3) + 1]; float grid_centre_z_mm = grid_centres[(current_grid_index * 3) + 2]; buffer[current_buffer_index].SetPosition(grid_centre_x_mm, grid_centre_y_mm, grid_centre_z_mm, 0); int next_grid_index = current_grid_index + 1; if (next_grid_index >= grid_centres.Count / 3) next_grid_index = current_grid_index; grid_centre_x_mm = grid_centres[next_grid_index * 3]; grid_centre_y_mm = grid_centres[(next_grid_index * 3) + 1]; grid_centre_z_mm = grid_centres[(next_grid_index * 3) + 2]; buffer[1 - current_buffer_index].SetPosition(grid_centre_x_mm, grid_centre_y_mm, grid_centre_z_mm, 0); } // distance to the centre of the currently active grid float dx = robot_pose.x - buffer[current_buffer_index].x; float dy = robot_pose.y - buffer[current_buffer_index].y; float dz = robot_pose.z - buffer[current_buffer_index].z; float dist_to_grid_centre_sqr_0 = dx*dx + dy*dy + dz*dz; dx = robot_pose.x - buffer[1 - current_buffer_index].x; dy = robot_pose.y - buffer[1 - current_buffer_index].y; dz = robot_pose.z - buffer[1 - current_buffer_index].z; float dist_to_grid_centre_sqr_1 = dx*dx + dy*dy + dz*dz; // if we are closer to the next grid than the current one // then swap the currently active grid //if (dist_to_grid_centre_sqr_0 > dimension_mm/2) if (dist_to_grid_centre_sqr_1 < dist_to_grid_centre_sqr_0) { if ((debug_mapping_filename != null) && (debug_mapping_filename != "")) { bool show_localisation = true; int debug_img_width = 640; int debug_img_height = 480; byte[] debug_img = new byte[debug_img_width * debug_img_height * 3]; Bitmap debug_bmp = new Bitmap(debug_img_width, debug_img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb); buffer[current_buffer_index].Show(0, debug_img, debug_img_width, debug_img_height, false, show_localisation); BitmapArrayConversions.updatebitmap_unsafe(debug_img, debug_bmp); if (debug_mapping_filename.ToLower().EndsWith("png")) debug_bmp.Save(debug_mapping_filename, System.Drawing.Imaging.ImageFormat.Png); if (debug_mapping_filename.ToLower().EndsWith("gif")) debug_bmp.Save(debug_mapping_filename, System.Drawing.Imaging.ImageFormat.Gif); if (debug_mapping_filename.ToLower().EndsWith("jpg")) debug_bmp.Save(debug_mapping_filename, System.Drawing.Imaging.ImageFormat.Jpeg); if (debug_mapping_filename.ToLower().EndsWith("bmp")) debug_bmp.Save(debug_mapping_filename, System.Drawing.Imaging.ImageFormat.Bmp); /* string[] str = debug_mapping_filename.Split('.'); string debug_mapping_filename2 = str[0] + "b." + str[1]; buffer[1 - current_buffer_index].Show(0, debug_img, debug_img_width, debug_img_height, false); BitmapArrayConversions.updatebitmap_unsafe(debug_img, debug_bmp); if (debug_mapping_filename2.ToLower().EndsWith("png")) debug_bmp.Save(debug_mapping_filename2, System.Drawing.Imaging.ImageFormat.Png); if (debug_mapping_filename2.ToLower().EndsWith("gif")) debug_bmp.Save(debug_mapping_filename2, System.Drawing.Imaging.ImageFormat.Gif); if (debug_mapping_filename2.ToLower().EndsWith("jpg")) debug_bmp.Save(debug_mapping_filename2, System.Drawing.Imaging.ImageFormat.Jpeg); if (debug_mapping_filename2.ToLower().EndsWith("bmp")) debug_bmp.Save(debug_mapping_filename2, System.Drawing.Imaging.ImageFormat.Bmp); */ } UpdateOverallMap( buffer, current_buffer_index, overall_map_filename, ref overall_map_img, overall_img_width, overall_img_height, overall_map_dimension_mm, overall_map_centre_x_mm, overall_map_centre_y_mm); // swap the two metagrids SwapBuffers( grid_centres, ref current_grid_index, buffer, ref current_buffer_index, ref update_map); buffer_transition = true; } return(buffer_transition); }
public void CreateStereoCameras( int no_of_stereo_cameras, float cam_baseline_mm, float dist_from_centre_of_tilt_mm, int cam_image_width, int cam_image_height, float cam_FOV_degrees, float head_diameter_mm, float default_head_orientation_degrees) { this.head_diameter_mm = head_diameter_mm; pose = new pos3D[no_of_stereo_cameras]; for (int i = 0; i < no_of_stereo_cameras; i++) pose[i] = new pos3D(0,0,0); baseline_mm = new float[no_of_stereo_cameras]; image_width = new int[no_of_stereo_cameras]; image_height = new int[no_of_stereo_cameras]; FOV_degrees = new float[no_of_stereo_cameras]; stereo_camera_position_x = new float[no_of_stereo_cameras]; stereo_camera_position_y = new float[no_of_stereo_cameras]; stereo_camera_position_z = new float[no_of_stereo_cameras]; stereo_camera_pan = new float[no_of_stereo_cameras]; stereo_camera_tilt = new float[no_of_stereo_cameras]; stereo_camera_roll = new float[no_of_stereo_cameras]; left_camera_location = new pos3D[no_of_stereo_cameras]; right_camera_location = new pos3D[no_of_stereo_cameras]; for (int cam = 0; cam < no_of_stereo_cameras; cam++) { float cam_orientation = cam * (float)Math.PI*2 / no_of_stereo_cameras; cam_orientation += default_head_orientation_degrees * (float)Math.PI / 180.0f; stereo_camera_position_x[cam] = head_diameter_mm * 0.5f * (float)Math.Sin(cam_orientation); stereo_camera_position_y[cam] = head_diameter_mm * 0.5f * (float)Math.Cos(cam_orientation); stereo_camera_position_z[cam] = dist_from_centre_of_tilt_mm; stereo_camera_pan[cam] = cam_orientation; baseline_mm[cam] = cam_baseline_mm; image_width[cam] = cam_image_width; image_height[cam] = cam_image_height; FOV_degrees[cam] = cam_FOV_degrees; } }
public float Localise( robotGeometry geom, float[][] stereo_features, byte[][,] stereo_features_colour, float[][] stereo_features_uncertainties, Random rnd, ref pos3D pose_offset, ref bool buffer_transition, string debug_mapping_filename, float known_best_pose_x_mm, float known_best_pose_y_mm, string overall_map_filename, ref byte[] overall_map_img, int overall_img_width, int overall_img_height, int overall_map_dimension_mm, int overall_map_centre_x_mm, int overall_map_centre_y_mm) { return(Localise( geom.body_width_mm, geom.body_length_mm, geom.body_centre_of_rotation_x, geom.body_centre_of_rotation_y, geom.body_centre_of_rotation_z, geom.head_centroid_x, geom.head_centroid_y, geom.head_centroid_z, geom.head_pan, geom.head_tilt, geom.head_roll, geom.baseline_mm, geom.stereo_camera_position_x, geom.stereo_camera_position_y, geom.stereo_camera_position_z, geom.stereo_camera_pan, geom.stereo_camera_tilt, geom.stereo_camera_roll, geom.image_width, geom.image_height, geom.FOV_degrees, stereo_features, stereo_features_colour, stereo_features_uncertainties, geom.sensormodel, ref geom.left_camera_location, ref geom.right_camera_location, geom.no_of_sample_poses, geom.sampling_radius_major_mm, geom.sampling_radius_minor_mm, geom.pose, geom.max_orientation_variance, geom.max_tilt_variance, geom.max_roll_variance, geom.poses, geom.pose_probability, rnd, ref pose_offset, ref buffer_transition, debug_mapping_filename, known_best_pose_x_mm, known_best_pose_y_mm, overall_map_filename, ref overall_map_img, overall_img_width, overall_img_height, overall_map_dimension_mm, overall_map_centre_x_mm, overall_map_centre_y_mm )); }
/// <summary> /// parse an xml node to extract geometry parameters /// </summary> /// <param name="xnod"></param> /// <param name="level"></param> public void LoadFromXml( XmlNode xnod, int level, ref int cameraIndex) { XmlNode xnodWorking; if (xnod.Name == "StereoCamera") { cameraIndex++; } if (xnod.Name == "BodyWidthMillimetres") { body_width_mm = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "BodyLengthMillimetres") { body_length_mm = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "BodyHeightMillimetres") { body_height_mm = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "CentreOfRotationX") { body_centre_of_rotation_x = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "CentreOfRotationY") { body_centre_of_rotation_y = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "CentreOfRotationZ") { body_centre_of_rotation_z = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "HeadCentreX") { head_centroid_x = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "HeadCentreY") { head_centroid_y = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "HeadCentreZ") { head_centroid_z = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "HeadDiameterMillimetres") { head_diameter_mm = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "MaxOrientationVarianceDegrees") { max_orientation_variance = Convert.ToSingle(xnod.InnerText) / 180.0f * (float)Math.PI; } if (xnod.Name == "MaxTiltVarianceDegrees") { max_tilt_variance = Convert.ToSingle(xnod.InnerText) / 180.0f * (float)Math.PI; } if (xnod.Name == "MaxRollVarianceDegrees") { max_roll_variance = Convert.ToSingle(xnod.InnerText) / 180.0f * (float)Math.PI; } if (xnod.Name == "NoOfStereoCameras") { cameraIndex = -1; int no_of_stereo_cameras = Convert.ToInt32(xnod.InnerText); baseline_mm = new float[no_of_stereo_cameras]; pose = new pos3D[no_of_stereo_cameras]; poses = new List<pos3D>(); stereo_camera_position_x = new float[no_of_stereo_cameras]; stereo_camera_position_y = new float[no_of_stereo_cameras]; stereo_camera_position_z = new float[no_of_stereo_cameras]; stereo_camera_pan = new float[no_of_stereo_cameras]; stereo_camera_tilt = new float[no_of_stereo_cameras]; stereo_camera_roll = new float[no_of_stereo_cameras]; image_width = new int[no_of_stereo_cameras]; image_height = new int[no_of_stereo_cameras]; FOV_degrees = new float[no_of_stereo_cameras]; left_camera_location = new pos3D[no_of_stereo_cameras]; right_camera_location = new pos3D[no_of_stereo_cameras]; for (int i = 0; i < no_of_stereo_cameras; i++) { left_camera_location[i] = new pos3D(0,0,0); right_camera_location[i] = new pos3D(0,0,0); } } if (xnod.Name == "BaselineMillimetres") { baseline_mm[cameraIndex] = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "PositionMillimetres") { string[] str = xnod.InnerText.Split(' '); stereo_camera_position_x[cameraIndex] = Convert.ToSingle(str[0]); stereo_camera_position_y[cameraIndex] = Convert.ToSingle(str[1]); stereo_camera_position_z[cameraIndex] = Convert.ToSingle(str[2]); } if (xnod.Name == "OrientationDegrees") { string[] str = xnod.InnerText.Split(' '); stereo_camera_pan[cameraIndex] = Convert.ToSingle(str[0]) / 180.0f * (float)Math.PI; stereo_camera_tilt[cameraIndex] = Convert.ToSingle(str[1]) / 180.0f * (float)Math.PI; stereo_camera_roll[cameraIndex] = Convert.ToSingle(str[2]) / 180.0f * (float)Math.PI; } if (xnod.Name == "ImageDimensions") { string[] str = xnod.InnerText.Split(' '); image_width[cameraIndex] = Convert.ToInt32(str[0]); image_height[cameraIndex] = Convert.ToInt32(str[1]); } if (xnod.Name == "FOVDegrees") { FOV_degrees[cameraIndex] = Convert.ToSingle(xnod.InnerText); } // call recursively on all children of the current node if (xnod.HasChildNodes) { xnodWorking = xnod.FirstChild; while (xnodWorking != null) { LoadFromXml(xnodWorking, level + 1, ref cameraIndex); xnodWorking = xnodWorking.NextSibling; } } }
/// <summary> /// Update the current grid with new mapping rays loaded from the disparities file /// </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="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="sensormodel">sensor model for each stereo camera</param> protected void UpdateMap( 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[] 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, stereoModel[][] sensormodel) { const bool use_compression = false; if (disparities_file_open) { float[] stereo_features; byte[,] stereo_features_colour; float[] stereo_features_uncertainties; pos3D robot_pose = new pos3D(0,0,0); //int next_grid_index = current_grid_index + 1; //if (next_grid_index >= grid_centres.Count / 3) next_grid_index = (grid_centres.Count / 3) - 1; int next_grid_index = current_grid_index; //if (current_grid_index < 1) // next_grid_index = 1; int next_disparity_index = disparities_index[next_grid_index]; for (int i = current_disparity_index; i < next_disparity_index; i++) { long time_long = disparities_reader.ReadInt64(); robot_pose.x = disparities_reader.ReadSingle(); robot_pose.y = disparities_reader.ReadSingle(); robot_pose.pan = disparities_reader.ReadSingle(); float head_pan = disparities_reader.ReadSingle(); float head_tilt = disparities_reader.ReadSingle(); float head_roll = disparities_reader.ReadSingle(); int stereo_camera_index = disparities_reader.ReadInt32(); int features_count = disparities_reader.ReadInt32(); if (use_compression) { int features_bytes = disparities_reader.ReadInt32(); byte[] fb = new byte[features_bytes]; disparities_reader.Read(fb, 0, features_bytes); byte[] packed_stereo_features2 = AcedInflator.Instance.Decompress(fb, 0, 0, 0); stereo_features = ArrayConversions.ToFloatArray(packed_stereo_features2); } else { byte[] fb = new byte[features_count * 3 * 4]; disparities_reader.Read(fb, 0, fb.Length); stereo_features = ArrayConversions.ToFloatArray(fb); } byte[] packed_stereo_feature_colours = null; if (use_compression) { int colour_bytes = disparities_reader.ReadInt32(); byte[] cb = new byte[colour_bytes]; disparities_reader.Read(cb, 0, colour_bytes); packed_stereo_feature_colours = AcedInflator.Instance.Decompress(cb, 0, 0, 0); } else { packed_stereo_feature_colours = new byte[features_count * 3]; disparities_reader.Read(packed_stereo_feature_colours, 0, packed_stereo_feature_colours.Length); } // unpack stereo features int ctr = 0; stereo_features_colour = new byte[features_count,3]; stereo_features_uncertainties = new float[features_count]; for (int f = 0; f < features_count; f++) { stereo_features_uncertainties[f] = 1; stereo_features_colour[f, 0] = packed_stereo_feature_colours[ctr++]; stereo_features_colour[f, 1] = packed_stereo_feature_colours[ctr++]; stereo_features_colour[f, 2] = packed_stereo_feature_colours[ctr++]; } // insert the rays into the map Map(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, stereo_camera_index, baseline_mm[stereo_camera_index], stereo_camera_position_x[stereo_camera_index], stereo_camera_position_y[stereo_camera_index], stereo_camera_position_z[stereo_camera_index], stereo_camera_pan[stereo_camera_index], stereo_camera_tilt[stereo_camera_index], stereo_camera_roll[stereo_camera_index], image_width[stereo_camera_index], image_height[stereo_camera_index], FOV_degrees[stereo_camera_index], stereo_features, stereo_features_colour, stereo_features_uncertainties, sensormodel, robot_pose); stereo_features = null; stereo_features_colour = null; stereo_features_uncertainties = null; } current_disparity_index = next_disparity_index; } else { Console.WriteLine("disparities file not open"); } }
/// <summary> /// sets the position and orientation of the robots head /// </summary> /// <param name="p"> /// A <see cref="pos3D"/> /// </param> public void SetHeadPositionOrientation(pos3D p) { head_centroid_x = p.x; head_centroid_y = p.y; head_centroid_z = p.z; head_pan = p.pan; head_tilt = p.tilt; head_roll = p.roll; }
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); }
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> /// insert a stereo ray into the grid /// </summary> /// <param name="ray">stereo ray</param> /// <param name="origin">pose from which this observation was made</param> /// <param name="sensormodel">sensor model for each grid level</param> /// <param name="left_camera_location">left stereo camera position and orientation</param> /// <param name="right_camera_location">right stereo camera position and orientation</param> /// <param name="localiseOnly">whether we are mapping or localising</param> /// <returns>localisation matching score</returns> public float Insert( evidenceRay ray, particlePose origin, stereoModel[] sensormodel, pos3D left_camera_location, pos3D right_camera_location, bool localiseOnly) { float matchingScore = 0; switch (grid_type) { case TYPE_MULTI_HYPOTHESIS: { for (int grid_level = 0; grid_level < grid.Length; grid_level++) { rayModelLookup sensormodel_lookup = sensormodel[grid_level].ray_model; occupancygridMultiHypothesis grd = (occupancygridMultiHypothesis)grid[grid_level]; matchingScore += grid_weight[grid_level] * grd.Insert(ray, origin, sensormodel_lookup, left_camera_location, right_camera_location, localiseOnly); } break; } } return (matchingScore); }
/// <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="scaling_factor">scaling factor (k)</param> /// <param name="img">image data</param> /// <param name="img_width">image width</param> /// <param name="img_height">image height</param> /// <param name="radius">radius in pixels</param> public static void ShowMoireGridVertices( 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, int radius) { List <pos3D> cells = new List <pos3D>(); CreateMoireGrid( sampling_radius_major_mm, sampling_radius_minor_mm, first_grid_spacing, second_grid_spacing, first_grid_rotation_degrees, second_grid_rotation_degrees, dimension_x_cells, dimension_y_cells, scaling_factor, ref cells); float min_x = float.MaxValue; float max_x = float.MinValue; float min_y = float.MaxValue; float max_y = float.MinValue; for (int i = 0; i < cells.Count; i++) { if (cells[i].x < min_x) { min_x = cells[i].x; } if (cells[i].y < min_y) { min_y = cells[i].y; } if (cells[i].x > max_x) { max_x = cells[i].x; } if (cells[i].y > max_y) { max_y = cells[i].y; } } if (max_x - min_x > max_y - min_y) { float cy = min_y + ((max_y - min_y) / 2); min_y = cy - ((max_x - min_x) / 2); max_y = min_y + (max_x - min_x); } else { float cx = min_x + ((max_x - min_x) / 2); min_x = cx - ((max_y - min_y) / 2); max_x = min_x + (max_y - min_y); } for (int i = (img_width * img_height * 3) - 1; i >= 0; i--) { img[i] = 0; } for (int i = 0; i < cells.Count; i++) { pos3D cell = cells[i]; int x = (int)((cell.x - min_x) * img_width / (max_x - min_x)); int y = (int)((cell.y - min_y) * img_height / (max_y - min_y)); drawing.drawSpot(img, img_width, img_height, x, y, radius, 255, 255, 255); } }