/// <summary>
/// create a map using all available track data
/// </summary>
/// <param name="grid"></param>
public void testMapping(occupancygridMultiResolution grid)
{
robotPath pathLocalisation = getLocalisationTrack(0);
robotPath pathMap = getMappingTrack(0);
grid.Clear();
grid.insert(pathMap);
grid.insert(pathLocalisation);
}
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 map using all available track data
/// </summary>
/// <param name="grid"></param>
public void testMapping(occupancygridMultiResolution grid)
{
robotPath pathLocalisation = getLocalisationTrack(0);
robotPath pathMap = getMappingTrack(0);
grid.Clear();
grid.insert(pathMap);
grid.insert(pathLocalisation);
}
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;
}
