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