/// <summary> /// creates some new poses /// </summary> private void createNewPoses(float x, float y, float pan) { for (int sample = Poses.Count; sample < survey_trial_poses; sample++) { particlePath path = new particlePath(x, y, pan, 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 pan, particlePath path) { this.x = x; this.y = y; this.pan = pan; 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 pan, particlePath path) { this.x = x; this.y = y; this.pan = pan; this.path = path; observed_grid_cells = new List <particleGridCell>(); }
/// <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> /// 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 bx, float by) { 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 pan = 2 * (float)Math.PI * rnd.Next(100000) / 100000.0f; particlePath path = new particlePath(x, y, pan, 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> /// 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> /// 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))); float ang_velocity = angular_velocity + sample_normal_distribution( (motion_noise[2] * Math.Abs(forward_velocity)) + (motion_noise[3] * Math.Abs(angular_velocity))); float v = sample_normal_distribution( (motion_noise[4] * Math.Abs(forward_velocity)) + (motion_noise[5] * Math.Abs(angular_velocity))); 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, new_pan, path); new_pose.time_step = time_step; path.Add(new_pose); }
/// <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); } } // adding an observation is sufficient to update the // pose localisation scores. This is, after all, SLAM. PosesEvaluated = true; }
/// <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> /// 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> /// 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; }
/// <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> /// 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 bx, float by) { 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 pan = 2 * (float)Math.PI * rnd.Next(100000) / 100000.0f; particlePath path = new particlePath(x, y, pan, max_path_length, time_step, path_ID, rob.LocalGridDimension); createNewPose(path); } }
/// <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))); float ang_velocity = angular_velocity + sample_normal_distribution( (motion_noise[2] * Math.Abs(forward_velocity)) + (motion_noise[3] * Math.Abs(angular_velocity))); float v = sample_normal_distribution( (motion_noise[4] * Math.Abs(forward_velocity)) + (motion_noise[5] * Math.Abs(angular_velocity))); 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, new_pan, path); new_pose.time_step = time_step; path.Add(new_pose); }
/// <summary> /// creates some new poses /// </summary> private void createNewPoses(float x, float y, float pan) { for (int sample = Poses.Count; sample < survey_trial_poses; sample++) { particlePath path = new particlePath(x, y, pan, 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> /// 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; }
/// <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> /// turns a list of path segments into a list of individual poses /// </summary> private void updatePath() { particlePose prev_pose=null; path = new particlePath(999999999); min_x = 9999; min_y = 9999; max_x = -9999; max_y = -9999; for (int s = 0; s < pathSegments.Count; s++) { simulationPathSegment segment = (simulationPathSegment)pathSegments[s]; // get the last pose if (s > 0) prev_pose = (particlePose)path.path[path.path.Count - 1]; // update the list of poses List<particlePose> poses = segment.getPoses(); if (prev_pose != null) { // is the last pose position the same as the first in this segment? // if so, remove the last pose added to the path particlePose firstPose = (particlePose)poses[0]; if (((int)firstPose.x == (int)prev_pose.x) && ((int)firstPose.y == (int)prev_pose.y) && (Math.Abs(firstPose.pan - prev_pose.pan) < 0.01f)) path.path.RemoveAt(path.path.Count - 1); } for (int i = 0; i < poses.Count; i++) { particlePose pose = (particlePose)poses[i]; if (pose.x < min_x) min_x = pose.x; if (pose.y < min_y) min_y = pose.y; if (pose.x > max_x) max_x = pose.x; if (pose.y > max_y) max_y = pose.y; path.Add(pose); } } // update the path velocities velocities = path.getVelocities(0, 0, time_per_index_sec); }
/// <summary> /// turns a list of path segments into a list of individual poses /// </summary> private void updatePath() { particlePose prev_pose = null; path = new particlePath(999999999); min_x = 9999; min_y = 9999; max_x = -9999; max_y = -9999; for (int s = 0; s < pathSegments.Count; s++) { simulationPathSegment segment = (simulationPathSegment)pathSegments[s]; // get the last pose if (s > 0) { prev_pose = (particlePose)path.path[path.path.Count - 1]; } // update the list of poses List <particlePose> poses = segment.getPoses(); if (prev_pose != null) { // is the last pose position the same as the first in this segment? // if so, remove the last pose added to the path particlePose firstPose = (particlePose)poses[0]; if (((int)firstPose.x == (int)prev_pose.x) && ((int)firstPose.y == (int)prev_pose.y) && (Math.Abs(firstPose.pan - prev_pose.pan) < 0.01f)) { path.path.RemoveAt(path.path.Count - 1); } } for (int i = 0; i < poses.Count; i++) { particlePose pose = (particlePose)poses[i]; if (pose.x < min_x) { min_x = pose.x; } if (pose.y < min_y) { min_y = pose.y; } if (pose.x > max_x) { max_x = pose.x; } if (pose.y > max_y) { max_y = pose.y; } path.Add(pose); } } // update the path velocities velocities = path.getVelocities(0, 0, time_per_index_sec); }
/// <summary> /// plan a route to a given location /// </summary> /// <param name="destination_waypoint"></param> public void PlanRoute(String destination_waypoint) { // clear the planned path planned_path = null; // retrieve the waypoint from the list of work sites kmlPlacemarkPoint waypoint = worksites.GetPoint(destination_waypoint); if (waypoint != null) { // get the destination waypoint position in millimetres // (the KML format stores positions in degrees) float destination_x = 0, destination_y = 0; waypoint.GetPositionMillimetres(ref destination_x, ref destination_y); // the best performing local grid occupancygridMultiHypothesis grid = LocalGrid[best_grid_index]; // create a planner createPlanner(grid); // set variables, in the unlikely case that the centre position // of the grid has been changed since the planner was initialised planner.init(grid.navigable_space, grid.x, grid.y); // update the planner, in order to assign safety scores to the // navigable space. The efficiency of this could be improved // by updating only those areas of the map which have changed planner.Update(0, 0, LocalGridDimension - 1, LocalGridDimension - 1); // create the plan List<float> new_plan = planner.CreatePlan(x, y, destination_x, destination_y); if (new_plan.Count > 0) { // convert the plan into a set of poses planned_path = new particlePath(new_plan.Count / 2); float prev_xx = 0, prev_yy = 0; for (int i = 0; i < new_plan.Count; i += 2) { float xx = (float)new_plan[i]; float yy = (float)new_plan[i + 1]; if (i > 0) { float dx = xx - prev_xx; float dy = yy - prev_yy; float dist = (float)Math.Sqrt((dx * dx) + (dy * dy)); if (dist > 0) { // TODO: check this pan angle float pan = (float)Math.Sin(dx / dist); if (dy < 0) pan = (2 * (float)Math.PI) - pan; // create a pose and add it to the planned path particlePose new_pose = new particlePose(prev_xx, prev_yy, pan, planned_path); planned_path.Add(new_pose); } } prev_xx = xx; prev_yy = yy; } } } }