/// <summary> /// shows rays along the path /// </summary> /// <param name="img"></param> /// <param name="img_width"></param> /// <param name="img_height"></param> /// <param name="scale"></param> public void showRays(Byte[] img, int img_width, int img_height, int scale) { // clear the image for (int i = 0; i < img.Length; i++) { img[i] = 0; } if (viewpoints.Count > 0) { // find the centre of the path 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; // insert the viewpoints 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; float pan = view.odometry_position.pan; viewpoint viewAdjusted = view.createTrialPose(pan, x, y); viewAdjusted.showAbove(img, img_width, img_height, scale, 255, 255, 255, true, 0, false); } } }
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 odometry positions /// </summary> /// <param name="odometry"></param> public void update(robotOdometry odometry) { for (int v = 0; v < viewpoints.Count; v++) { viewpoint view = (viewpoint)viewpoints[v]; view.odometry_position = odometry.getPosition(v); } }
public void Load(BinaryReader binfile) { viewpoints.Clear(); int no_of_viewpoints = binfile.ReadInt32(); for (int v = 0; v < no_of_viewpoints; v++) { viewpoint new_viewpoint = new viewpoint(1); new_viewpoint.Load(binfile); Add(new_viewpoint); } }
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> /// create a new viewpoint which is a trial pose /// </summary> /// <param name="extra_pan"></param> /// <param name="translation_x"></param> /// <param name="translation_y"></param> /// <returns></returns> public viewpoint createTrialPose(float extra_pan, float translation_x, float translation_y) { viewpoint trialPose = new viewpoint(rays.Length); for (int cam = 0; cam < rays.Length; cam++) { for (int r = 0; r < rays[cam].Count; r++) { evidenceRay ray = (evidenceRay)rays[cam][r]; evidenceRay trial_ray = ray.trialPose(extra_pan, translation_x, translation_y); trialPose.rays[cam].Add(trial_ray); } } return(trialPose); }
// 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 void Add(viewpoint v) { viewpoints.Add(v); }
/// <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> /// create a new viewpoint which is a trial pose /// </summary> /// <param name="extra_pan"></param> /// <param name="translation_x"></param> /// <param name="translation_y"></param> /// <returns></returns> public viewpoint createTrialPose(float extra_pan, float translation_x, float translation_y) { viewpoint trialPose = new viewpoint(rays.Length); for (int cam = 0; cam < rays.Length; cam++) { for (int r = 0; r < rays[cam].Count; r++) { evidenceRay ray = (evidenceRay)rays[cam][r]; evidenceRay trial_ray = ray.trialPose(extra_pan, translation_x, translation_y); trialPose.rays[cam].Add(trial_ray); } } return (trialPose); }
/// <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); }