/// <summary>
/// add a new grid hypothesis to this pose
/// </summary>
/// <param name="hypothesis">occupancy hypothesis for a grid cell</param>
/// <param name="radius_cells">local map cache radius in cells</param>
/// <param name="grid_dimension">dimension of the occupancy grid in cells</param>
/// <param name="grid_dimension_vertical">height of the occupancy grid (z axis) in cells</param>
/// <returns>true if the hypothesis was added</returns>
public bool AddHypothesis(
particleGridCell hypothesis,
int radius_cells,
int grid_dimension,
int grid_dimension_vertical)
{
bool added = false;
if (path != null)
{
added = path.Add(hypothesis, radius_cells, grid_dimension, grid_dimension_vertical);
if (added)
{
observed_grid_cells.Add(hypothesis);
}
}
return(added);
}
/// <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>
/// 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_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);
}
