/// <summary>
/// update all current poses with the current observation
/// </summary>
/// <param name="stereo_rays">list of evidence rays to be inserted into the grid</param>
/// <param name="localiseOnly">if true does not add any mapping particles (pure localisation)</param>
public void AddObservation(
List <evidenceRay>[] stereo_rays,
bool localiseOnly)
{
for (int p = 0; p < Poses.Count; p++)
{
particlePath path = Poses[p];
float logodds_localisation_score =
path.current_pose.AddObservation(
stereo_rays,
rob, LocalGrid,
localiseOnly);
if (logodds_localisation_score != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
{
updatePoseScore(path, logodds_localisation_score);
}
else
{
//Console.WriteLine("Pose has no score");
}
}
// adding an observation is sufficient to update the
// pose localisation scores. This is, after all, SLAM.
PosesEvaluated = true;
//Console.WriteLine(Poses.Count.ToString() + " poses evaluated");
}
/// <summary>
/// add a new path to the list
/// </summary>
/// <param name="path"></param>
private void createNewPose(particlePath path)
{
Poses.Add(path);
ActivePoses.Add(path);
// increment the path ID
path_ID++;
if (path_ID > UInt32.MaxValue - 3)
{
path_ID = 0; // rollover
}
}
/// <summary>
/// increment or decrement the total number of child branches supported by this path
/// </summary>
/// <param name="path"></param>
/// <param name="increment">increment of 1 or -1</param>
private void incrementPathChildren(
particlePath path,
int increment)
{
path.total_children += increment;
particlePath p = path;
while (p.branch_pose != null)
{
p = p.branch_pose.path;
p.total_children += increment;
}
}
/// <summary>
/// increment or decrement the total number of child branches supported by this path
/// </summary>
/// <param name="path"></param>
/// <param name="increment">increment of 1 or -1</param>
private void incrementPathChildren(
particlePath path,
int increment)
{
path.total_children += increment;
particlePath p = path;
while (p.branch_pose != null)
{
p = p.branch_pose.path;
p.total_children += increment;
}
}
/// <summary>
/// creates some new poses
/// </summary>
private void createNewPoses(
float x,
float y,
float z,
float pan,
float tilt,
float roll)
{
for (int sample = Poses.Count; sample < survey_trial_poses; sample++)
{
particlePath path = new particlePath(x, y, z, pan, tilt, roll, max_path_length, time_step, path_ID, rob.LocalGridDimension);
createNewPose(path);
}
}
/// <summary>
/// constructor
/// </summary>
/// <param name="x">x position in millimetres</param>
/// <param name="y">y position in millimetres</param>
/// <param name="pan">pan angle in radians</param>
/// <param name="path">the path which this pose belongs to</param>
public particlePose(
float x, float y, float z,
float pan, float tilt, float roll,
particlePath path)
{
this.x = x;
this.y = y;
this.z = z;
this.pan = pan;
this.tilt = tilt;
this.roll = roll;
this.path = path;
observed_grid_cells = new List<particleGridCell>();
}
/// <summary>
/// constructor
/// </summary>
/// <param name="x">x position in millimetres</param>
/// <param name="y">y position in millimetres</param>
/// <param name="pan">pan angle in radians</param>
/// <param name="path">the path which this pose belongs to</param>
public particlePose(
float x, float y, float z,
float pan, float tilt, float roll,
particlePath path)
{
this.x = x;
this.y = y;
this.z = z;
this.pan = pan;
this.tilt = tilt;
this.roll = roll;
this.path = path;
observed_grid_cells = new List <particleGridCell>();
}
/// <summary>
/// show the tree of possible paths
/// </summary>
/// <param name="img"></param>
/// <param name="width"></param>
/// <param name="height"></param>
/// <param name="r"></param>
/// <param name="g"></param>
/// <param name="b"></param>
/// <param name="line_thickness"></param>
public void ShowTree(
byte[] img, int width, int height,
int r, int g, int b, int line_thickness)
{
for (int i = 0; i < ActivePoses.Count; i++)
{
bool clearBackground = false;
if (i == 0)
{
clearBackground = true;
}
particlePath path = ActivePoses[i];
path.Show(img, width, height, r, g, b, line_thickness,
min_tree_x, min_tree_y, max_tree_x, max_tree_y,
clearBackground, root_time_step);
}
}
/// <summary>
/// update the given pose using the motion model
/// </summary>
/// <param name="path">path to add the new estimated pose to</param>
/// <param name="time_elapsed_sec">time elapsed since the last update in seconds</param>
private void sample_motion_model_velocity(
particlePath path,
float time_elapsed_sec)
{
// calculate noisy velocities
float fwd_velocity = forward_velocity + sample_normal_distribution(
(motion_noise[0] * Math.Abs(forward_velocity)) +
(motion_noise[1] * Math.Abs(angular_velocity_pan)));
float ang_velocity = angular_velocity_pan + sample_normal_distribution(
(motion_noise[2] * Math.Abs(forward_velocity)) +
(motion_noise[3] * Math.Abs(angular_velocity_pan)));
float v = sample_normal_distribution(
(motion_noise[4] * Math.Abs(forward_velocity)) +
(motion_noise[5] * Math.Abs(angular_velocity_pan)));
float fraction = 0;
if (Math.Abs(ang_velocity) > 0.000001f)
{
fraction = fwd_velocity / ang_velocity;
}
float current_pan = path.current_pose.pan;
// if scan matching is active use the current estimated pan angle
if (rob.ScanMatchingPanAngleEstimate != scanMatching.NOT_MATCHED)
{
current_pan = rob.ScanMatchingPanAngleEstimate;
}
float pan2 = current_pan - (ang_velocity * time_elapsed_sec);
float new_y = path.current_pose.y + (fraction * (float)Math.Sin(current_pan)) -
(fraction * (float)Math.Sin(pan2));
float new_x = path.current_pose.x - (fraction * (float)Math.Cos(current_pan)) +
(fraction * (float)Math.Cos(pan2));
float new_pan = pan2 + (v * time_elapsed_sec);
particlePose new_pose = new particlePose(new_x, new_y, 0, new_pan, 0, 0, path);
new_pose.time_step = time_step;
path.Add(new_pose);
}
/// <summary>
/// create poses distributed over an area, suitable for
/// monte carlo localisation
/// </summary>
/// <param name="tx">top left of the area in millimetres</param>
/// <param name="ty">top left of the area in millimetres</param>
/// <param name="bx">bottom right of the area in millimetres</param>
/// <param name="by">bottom right of the area in millimetres</param>
private void createNewPosesMonteCarlo(
float tx,
float ty,
float tz,
float bx,
float by,
float bz)
{
for (int sample = Poses.Count; sample < survey_trial_poses; sample++)
{
float x = tx + rnd.Next((int)(bx - tx));
float y = ty + rnd.Next((int)(by - ty));
float z = tz + rnd.Next((int)(bz - tz));
float pan = 2 * (float)Math.PI * rnd.Next(100000) / 100000.0f;
float tilt = 2 * (float)Math.PI * rnd.Next(100000) / 100000.0f;
float roll = 2 * (float)Math.PI * rnd.Next(100000) / 100000.0f;
particlePath path = new particlePath(x, y, z, pan, tilt, roll, max_path_length, time_step, path_ID, rob.LocalGridDimension);
createNewPose(path);
}
}
/// <summary>
/// constructor
/// </summary>
/// <param name="parent">parent path from which this originates</param>
/// <param name="path_ID">unique ID for this path</param>
/// <param name="grid_dimension_cells">dimension of the grid</param>
public particlePath(
particlePath parent,
UInt32 path_ID,
int grid_dimension_cells)
{
ID = path_ID;
parent.current_pose.no_of_children++;
current_pose = parent.current_pose;
branch_pose = parent.current_pose;
incrementPathChildren(parent, 1);
this.max_length = parent.max_length;
path = new List <particlePose>();
total_score = parent.total_score;
total_poses = parent.total_poses;
Enabled = true;
// map cache for this path
//map_cache = new ArrayList[grid_dimension_cells][][];
}
/// <summary>
/// update the given pose using the motion model
/// </summary>
/// <param name="path">path to add the new estimated pose to</param>
/// <param name="time_elapsed_sec">time elapsed since the last update in seconds</param>
private void sample_motion_model_speed(
particlePath path,
float time_elapsed_sec)
{
float fwd_velocity = forward_velocity +
sample_normal_distribution(
speed_uncertainty_forward);
float ang_velocity = angular_velocity_pan +
sample_normal_distribution(
speed_uncertainty_angular);
float new_pan = path.current_pose.pan + (ang_velocity * time_elapsed_sec);
float dist = fwd_velocity * time_elapsed_sec;
float new_y = path.current_pose.y + (dist * (float)Math.Cos(new_pan));
float new_x = path.current_pose.x + (dist * (float)Math.Sin(new_pan));
particlePose new_pose = new particlePose(new_x, new_y, 0, new_pan, 0, 0, path);
new_pose.time_step = time_step;
path.Add(new_pose);
}
/// <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 path
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>
/// update the given pose using the motion model
/// </summary>
/// <param name="path">path to add the new estimated pose to</param>
/// <param name="time_elapsed_sec">time elapsed since the last update in seconds</param>
private void sample_motion_model_speed(
particlePath path,
float time_elapsed_sec)
{
float fwd_velocity = forward_velocity +
sample_normal_distribution(
speed_uncertainty_forward);
float ang_velocity = angular_velocity_pan +
sample_normal_distribution(
speed_uncertainty_angular);
float new_pan = path.current_pose.pan + (ang_velocity * time_elapsed_sec);
float dist = fwd_velocity * time_elapsed_sec;
float new_y = path.current_pose.y + (dist * (float)Math.Cos(new_pan));
float new_x = path.current_pose.x + (dist * (float)Math.Sin(new_pan));
particlePose new_pose = new particlePose(new_x, new_y, 0, new_pan, 0, 0, path);
new_pose.time_step = time_step;
path.Add(new_pose);
}
/// <summary>
/// update the given pose using the motion model
/// </summary>
/// <param name="path">path to add the new estimated pose to</param>
/// <param name="time_elapsed_sec">time elapsed since the last update in seconds</param>
private void sample_motion_model_velocity(
particlePath path,
float time_elapsed_sec)
{
// calculate noisy velocities
float fwd_velocity = forward_velocity + sample_normal_distribution(
(motion_noise[0] * Math.Abs(forward_velocity)) +
(motion_noise[1] * Math.Abs(angular_velocity_pan)));
float ang_velocity = angular_velocity_pan + sample_normal_distribution(
(motion_noise[2] * Math.Abs(forward_velocity)) +
(motion_noise[3] * Math.Abs(angular_velocity_pan)));
float v = sample_normal_distribution(
(motion_noise[4] * Math.Abs(forward_velocity)) +
(motion_noise[5] * Math.Abs(angular_velocity_pan)));
float fraction = 0;
if (Math.Abs(ang_velocity) > 0.000001f) fraction = fwd_velocity / ang_velocity;
float current_pan = path.current_pose.pan;
// if scan matching is active use the current estimated pan angle
if (rob.ScanMatchingPanAngleEstimate != scanMatching.NOT_MATCHED)
current_pan = rob.ScanMatchingPanAngleEstimate;
float pan2 = current_pan - (ang_velocity * time_elapsed_sec);
float new_y = path.current_pose.y + (fraction * (float)Math.Sin(current_pan)) -
(fraction * (float)Math.Sin(pan2));
float new_x = path.current_pose.x - (fraction * (float)Math.Cos(current_pan)) +
(fraction * (float)Math.Cos(pan2));
float new_pan = pan2 + (v * time_elapsed_sec);
particlePose new_pose = new particlePose(new_x, new_y, 0, new_pan, 0, 0, path);
new_pose.time_step = time_step;
path.Add(new_pose);
}
/// <summary>
/// create poses distributed over an area, suitable for
/// monte carlo localisation
/// </summary>
/// <param name="tx">top left of the area in millimetres</param>
/// <param name="ty">top left of the area in millimetres</param>
/// <param name="bx">bottom right of the area in millimetres</param>
/// <param name="by">bottom right of the area in millimetres</param>
private void createNewPosesMonteCarlo(
float tx,
float ty,
float tz,
float bx,
float by,
float bz)
{
for (int sample = Poses.Count; sample < survey_trial_poses; sample++)
{
float x = tx + rnd.Next((int)(bx - tx));
float y = ty + rnd.Next((int)(by - ty));
float z = tz + rnd.Next((int)(bz - tz));
float pan = 2 * (float)Math.PI * rnd.Next(100000) / 100000.0f;
float tilt = 2 * (float)Math.PI * rnd.Next(100000) / 100000.0f;
float roll = 2 * (float)Math.PI * rnd.Next(100000) / 100000.0f;
particlePath path = new particlePath(x, y, z, pan, tilt, roll, max_path_length, time_step, path_ID, rob.LocalGridDimension);
createNewPose(path);
}
}
/// <summary>
/// creates some new poses
/// </summary>
private void createNewPoses(
float x,
float y,
float z,
float pan,
float tilt,
float roll)
{
for (int sample = Poses.Count; sample < survey_trial_poses; sample++)
{
particlePath path = new particlePath(x, y, z, pan, tilt, roll, max_path_length, time_step, path_ID, rob.LocalGridDimension);
createNewPose(path);
}
}
/// <summary>
/// add a new path to the list
/// </summary>
/// <param name="path"></param>
private void createNewPose(particlePath path)
{
Poses.Add(path);
ActivePoses.Add(path);
// increment the path ID
path_ID++;
if (path_ID > UInt32.MaxValue - 3) path_ID = 0; // rollover
}
/// <summary>
/// updates the given path with the score obtained from the current pose
/// Note that scores are always expressed as log odds matching probability
/// from grid localisation
/// </summary>
/// <param name="path">path whose score is to be updated</param>
/// <param name="new_score">the new score to be added to the path</param>
public void updatePoseScore(particlePath path, float new_score)
{
path.total_score += new_score;
path.local_score += new_score;
}
/// <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 path
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>
/// updates the given path with the score obtained from the current pose
/// Note that scores are always expressed as log odds matching probability
/// from grid localisation
/// </summary>
/// <param name="path">path whose score is to be updated</param>
/// <param name="new_score">the new score to be added to the path</param>
public void updatePoseScore(particlePath path, float new_score)
{
path.total_score += new_score;
path.local_score += new_score;
}
/// <summary>
/// constructor
/// </summary>
/// <param name="parent">parent path from which this originates</param>
/// <param name="path_ID">unique ID for this path</param>
/// <param name="grid_dimension_cells">dimension of the grid</param>
public particlePath(
particlePath parent,
UInt32 path_ID,
int grid_dimension_cells)
{
ID = path_ID;
parent.current_pose.no_of_children++;
current_pose = parent.current_pose;
branch_pose = parent.current_pose;
incrementPathChildren(parent, 1);
this.max_length = parent.max_length;
path = new List<particlePose>();
total_score = parent.total_score;
total_poses = parent.total_poses;
Enabled = true;
// map cache for this path
//map_cache = new ArrayList[grid_dimension_cells][][];
}
