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