/// <summary>
/// show the position uncertainty distribution
/// </summary>
/// <param name="img">image data within which to draw</param>
/// <param name="width">width of the image</param>
/// <param name="height">height of the image</param>
public void Show(Byte[] img, int width, int height,
bool clearBackground)
{
float min_x = 99999;
float min_y = 99999;
float max_x = -99999;
float max_y = -99999;
for (int sample = 0; sample < Poses.Count; sample++)
{
particlePose pose = Poses[sample].current_pose;
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;
}
}
Show(img, width, height, min_x, min_y, max_x, max_y, clearBackground);
}
/// <summary>
/// remove all poses along the path until a branch point is located
/// </summary>
/// <param name="pose"></param>
/// <returns></returns>
private particlePose CollapseBranch(particlePose pose,
occupancygridMultiHypothesis grid)
{
int children = 0;
while ((pose != branch_pose) &&
(pose.path == this) &&
(children == 0))
{
if (pose != current_pose)
{
children = pose.no_of_children;
}
if (children == 0)
{
if (pose.path == this)
{
pose.Remove(grid);
pose = pose.parent;
}
}
}
return(pose);
}
/// <summary>
/// show an overhead view of the grid map as an image
/// </summary>
/// <param name="grid_index">index number of the grid to be shown</param>
/// <param name="img">colour image data</param>
/// <param name="width">width in pixels</param>
/// <param name="height">height in pixels</param>
/// <param name="show_all_occupied_cells">show all occupied pixels</param>
/// <param name="map_width_mm">width of the larger map area within this will be displayed</param>
/// <param name="map_height_mm">height of the larger map area within this will be displayed</param>
/// <param name="map_centre_x_mm">centre x position of the larger map area within this will be displayed</param>
/// <param name="map_centre_y_mm">centre x position of the larger map area within this will be displayed</param>
/// <param name="clear_image">clear the image</param>
/// <param name="show_localisation">whether to show grid cells probed during localisation</param>
public void Show(
int grid_index,
byte[] img,
int width,
int height,
bool show_all_occupied_cells,
int map_width_mm,
int map_height_mm,
int map_centre_x_mm,
int map_centre_y_mm,
bool clear_image,
bool show_localisation)
{
switch (grid_type)
{
case TYPE_SIMPLE:
{
occupancygridSimple grd = (occupancygridSimple)grid[grid_index];
grd.Show(img, width, height, show_all_occupied_cells, map_width_mm, map_height_mm, map_centre_x_mm, map_centre_y_mm, clear_image, show_localisation);
break;
}
case TYPE_MULTI_HYPOTHESIS:
{
// TODO: get the best pose
particlePose pose = null;
occupancygridMultiHypothesis grd = (occupancygridMultiHypothesis)grid[grid_index];
grd.Show(
occupancygridMultiHypothesis.VIEW_ABOVE,
img, width, height, pose,
true, true);
break;
}
}
}
/// <summary>
/// returns a list containing the robots velocity at each point in the path
/// </summary>
/// <param name="starting_forward_velocity">the forward velocity to start with in mm/sec</param>
/// <param name="starting_angular_velocity">the angular velocity to start with in radians/sec</param>
/// <param name="time_per_index_sec">time taken between indexes along the path in seconds</param>
/// <returns>list of forward and angular velocities</returns>
public List <float> getVelocities(float starting_forward_velocity,
float starting_angular_velocity,
float time_per_index_sec)
{
List <float> result = new List <float>();
float forward_velocity = starting_forward_velocity;
float angular_velocity = starting_angular_velocity;
for (int i = 1; i < path.Count; i++)
{
particlePose prev_pose = path[i - 1];
particlePose pose = path[i];
float dx = pose.x - prev_pose.x;
float dy = pose.y - prev_pose.y;
float distance = (float)Math.Sqrt((dx * dx) + (dy * dy));
float acceleration = (2 * (distance - (forward_velocity * time_per_index_sec))) / (time_per_index_sec * time_per_index_sec);
acceleration /= time_per_index_sec;
forward_velocity = forward_velocity + (acceleration * time_per_index_sec);
result.Add(forward_velocity);
distance = pose.pan - prev_pose.pan;
acceleration = (2 * (distance - (angular_velocity * time_per_index_sec))) / (time_per_index_sec * time_per_index_sec);
acceleration /= time_per_index_sec;
angular_velocity = angular_velocity + (acceleration * time_per_index_sec);
result.Add(angular_velocity);
}
return(result);
}
/// <summary>
/// probes the grid using the given 3D line and returns the distance to the nearest occupied grid cell
/// </summary>
/// <param name="grid_index">index number of the grid to be probed</param>
/// <param name="x0_mm">start x coordinate</param>
/// <param name="y0_mm">start y coordinate</param>
/// <param name="z0_mm">start z coordinate</param>
/// <param name="x1_mm">end x coordinate</param>
/// <param name="y1_mm">end y coordinate</param>
/// <param name="z1_mm">end z coordinate</param>
/// <returns>range to the nearest occupied grid cell in millimetres</returns>
public float ProbeRange(
int grid_index,
float x0_mm,
float y0_mm,
float z0_mm,
float x1_mm,
float y1_mm,
float z1_mm)
{
float range_mm = -1;
switch (grid_type)
{
case TYPE_SIMPLE:
{
range_mm = grid[grid_index].ProbeRange(x0_mm, y0_mm, z0_mm, x1_mm, y1_mm, z1_mm);
break;
}
case TYPE_MULTI_HYPOTHESIS:
{
occupancygridMultiHypothesis grd = (occupancygridMultiHypothesis)grid[grid_index];
// TODO get the best pose estimate
particlePose pose = null;
range_mm = grd.ProbeRange(x0_mm, y0_mm, z0_mm, x1_mm, y1_mm, z1_mm);
break;
}
}
return(range_mm);
}
/// <summary>
/// insert a stereo ray into the grid
/// </summary>
/// <param name="ray">stereo ray</param>
/// <param name="origin">pose from which this observation was made</param>
/// <param name="sensormodel">sensor model for each grid level</param>
/// <param name="left_camera_location">left stereo camera position and orientation</param>
/// <param name="right_camera_location">right stereo camera position and orientation</param>
/// <param name="localiseOnly">whether we are mapping or localising</param>
/// <returns>localisation matching score</returns>
public float Insert(
evidenceRay ray,
particlePose origin,
stereoModel[] sensormodel,
pos3D left_camera_location,
pos3D right_camera_location,
bool localiseOnly)
{
float matchingScore = 0;
switch (grid_type)
{
case TYPE_MULTI_HYPOTHESIS:
{
for (int grid_level = 0; grid_level < grid.Length; grid_level++)
{
rayModelLookup sensormodel_lookup = sensormodel[grid_level].ray_model;
occupancygridMultiHypothesis grd = (occupancygridMultiHypothesis)grid[grid_level];
matchingScore += grid_weight[grid_level] * grd.Insert(ray, origin, sensormodel_lookup, left_camera_location, right_camera_location, localiseOnly);
}
break;
}
}
return(matchingScore);
}
/// <summary>
/// parse an xml node to extract camera calibration parameters
/// </summary>
/// <param name="xnod"></param>
/// <param name="level"></param>
public void LoadFromXml(XmlNode xnod,
int level)
{
XmlNode xnodWorking;
if (xnod.Name == "Pose")
{
IFormatProvider format = new System.Globalization.CultureInfo("en-GB");
string[] poseStr = xnod.InnerText.Split(',');
particlePose new_pose = new particlePose(Convert.ToSingle(poseStr[0], format),
Convert.ToSingle(poseStr[1], format),
Convert.ToSingle(poseStr[2], format) * (float)Math.PI / 180.0f, null);
Add(new_pose);
}
// call recursively on all children of the current node
if ((xnod.HasChildNodes) &&
((xnod.Name == "RobotPath") ||
(xnod.Name == "Sentience")
))
{
xnodWorking = xnod.FirstChild;
while (xnodWorking != null)
{
LoadFromXml(xnodWorking, level + 1);
xnodWorking = xnodWorking.NextSibling;
}
}
}
/// <summary>
/// remove the mapping particles associated with this path
/// </summary>
public bool Remove(occupancygridMultiHypothesis grid)
{
Enabled = false;
particlePose pose = current_pose;
if (current_pose != null)
{
// collapse this path down to the next branching point
pose = CollapseBranch(pose, grid);
if (pose != null)
{
if (pose != current_pose)
{
if (pose.no_of_children > 0)
{
// reduce the number of children at the branch point
pose.no_of_children--;
}
}
// keep on truckin' down the line...
incrementPathChildren(pose.path, -1);
if (pose.path.total_children == 0)
{
pose.path.Remove(grid);
}
}
}
return(!Enabled);
}
/// <summary>
/// add a new pose to the path
/// </summary>
/// <param name="pose">pose to be added</param>
public void Add(particlePose pose)
{
pose.path = this;
// set the parent of this pose
pose.parent = current_pose;
// update the list of previous path IDs
if (branch_pose != null)
{
if (current_pose == branch_pose)
{
pose.previous_paths = new List <particlePath>();
int min = branch_pose.previous_paths.Count - MAX_PATH_HISTORY;
if (min < 0)
{
min = 0;
}
for (int i = min; i < branch_pose.previous_paths.Count; i++)
{
pose.previous_paths.Add(branch_pose.previous_paths[i]);
}
pose.previous_paths.Add(this);
}
else
{
pose.previous_paths = pose.parent.previous_paths;
}
}
else
{
if (pose.parent == null)
{
pose.previous_paths = new List <particlePath>();
pose.previous_paths.Add(this);
}
else
{
pose.previous_paths = pose.parent.previous_paths;
}
}
// set the current pose
current_pose = pose;
// add the pose to the path
path.Add(pose);
//total_score += pose.score;
total_poses++;
// ensure that the path does not exceed a maximum length
if (path.Count > max_length)
{
// remove the oldest pose in the path
path.RemoveAt(0);
}
}
/// <summary>
/// distill all grid particles within this path
/// </summary>
/// <param name="grid"></param>
public void Distill(occupancygridMultiHypothesis grid)
{
particlePose pose = current_pose;
while (pose != null)
{
pose.Distill(grid);
pose = pose.parent;
}
map_cache = null;
Enabled = false;
}
/// <summary>
/// constructor
/// </summary>
/// <param name="x">x coordinate of the cell within the grid</param>
/// <param name="y">y coordinate of the cell within the grid</param>
/// <param name="z">z coordinate of the cell within the grid</param>
/// <param name="probability">probability assigned to this cell</param>
/// <param name="pose">the pose from which this cell was observed</param>
/// <param name="colour">the colour of this cell</param>
public particleGridCell(
int x, int y, int z,
float probability,
particlePose pose,
byte[] colour)
{
this.x = (short)x;
this.y = (short)y;
this.z = (short)z;
this.probabilityLogOdds = probabilities.LogOdds(probability); // store as log odds
this.pose = pose;
this.colour = colour;
Enabled = true;
}
/// <summary>
/// constructor
/// </summary>
/// <param name="x">x coordinate of the cell within the grid</param>
/// <param name="y">y coordinate of the cell within the grid</param>
/// <param name="z">z coordinate of the cell within the grid</param>
/// <param name="probability">probability assigned to this cell</param>
/// <param name="pose">the pose from which this cell was observed</param>
/// <param name="colour">the colour of this cell</param>
public particleGridCell(
int x, int y, int z,
float probability,
particlePose pose,
byte[] colour)
{
this.x = (short)x;
this.y = (short)y;
this.z = (short)z;
this.probabilityLogOdds = probabilities.LogOdds(probability); // store as log odds
this.pose = pose;
this.colour = colour;
Enabled = true;
}
/// <summary>
/// show an above view of the robot using the best available pose
/// </summary>
/// <param name="img">image data within which to draw</param>
/// <param name="width">width of the image</param>
/// <param name="height">height of the image</param>
/// <param name="min_x">bounding box top left x coordinate</param>
/// <param name="min_y">bounding box top left y coordinate</param>
/// <param name="max_x">bounding box bottom right x coordinate</param>
/// <param name="max_y">bounding box bottom right y coordinate</param>
/// <param name="clear_background">whether to clear the image before drawing</param>
public void ShowBestPose(Byte[] img, int width, int height,
int min_x, int min_y, int max_x, int max_y,
bool clear_background, bool showFieldOfView)
{
if (best_path != null)
{
particlePose best_pose = best_path.current_pose;
if (best_pose != null)
{
best_pose.Show(rob, img, width, height,
clear_background, min_x, min_y, max_x, max_y, 0, showFieldOfView);
}
}
}
/// <summary>
/// return this path segment as a sequence of poses
/// </summary>
/// <returns>list of poses of type particlePose</returns>
public List<particlePose> getPoses()
{
List<particlePose> result = new List<particlePose>();
float xx = x;
float yy = y;
float curr_pan = pan;
for (int i = 0; i < no_of_steps; i++)
{
particlePose pose = new particlePose(xx, yy, curr_pan, null);
result.Add(pose);
curr_pan += pan_per_step;
xx += (distance_per_step_mm * (float)Math.Sin(curr_pan));
yy += (distance_per_step_mm * (float)Math.Cos(curr_pan));
}
return (result);
}
/// <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="max_length">the maximum number of poses which may be stored within this path</param>
/// <param name="time_step">time step</param>
/// <param name="path_ID">ID number for the path</param>
/// <param name="grid_dimension_cells">occupancy grid dimension</param>
public particlePath(float x,
float y,
float pan,
int max_length,
UInt32 time_step,
UInt32 path_ID,
int grid_dimension_cells)
{
ID = path_ID;
this.max_length = max_length;
path = new List <particlePose>();
particlePose pose = new particlePose(x, y, pan, this);
pose.time_step = time_step;
Enabled = true;
Add(pose);
}
/// <summary>
/// return this path segment as a sequence of poses
/// </summary>
/// <returns>list of poses of type particlePose</returns>
public List <particlePose> getPoses()
{
List <particlePose> result = new List <particlePose>();
float xx = x;
float yy = y;
float curr_pan = pan;
for (int i = 0; i < no_of_steps; i++)
{
particlePose pose = new particlePose(xx, yy, curr_pan, null);
result.Add(pose);
curr_pan += pan_per_step;
xx += (distance_per_step_mm * (float)Math.Sin(curr_pan));
yy += (distance_per_step_mm * (float)Math.Cos(curr_pan));
}
return(result);
}
/// <summary>
/// show an above view of the robot using the best available pose
/// </summary>
/// <param name="img">image data within which to draw</param>
/// <param name="width">width of the image</param>
/// <param name="height">height of the image</param>
/// <param name="clear_background">whether to clear the image before drawing</param>
public void ShowBestPose(byte[] img, int width, int height,
bool clear_background, bool showFieldOfView)
{
if (best_path != null)
{
particlePose best_pose = best_path.current_pose;
if (best_pose != null)
{
int min_x = (int)(best_pose.x - rob.BodyWidth_mm);
int min_y = (int)(best_pose.y - rob.BodyLength_mm);
int max_x = (int)(best_pose.x + rob.BodyWidth_mm);
int max_y = (int)(best_pose.y + rob.BodyLength_mm);
ShowBestPose(img, width, height, min_x, min_y, max_x, max_y, clear_background, showFieldOfView);
}
}
}
/// <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>
/// 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>
/// returns the path as an xml object
/// </summary>
/// <param name="doc"></param>
/// <param name="parent"></param>
/// <returns></returns>
public XmlElement getXml(XmlDocument doc,
XmlElement parent)
{
XmlElement nodePath = doc.CreateElement("RobotPath");
parent.AppendChild(nodePath);
xml.AddComment(doc, nodePath, "The path through which the robot has moved, as an X,Y coordinate");
xml.AddComment(doc, nodePath, "in millimetres followed by the heading in degrees");
IFormatProvider format = new System.Globalization.CultureInfo("en-GB");
for (int i = 0; i < path.Count; i++)
{
particlePose pose = path[i];
xml.AddTextElement(doc, nodePath, "Pose", Convert.ToString(pose.x, format) + "," +
Convert.ToString(pose.y, format) + "," +
Convert.ToString(pose.pan * 180 / Math.PI, format));
}
return(nodePath);
}
/// <summary>
/// show the position uncertainty distribution within the given region
/// </summary>
/// <param name="img">image data within which to draw</param>
/// <param name="width">width of the image</param>
/// <param name="height">height of the image</param>
/// <param name="min_x">bounding box top left x coordinate</param>
/// <param name="min_y">bounding box top left y coordinate</param>
/// <param name="max_x">bounding box bottom right x coordinate</param>
/// <param name="max_y">bounding box bottom right y coordinate</param>
/// <param name="clear_background">whether to clear the image before drawing</param>
public void Show(byte[] img, int width, int height,
float min_x_mm, float min_y_mm,
float max_x_mm, float max_y_mm,
bool clear_background)
{
if (clear_background)
{
// clear the image
for (int i = 0; i < width * height * 3; i++)
{
img[i] = (Byte)250;
}
}
if ((max_x_mm > min_x_mm) && (max_y_mm > min_y_mm))
{
for (int sample = 0; sample < Poses.Count; sample++)
{
particlePose pose = Poses[sample].current_pose;
int x = (int)((pose.x - min_x_mm) * (width - 1) / (max_x_mm - min_x_mm));
if ((x > 0) && (x < width))
{
int y = height - 1 - (int)((pose.y - min_y_mm) * (height - 1) / (max_y_mm - min_y_mm));
if ((y > 0) && (y < height))
{
int n = ((y * width) + x) * 3;
for (int col = 0; col < 3; col++)
{
img[n + col] = (Byte)0;
}
}
}
}
}
// show the path
ShowPath(img, width, height, 0, 255, 0, 0, min_x_mm, min_y_mm, max_x_mm, max_y_mm, false);
}
/// <summary>
/// return the probability of occupancy for the entire cell
/// </summary>
/// <param name="pose">pose from which the observation was made</param>
/// <returns>probability as log odds</returns>
public float GetProbability(
particlePose pose,
int x, int y,
float[] mean_colour,
ref float mean_variance)
{
float probabilityLogOdds = 0;
int hits = 0;
mean_variance = 0;
mean_colour[0] = 0;
mean_colour[1] = 0;
mean_colour[2] = 0;
for (int z = Hypothesis.Length - 1; z >= 0; z--)
{
float variance = 0;
float probLO = GetProbability(pose, x, y, z, true, temp_colour, ref variance);
if (probLO != NO_OCCUPANCY_EVIDENCE)
{
probabilityLogOdds += probLO;
mean_colour[0] += temp_colour[0];
mean_colour[1] += temp_colour[1];
mean_colour[2] += temp_colour[2];
mean_variance += variance;
hits++;
}
}
if (hits > 0)
{
mean_variance /= hits;
mean_colour[0] /= hits;
mean_colour[1] /= hits;
mean_colour[2] /= hits;
}
return(probabilities.LogOddsToProbability(probabilityLogOdds));
}
/// <summary>
/// save the occupancy grid data to file as a tile
/// it is expected that multiple tiles will be saved per grid
/// </summary>
/// <param name="binfile">file to write to</param>
/// <param name="pose">best available pose</param>
/// <param name="centre_x">centre of the tile in grid cells</param>
/// <param name="centre_y">centre of the tile in grid cells</param>
/// <param name="width_cells">width of the tile in grid cells</param>
public void SaveTile(BinaryWriter binfile,
particlePose pose,
int centre_x,
int centre_y,
int width_cells)
{
// write the whole thing to disk in one go
binfile.Write(SaveTile(pose, centre_x, centre_y, width_cells));
}
/// <summary>
/// save the entire grid to file
/// </summary>
/// <param name="filename"></param>
/// <param name="pose">best available pose</param>
public void Save(string filename,
particlePose pose)
{
FileStream fp = new FileStream(filename, FileMode.Create);
BinaryWriter binfile = new BinaryWriter(fp);
Save(binfile, pose);
binfile.Close();
fp.Close();
}
/// <summary>
/// returns the probability of occupancy at this grid cell at the given vertical (z) coordinate
/// warning: this could potentially suffer from path ID or time step rollover problems
/// </summary>
/// <param name="pose">the robot pose from which this cell was observed</param>
/// <param name="x">x grid coordinate</param>
/// <param name="y">y grid coordinate</param>
/// <param name="z">z grid coordinate</param>
/// <param name="returnLogOdds">return the probability value expressed as log odds</param>
/// <param name="colour">average colour of this grid cell</param>
/// <param name="mean_variance">average colour variance of this grid cell</param>
/// <returns>probability value</returns>
public unsafe float GetProbability(
particlePose pose,
int x,
int y,
int z,
bool returnLogOdds,
float[] colour,
ref float mean_variance)
{
int col, p, i, hits = 0;
float probabilityLogOdds = 0;
byte level;
List<particleGridCell> map_cache_observations;
List<particlePath> previous_paths;
particleGridCell h;
particlePath path;
uint time_step;
mean_variance = 1;
// pin some arrays to avoid range checking
fixed (float* unsafe_colour = colour)
{
fixed (float* unsafe_min_level = min_level)
{
fixed (float* unsafe_max_level = max_level)
{
unsafe_colour[0] = 0;
unsafe_colour[1] = 0;
unsafe_colour[2] = 0;
// first retrieve any distilled occupancy value
if (distilled != null)
if (distilled[z] != null)
{
probabilityLogOdds += distilled[z].probabilityLogOdds;
unsafe_colour[0] += distilled[z].colour[0];
unsafe_colour[1] += distilled[z].colour[1];
unsafe_colour[2] += distilled[z].colour[2];
hits++;
}
// and now get the data for any additional non-distilled particles
if ((Hypothesis[z] != null) && (pose != null))
{
previous_paths = pose.previous_paths;
if (previous_paths != null)
{
time_step = pose.time_step;
// cycle through the previous paths for this pose
for (p = previous_paths.Count-1; p >= 0; p--)
{
path = previous_paths[p];
if (path != null)
{
if (path.Enabled)
{
// do any hypotheses for this path exist at this location ?
map_cache_observations = path.GetHypotheses(x, y, z);
if (map_cache_observations != null)
{
for (i = map_cache_observations.Count - 1; i >= 0; i--)
{
h = map_cache_observations[i];
if (h.Enabled)
{
// only use evidence older than the current time
// step to avoid getting into a muddle
if (time_step != h.pose.time_step)
{
probabilityLogOdds += h.probabilityLogOdds;
level = h.colour[0];
level = h.colour[1];
level = h.colour[2];
unsafe_colour[0] += level;
unsafe_colour[1] += level;
unsafe_colour[2] += level;
// update mean colour and variance
if (hits > 0)
{
if (level < unsafe_min_level[0]) unsafe_min_level[0] = level;
if (level < unsafe_min_level[1]) unsafe_min_level[1] = level;
if (level < unsafe_min_level[2]) unsafe_min_level[2] = level;
if (level > unsafe_max_level[0]) unsafe_max_level[0] = level;
if (level > unsafe_max_level[1]) unsafe_max_level[1] = level;
if (level > unsafe_max_level[2]) unsafe_max_level[2] = level;
}
else
{
unsafe_min_level[0] = level;
unsafe_min_level[1] = level;
unsafe_min_level[2] = level;
unsafe_max_level[0] = level;
unsafe_max_level[1] = level;
unsafe_max_level[2] = level;
}
hits++;
}
}
}
}
}
else
{
// remove a dead path, because it has been distilled
pose.previous_paths.RemoveAt(p);
}
}
}
}
}
}
}
}
if (hits > 0)
{
// calculate mean colour variance
mean_variance = 0;
mean_variance += max_level[0] - min_level[0];
mean_variance += max_level[1] - min_level[1];
mean_variance += max_level[2] - min_level[2];
//mean_variance /= (3 * 255.0f);
mean_variance *= 0.001307189542483660130718954248366f;
// calculate the average colour
colour[0] /= hits;
colour[1] /= hits;
colour[2] /= hits;
// at the end we convert the total log odds value into a probability
if (returnLogOdds)
return (probabilityLogOdds);
else
return (probabilities.LogOddsToProbability(probabilityLogOdds));
}
else
return (NO_OCCUPANCY_EVIDENCE);
}
/// <summary>
/// insert a stereo ray into the grid
/// </summary>
/// <param name="ray">stereo ray</param>
/// <param name="origin">pose from which this observation was made</param>
/// <param name="sensormodel">sensor model for each grid level</param>
/// <param name="left_camera_location">left stereo camera position and orientation</param>
/// <param name="right_camera_location">right stereo camera position and orientation</param>
/// <param name="localiseOnly">whether we are mapping or localising</param>
/// <returns>localisation matching score</returns>
public float Insert(
evidenceRay ray,
particlePose origin,
stereoModel[] sensormodel,
pos3D left_camera_location,
pos3D right_camera_location,
bool localiseOnly)
{
float matchingScore = 0;
switch (grid_type)
{
case TYPE_MULTI_HYPOTHESIS:
{
for (int grid_level = 0; grid_level < grid.Length; grid_level++)
{
rayModelLookup sensormodel_lookup = sensormodel[grid_level].ray_model;
occupancygridMultiHypothesis grd = (occupancygridMultiHypothesis)grid[grid_level];
matchingScore += grid_weight[grid_level] * grd.Insert(ray, origin, sensormodel_lookup, left_camera_location, right_camera_location, localiseOnly);
}
break;
}
}
return (matchingScore);
}
/// <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="max_length">the maximum number of poses which may be stored within this path</param>
/// <param name="time_step">time step</param>
/// <param name="path_ID">ID number for the path</param>
/// <param name="grid_dimension_cells">occupancy grid dimension</param>
public particlePath(float x,
float y,
float pan,
int max_length,
UInt32 time_step,
UInt32 path_ID,
int grid_dimension_cells)
{
ID = path_ID;
this.max_length = max_length;
path = new List<particlePose>();
particlePose pose = new particlePose(x, y, pan, this);
pose.time_step = time_step;
Enabled = true;
Add(pose);
}
/// <summary>
/// show the path
/// </summary>
/// <param name="img">image data</param>
/// <param name="width">width of the image</param>
/// <param name="height">height of the image</param>
/// <param name="r">red</param>
/// <param name="g">green</param>
/// <param name="b">blue</param>
/// <param name="min_x_mm">minimum x coordinate of the window within which to show the path</param>
/// <param name="min_y_mm">minimum y coordinate of the window within which to show the path</param>
/// <param name="max_x_mm">maximum x coordinate of the window within which to show the path</param>
/// <param name="max_y_mm">maximum y coordinate of the window within which to show the path</param>
/// <param name="clearBackground">whether to clear the image before drawing the path</param>
/// <param name="root_time_step">time step at which to begin drawing the path</param>
public void Show(byte[] img,
int width,
int height,
int r,
int g,
int b,
int line_thickness,
float min_x_mm,
float min_y_mm,
float max_x_mm,
float max_y_mm,
bool clearBackground,
UInt32 root_time_step)
{
if (clearBackground)
{
for (int i = 0; i < width * height * 3; i++)
{
img[i] = 255;
}
}
int rr, gg, bb;
int x = -1, y = -1;
int prev_x = -1, prev_y = -1;
if ((max_x_mm > min_x_mm) && (max_y_mm > min_y_mm))
{
particlePose pose = current_pose;
if (pose != null)
{
while (pose.parent != null)
{
if ((pose.time_step <= root_time_step) &&
(root_time_step != UInt32.MaxValue))
{
rr = 0;
gg = 255;
bb = 0;
}
else
{
rr = r;
gg = g;
bb = b;
}
x = (int)((pose.x - min_x_mm) * (width - 1) / (max_x_mm - min_x_mm));
if ((x > 0) && (x < width))
{
y = height - 1 - (int)((pose.y - min_y_mm) * (height - 1) / (max_y_mm - min_y_mm));
if ((y < 0) || (y >= height))
{
y = -1;
}
}
else
{
x = -1;
}
if ((x > -1) && (y > -1))
{
if (prev_x > -1)
{
drawing.drawLine(img, width, height, x, y, prev_x, prev_y, rr, gg, bb, line_thickness, false);
}
prev_x = x;
prev_y = y;
}
// move along, nothing to see...
pose = pose.parent;
}
}
}
}
/// <summary>
/// updates the navigable space
/// </summary>
/// <param name="pose">best pose from which the occupancy value is calculated</param>
/// <param name="x">x coordinate in cells</param>
/// <param name="y">y coordinate in cells</param>
private void updateNavigableSpace(particlePose pose, int x, int y)
{
if (x < 1) x = 1;
if (y < 1) y = 1;
float[] mean_colour = new float[3];
float mean_variance = 0;
float prob = cell[x][y].GetProbability(pose, x, y,
mean_colour,
ref mean_variance);
bool state = false;
if (prob < 0.5f) state = true;
navigable_space[x][y] = state;
navigable_space[x - 1][y] = state;
navigable_space[x - 1][y - 1] = state;
navigable_space[x][y - 1] = state;
}
/// <summary>
/// remove all poses along the path until a branch point is located
/// </summary>
/// <param name="pose"></param>
/// <returns></returns>
private particlePose CollapseBranch(particlePose pose,
occupancygridMultiHypothesis grid)
{
int children = 0;
while ((pose != branch_pose) &&
(pose.path == this) &&
(children == 0))
{
if (pose != current_pose)
children = pose.no_of_children;
if (children == 0)
if (pose.path == this)
{
pose.Remove(grid);
pose = pose.parent;
}
}
return (pose);
}
/// <summary>
/// probes the grid using the given 3D line and returns the distance to the nearest occupied grid cell
/// </summary>
/// <param name="pose">current best pose estimate</param>
/// <param name="x0_mm">start x coordinate</param>
/// <param name="y0_mm">start y coordinate</param>
/// <param name="z0_mm">start z coordinate</param>
/// <param name="x1_mm">end x coordinate</param>
/// <param name="y1_mm">end y coordinate</param>
/// <param name="z1_mm">end z coordinate</param>
/// <returns>range to the nearest occupied grid cell in millimetres</returns>
public float ProbeRange(
particlePose pose,
float x0_mm,
float y0_mm,
float z0_mm,
float x1_mm,
float y1_mm,
float z1_mm)
{
float range_mm = -1;
int half_dimension_cells = dimension_cells/2;
int x0_cell = half_dimension_cells + (int)((x0_mm - x) / cellSize_mm);
int y0_cell = half_dimension_cells + (int)((y0_mm - y) / cellSize_mm);
int z0_cell = half_dimension_cells + (int)((z0_mm - z) / cellSize_mm);
int x1_cell = half_dimension_cells + (int)((x1_mm - x) / cellSize_mm);
int y1_cell = half_dimension_cells + (int)((y1_mm - y) / cellSize_mm);
int z1_cell = half_dimension_cells + (int)((z1_mm - z) / cellSize_mm);
int dx = x1_cell - x0_cell;
int dy = y1_cell - y0_cell;
int dz = z1_cell - z0_cell;
int dist_cells = (int)Math.Sqrt(dx*dx + dy*dy + dz*dz);
int x_cell, y_cell, z_cell;
float incr = 1 / (float)dist_cells;
float fraction = 0;
float[] colour = new float[3];
float mean_variance = 0;
for (int dist = 0; dist < dist_cells; dist++, fraction += incr)
{
x_cell = x0_cell + (int)(fraction * dx);
if ((x_cell > -1) && (x_cell < dimension_cells))
{
y_cell = y0_cell + (int)(fraction * dy);
if ((y_cell > -1) && (y_cell < dimension_cells))
{
z_cell = z0_cell + (int)(fraction * dz);
if ((z_cell > -1) && (z_cell < dimension_cells_vertical))
{
occupancygridCellMultiHypothesis c = cell[x_cell][y_cell];
if (c != null)
{
if (c.GetProbability(pose, x_cell, y_cell, z_cell, false, colour, ref mean_variance) > 0.5f)
{
range_mm = dist * cellSize_mm;
break;
}
}
}
else break;
}
else break;
}
else break;
}
return(range_mm);
}
/// <summary>
/// returns the average colour variance for the entire grid
/// </summary>
/// <param name="pose">best available robot pose hypothesis</param>
/// <returns>colour variance value for the occupancy grid</returns>
public float GetMeanColourVariance(particlePose pose)
{
float[] mean_colour = new float[3];
float total_variance = 0;
int hits = 0;
for (int cell_y = 0; cell_y < dimension_cells; cell_y++)
{
for (int cell_x = 0; cell_x < dimension_cells; cell_x++)
{
if (cell[cell_x][cell_y] != null)
{
float mean_variance = 0;
float prob = cell[cell_x][cell_y].GetProbability(pose, cell_x, cell_y,
mean_colour,
ref mean_variance);
if (prob > 0.5)
{
total_variance += mean_variance;
hits++;
}
}
}
}
if (hits > 0) total_variance /= hits;
return (total_variance);
}
/// <summary>
/// export the occupancy grid data to IFrIT basic particle file format for visualisation
/// </summary>
/// <param name="filename">name of the file to save as</param>
/// <param name="pose">best available pose</param>
/// <param name="centre_x">centre of the tile in grid cells</param>
/// <param name="centre_y">centre of the tile in grid cells</param>
/// <param name="width_cells">width of the tile in grid cells</param>
public void ExportToIFrIT(string filename,
particlePose pose,
int centre_x, int centre_y,
int width_cells)
{
float threshold = 0.5f;
int half_width_cells = width_cells / 2;
int tx = 99999;
int ty = 99999;
int tz = 99999;
int bx = -99999;
int by = -99999;
int bz = -99999;
// dummy variables needed by GetProbability
float[] colour = new float[3];
// another dummy variable needed by GetProbability but otherwise not used
float mean_variance = 0;
// get the bounding region within which there are actice grid cells
int occupied_cells = 0;
for (int x = centre_x - half_width_cells; x <= centre_x + half_width_cells; x++)
{
if ((x >= 0) && (x < dimension_cells))
{
for (int y = centre_y - half_width_cells; y <= centre_y + half_width_cells; y++)
{
if ((y >= 0) && (y < dimension_cells))
{
if (cell[x][y] != null)
{
for (int z = 0; z < dimension_cells_vertical; z++)
{
float prob = cell[x][y].GetProbability(pose, x, y,
colour, ref mean_variance);
if (prob != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
{
occupied_cells++;
if (x < tx) tx = x;
if (y < ty) ty = y;
if (z < tz) tz = z;
if (x > bx) bx = x;
if (y > by) by = y;
if (z > bz) bz = z;
}
}
}
}
}
}
}
bx++;
by++;
if (occupied_cells > 0)
{
// add bounding box information
string bounding_box = Convert.ToString(0) + " " +
Convert.ToString(0) + " " +
Convert.ToString(0) + " " +
Convert.ToString(dimension_cells) + " " +
Convert.ToString(dimension_cells) + " " +
Convert.ToString(dimension_cells_vertical) + " X Y Z";
ArrayList particles = new ArrayList();
for (int y = ty; y < by; y++)
{
for (int x = tx; x < bx; x++)
{
if (cell[x][y] != null)
{
float prob = cell[x][y].GetProbability(pose, x, y,
colour, ref mean_variance);
if (prob != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
{
for (int z = 0; z < dimension_cells_vertical; z++)
{
prob = cell[x][y].GetProbability(pose, x, y, z, false,
colour, ref mean_variance);
if (prob != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
{
if (prob > threshold) // probably occupied space
{
// get the colour of the grid cell as a floating point value
float colour_value = int.Parse(colours.GetHexFromRGB((int)colour[0], (int)colour[1], (int)colour[2]), NumberStyles.HexNumber);
string particleStr = Convert.ToString(x) + " " +
Convert.ToString(y) + " " +
Convert.ToString(dimension_cells_vertical-1-z) + " " +
Convert.ToString(prob) + " " +
Convert.ToString(colour_value);
particles.Add(particleStr);
}
}
}
}
}
}
}
// write the text file
if (particles.Count > 0)
{
StreamWriter oWrite = null;
bool allowWrite = true;
try
{
oWrite = File.CreateText(filename);
}
catch
{
allowWrite = false;
}
if (allowWrite)
{
oWrite.WriteLine(Convert.ToString(particles.Count));
oWrite.WriteLine(bounding_box);
for (int p = 0; p < particles.Count; p++)
oWrite.WriteLine((string)particles[p]);
oWrite.Close();
}
}
}
}
/// <summary>
/// returns the probability of occupancy at this grid cell at the given vertical (z) coordinate
/// warning: this could potentially suffer from path ID or time step rollover problems
/// </summary>
/// <param name="pose">the robot pose from which this cell was observed</param>
/// <param name="x">x grid coordinate</param>
/// <param name="y">y grid coordinate</param>
/// <param name="z">z grid coordinate</param>
/// <param name="returnLogOdds">return the probability value expressed as log odds</param>
/// <param name="colour">average colour of this grid cell</param>
/// <param name="mean_variance">average colour variance of this grid cell</param>
/// <returns>probability value</returns>
public unsafe float GetProbability(
particlePose pose,
int x,
int y,
int z,
bool returnLogOdds,
float[] colour,
ref float mean_variance)
{
int col, p, i, hits = 0;
float probabilityLogOdds = 0;
byte level;
List <particleGridCell> map_cache_observations;
List <particlePath> previous_paths;
particleGridCell h;
particlePath path;
uint time_step;
mean_variance = 1;
// pin some arrays to avoid range checking
fixed(float *unsafe_colour = colour)
{
fixed(float *unsafe_min_level = min_level)
{
fixed(float *unsafe_max_level = max_level)
{
unsafe_colour[0] = 0;
unsafe_colour[1] = 0;
unsafe_colour[2] = 0;
// first retrieve any distilled occupancy value
if (distilled != null)
{
if (distilled[z] != null)
{
probabilityLogOdds += distilled[z].probabilityLogOdds;
unsafe_colour[0] += distilled[z].colour[0];
unsafe_colour[1] += distilled[z].colour[1];
unsafe_colour[2] += distilled[z].colour[2];
hits++;
}
}
// and now get the data for any additional non-distilled particles
if ((Hypothesis[z] != null) && (pose != null))
{
previous_paths = pose.previous_paths;
if (previous_paths != null)
{
time_step = pose.time_step;
// cycle through the previous paths for this pose
for (p = previous_paths.Count - 1; p >= 0; p--)
{
path = previous_paths[p];
if (path != null)
{
if (path.Enabled)
{
// do any hypotheses for this path exist at this location ?
map_cache_observations = path.GetHypotheses(x, y, z);
if (map_cache_observations != null)
{
for (i = map_cache_observations.Count - 1; i >= 0; i--)
{
h = map_cache_observations[i];
if (h.Enabled)
{
// only use evidence older than the current time
// step to avoid getting into a muddle
if (time_step != h.pose.time_step)
{
probabilityLogOdds += h.probabilityLogOdds;
level = h.colour[0];
level = h.colour[1];
level = h.colour[2];
unsafe_colour[0] += level;
unsafe_colour[1] += level;
unsafe_colour[2] += level;
// update mean colour and variance
if (hits > 0)
{
if (level < unsafe_min_level[0])
{
unsafe_min_level[0] = level;
}
if (level < unsafe_min_level[1])
{
unsafe_min_level[1] = level;
}
if (level < unsafe_min_level[2])
{
unsafe_min_level[2] = level;
}
if (level > unsafe_max_level[0])
{
unsafe_max_level[0] = level;
}
if (level > unsafe_max_level[1])
{
unsafe_max_level[1] = level;
}
if (level > unsafe_max_level[2])
{
unsafe_max_level[2] = level;
}
}
else
{
unsafe_min_level[0] = level;
unsafe_min_level[1] = level;
unsafe_min_level[2] = level;
unsafe_max_level[0] = level;
unsafe_max_level[1] = level;
unsafe_max_level[2] = level;
}
hits++;
}
}
}
}
}
else
{
// remove a dead path, because it has been distilled
pose.previous_paths.RemoveAt(p);
}
}
}
}
}
}
}
}
if (hits > 0)
{
// calculate mean colour variance
mean_variance = 0;
mean_variance += max_level[0] - min_level[0];
mean_variance += max_level[1] - min_level[1];
mean_variance += max_level[2] - min_level[2];
//mean_variance /= (3 * 255.0f);
mean_variance *= 0.001307189542483660130718954248366f;
// calculate the average colour
colour[0] /= hits;
colour[1] /= hits;
colour[2] /= hits;
// at the end we convert the total log odds value into a probability
if (returnLogOdds)
{
return(probabilityLogOdds);
}
else
{
return(probabilities.LogOddsToProbability(probabilityLogOdds));
}
}
else
{
return(NO_OCCUPANCY_EVIDENCE);
}
}
/// <summary>
/// save the entire grid to a single file
/// </summary>
/// <param name="binfile"></param>
/// <param name="pose"></param>
public void Save(BinaryWriter binfile,
particlePose pose)
{
SaveTile(binfile, pose, dimension_cells / 2, dimension_cells / 2, 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_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>
/// returns the localisation probability
/// </summary>
/// <param name="x_cell">x grid coordinate</param>
/// <param name="y_cell">y grid coordinate</param>
/// <param name="origin">pose of the robot</param>
/// <param name="sensormodel_probability">probability value from a specific point in the ray, taken from the sensor model</param>
/// <param name="colour">colour of the localisation ray</param>
/// <returns>log odds probability of there being a match between the ray and the grid</returns>
private float matchingProbability(
int x_cell, int y_cell, int z_cell,
particlePose origin,
float sensormodel_probability,
byte[] colour)
{
float prob_log_odds = occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE;
float colour_variance = 0;
// localise using this grid cell
// first get the existing probability value at this cell
float[] existing_colour = new float[3];
float existing_probability =
cell[x_cell][y_cell].GetProbability(origin, x_cell, y_cell, z_cell,
false, existing_colour, ref colour_variance);
if (existing_probability != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
{
// combine the occupancy probabilities
float occupancy_probability =
((sensormodel_probability * existing_probability) +
((1.0f - sensormodel_probability) * (1.0f - existing_probability)));
// get the colour difference between the map and the observation
float colour_difference = getColourDifference(colour, existing_colour);
// turn the colour difference into a probability
float colour_probability = 1.0f - colour_difference;
// localisation matching probability, expressed as log odds
prob_log_odds = LogOdds[(int)(occupancy_probability * colour_probability * LOG_ODDS_LOOKUP_LEVELS)];
}
return (prob_log_odds);
}
/// <summary>
/// parse an xml node to extract camera calibration parameters
/// </summary>
/// <param name="xnod"></param>
/// <param name="level"></param>
public void LoadFromXml(XmlNode xnod,
int level)
{
XmlNode xnodWorking;
if (xnod.Name == "Pose")
{
IFormatProvider format = new System.Globalization.CultureInfo("en-GB");
string[] poseStr = xnod.InnerText.Split(',');
particlePose new_pose = new particlePose(Convert.ToSingle(poseStr[0], format),
Convert.ToSingle(poseStr[1], format),
Convert.ToSingle(poseStr[2], format)*(float)Math.PI/180.0f, null);
Add(new_pose);
}
// call recursively on all children of the current node
if ((xnod.HasChildNodes) &&
((xnod.Name == "RobotPath") ||
(xnod.Name == "Sentience")
))
{
xnodWorking = xnod.FirstChild;
while (xnodWorking != null)
{
LoadFromXml(xnodWorking, level + 1);
xnodWorking = xnodWorking.NextSibling;
}
}
}
/// <summary>
/// inserts the given ray into the grid
/// There are three components to the sensor model used here:
/// two areas determining the probably vacant area and one for
/// the probably occupied space
/// </summary>
/// <param name="ray">ray object to be inserted into the grid</param>
/// <param name="origin">the pose of the robot</param>
/// <param name="sensormodel">the sensor model to be used</param>
/// <param name="leftcam_x">x position of the left camera in millimetres</param>
/// <param name="leftcam_y">y position of the left camera in millimetres</param>
/// <param name="rightcam_x">x position of the right camera in millimetres</param>
/// <param name="rightcam_y">y position of the right camera in millimetres</param>
/// <param name="localiseOnly">if true does not add any mapping particles (pure localisation)</param>
/// <returns>matching probability, expressed as log odds</returns>
public float Insert(evidenceRay ray,
particlePose origin,
rayModelLookup sensormodel_lookup,
pos3D left_camera_location,
pos3D right_camera_location,
bool localiseOnly)
{
// some constants to aid readability
const int OCCUPIED_SENSORMODEL = 0;
const int VACANT_SENSORMODEL_LEFT_CAMERA = 1;
const int VACANT_SENSORMODEL_RIGHT_CAMERA = 2;
const int X_AXIS = 0;
const int Y_AXIS = 1;
// which disparity index in the lookup table to use
// we multiply by 2 because the lookup is in half pixel steps
int sensormodel_index = (int)Math.Round(ray.disparity * 2);
// the initial models are blank, so just default to the one disparity pixel model
bool small_disparity_value = false;
if (sensormodel_index < 2)
{
sensormodel_index = 2;
small_disparity_value = true;
}
// beyond a certain disparity the ray model for occupied space
// is always only going to be only a single grid cell
if (sensormodel_index >= sensormodel_lookup.probability.GetLength(0))
sensormodel_index = sensormodel_lookup.probability.GetLength(0) - 1;
float xdist_mm=0, ydist_mm=0, zdist_mm=0, xx_mm=0, yy_mm=0, zz_mm=0;
float occupied_dx = 0, occupied_dy = 0, occupied_dz = 0;
float intersect_x = 0, intersect_y = 0, intersect_z = 0;
float centre_prob = 0, prob = 0, prob_localisation = 0; // probability values at the centre axis and outside
float matchingScore = occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE; // total localisation matching score
int rayWidth = 0; // widest point in the ray in cells
int widest_point; // widest point index
int step_size = 1;
particleGridCell hypothesis;
// ray width at the fattest point in cells
rayWidth = (int)Math.Round(ray.width / (cellSize_mm * 2));
// calculate the centre position of the grid in millimetres
int half_grid_width_mm = dimension_cells * cellSize_mm / 2;
//int half_grid_width_vertical_mm = dimension_cells_vertical * cellSize_mm / 2;
int grid_centre_x_mm = (int)(x - half_grid_width_mm);
int grid_centre_y_mm = (int)(y - half_grid_width_mm);
int grid_centre_z_mm = (int)z;
int max_dimension_cells = dimension_cells - rayWidth;
// in turbo mode only use a single vacancy ray
int max_modelcomponent = VACANT_SENSORMODEL_RIGHT_CAMERA;
if (TurboMode) max_modelcomponent = VACANT_SENSORMODEL_LEFT_CAMERA;
float[][] sensormodel_lookup_probability = sensormodel_lookup.probability;
// consider each of the three parts of the sensor model
for (int modelcomponent = OCCUPIED_SENSORMODEL; modelcomponent <= max_modelcomponent; modelcomponent++)
{
// the range from the cameras from which insertion of data begins
// for vacancy rays this will be zero, but will be non-zero for the occupancy area
int starting_range_cells = 0;
switch (modelcomponent)
{
case OCCUPIED_SENSORMODEL:
{
// distance between the beginning and end of the probably
// occupied area
occupied_dx = ray.vertices[1].x - ray.vertices[0].x;
occupied_dy = ray.vertices[1].y - ray.vertices[0].y;
occupied_dz = ray.vertices[1].z - ray.vertices[0].z;
intersect_x = ray.vertices[0].x + (occupied_dx * ray.fattestPoint);
intersect_y = ray.vertices[0].y + (occupied_dy * ray.fattestPoint);
intersect_z = ray.vertices[0].z + (occupied_dz * ray.fattestPoint);
xdist_mm = occupied_dx;
ydist_mm = occupied_dy;
zdist_mm = occupied_dz;
// begin insertion at the beginning of the
// probably occupied area
xx_mm = ray.vertices[0].x;
yy_mm = ray.vertices[0].y;
zz_mm = ray.vertices[0].z;
break;
}
case VACANT_SENSORMODEL_LEFT_CAMERA:
{
// distance between the left camera and the left side of
// the probably occupied area of the sensor model
xdist_mm = intersect_x - left_camera_location.x;
ydist_mm = intersect_y - left_camera_location.y;
zdist_mm = intersect_z - left_camera_location.z;
// begin insertion from the left camera position
xx_mm = left_camera_location.x;
yy_mm = left_camera_location.y;
zz_mm = left_camera_location.z;
step_size = 2;
break;
}
case VACANT_SENSORMODEL_RIGHT_CAMERA:
{
// distance between the right camera and the right side of
// the probably occupied area of the sensor model
xdist_mm = intersect_x - right_camera_location.x;
ydist_mm = intersect_y - right_camera_location.y;
zdist_mm = intersect_z - right_camera_location.z;
// begin insertion from the right camera position
xx_mm = right_camera_location.x;
yy_mm = right_camera_location.y;
zz_mm = right_camera_location.z;
step_size = 2;
break;
}
}
// which is the longest axis ?
int longest_axis = X_AXIS;
float longest = Math.Abs(xdist_mm);
if (Math.Abs(ydist_mm) > longest)
{
// y has the longest length
longest = Math.Abs(ydist_mm);
longest_axis = Y_AXIS;
}
// ensure that the vacancy model does not overlap
// the probably occupied area
// This is crude and could potentially leave a gap
if (modelcomponent != OCCUPIED_SENSORMODEL)
longest -= ray.width;
int steps = (int)(longest / cellSize_mm);
if (steps < 1) steps = 1;
// calculate the range from the cameras to the start of the ray in grid cells
if (modelcomponent == OCCUPIED_SENSORMODEL)
{
if (longest_axis == Y_AXIS)
starting_range_cells = (int)Math.Abs((ray.vertices[0].y - ray.observedFrom.y) / cellSize_mm);
else
starting_range_cells = (int)Math.Abs((ray.vertices[0].x - ray.observedFrom.x) / cellSize_mm);
}
// what is the widest point of the ray in cells
if (modelcomponent == OCCUPIED_SENSORMODEL)
widest_point = (int)(ray.fattestPoint * steps / ray.length);
else
widest_point = steps;
// calculate increment values in millimetres
float x_incr_mm = xdist_mm / steps;
float y_incr_mm = ydist_mm / steps;
float z_incr_mm = zdist_mm / steps;
// step through the ray, one grid cell at a time
int grid_step = 0;
while (grid_step < steps)
{
// is this position inside the maximum mapping range
bool withinMappingRange = true;
if (grid_step + starting_range_cells > max_mapping_range_cells)
{
withinMappingRange = false;
if ((grid_step==0) && (modelcomponent == OCCUPIED_SENSORMODEL))
{
grid_step = steps;
modelcomponent = 9999;
}
}
// calculate the width of the ray in cells at this point
// using a diamond shape ray model
int ray_wdth = 0;
if (rayWidth > 0)
{
if (grid_step < widest_point)
ray_wdth = grid_step * rayWidth / widest_point;
else
{
if (!small_disparity_value)
// most disparity values tail off to some point in the distance
ray_wdth = (steps - grid_step + widest_point) * rayWidth / (steps - widest_point);
else
// for very small disparity values the ray model has an infinite tail
// and maintains its width after the widest point
ray_wdth = rayWidth;
}
}
// localisation rays are wider, to enable a more effective matching score
// which is not too narrowly focussed and brittle
int ray_wdth_localisation = ray_wdth + 1; //localisation_search_cells;
xx_mm += x_incr_mm*step_size;
yy_mm += y_incr_mm*step_size;
zz_mm += z_incr_mm*step_size;
// convert the x millimetre position into a grid cell position
int x_cell = (int)Math.Round((xx_mm - grid_centre_x_mm) / (float)cellSize_mm);
if ((x_cell > ray_wdth_localisation) && (x_cell < dimension_cells - ray_wdth_localisation))
{
// convert the y millimetre position into a grid cell position
int y_cell = (int)Math.Round((yy_mm - grid_centre_y_mm) / (float)cellSize_mm);
if ((y_cell > ray_wdth_localisation) && (y_cell < dimension_cells - ray_wdth_localisation))
{
// convert the z millimetre position into a grid cell position
int z_cell = (int)Math.Round((zz_mm - grid_centre_z_mm) / (float)cellSize_mm);
if ((z_cell >= 0) && (z_cell < dimension_cells_vertical))
{
int x_cell2 = x_cell;
int y_cell2 = y_cell;
// get the probability at this point
// for the central axis of the ray using the inverse sensor model
if (modelcomponent == OCCUPIED_SENSORMODEL)
centre_prob = 0.5f + (sensormodel_lookup_probability[sensormodel_index][grid_step] * 0.5f);
else
// calculate the probability from the vacancy model
centre_prob = vacancyFunction(grid_step / (float)steps, steps);
// width of the localisation ray
for (int width = -ray_wdth_localisation; width <= ray_wdth_localisation; width++)
{
// is the width currently inside the mapping area of the ray ?
bool isInsideMappingRayWidth = false;
if ((width >= -ray_wdth) && (width <= ray_wdth))
isInsideMappingRayWidth = true;
// adjust the x or y cell position depending upon the
// deviation from the main axis of the ray
if (longest_axis == Y_AXIS)
x_cell2 = x_cell + width;
else
y_cell2 = y_cell + width;
// probability at the central axis
prob = centre_prob;
prob_localisation = centre_prob;
// probabilities are symmetrical about the axis of the ray
// this multiplier implements a gaussian distribution around the centre
if (width != 0) // don't bother if this is the centre of the ray
{
// the probability used for wide localisation rays
prob_localisation *= gaussianLookup[Math.Abs(width) * 9 / ray_wdth_localisation];
// the probability used for narrower mapping rays
if (isInsideMappingRayWidth)
prob *= gaussianLookup[Math.Abs(width) * 9 / ray_wdth];
}
if ((cell[x_cell2][y_cell2] != null) && (withinMappingRange))
{
// only localise using occupancy, not vacancy
if (modelcomponent == OCCUPIED_SENSORMODEL)
{
// update the matching score, by combining the probability
// of the grid cell with the probability from the localisation ray
float score = occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE;
if (longest_axis == X_AXIS)
score = matchingProbability(x_cell2, y_cell2, z_cell, origin, prob_localisation, ray.colour);
if (longest_axis == Y_AXIS)
score = matchingProbability(x_cell2, y_cell2, z_cell, origin, prob_localisation, ray.colour);
if (score != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
{
if (matchingScore != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
matchingScore += score;
else
matchingScore = score;
}
}
}
if ((isInsideMappingRayWidth) &&
(withinMappingRange) &&
(!localiseOnly))
{
// add a new hypothesis to this grid coordinate
// note that this is also added to the original pose
hypothesis = new particleGridCell(x_cell2, y_cell2, z_cell,
prob, origin,
ray.colour);
if (origin.AddHypothesis(hypothesis, max_mapping_range_cells, dimension_cells, dimension_cells_vertical))
{
// generate a grid cell if necessary
if (cell[x_cell2][y_cell2] == null)
cell[x_cell2][y_cell2] = new occupancygridCellMultiHypothesis(dimension_cells_vertical);
cell[x_cell2][y_cell2].AddHypothesis(hypothesis);
total_valid_hypotheses++;
}
}
}
}
else grid_step = steps; // its the end of the ray, break out of the loop
}
else grid_step = steps; // its the end of the ray, break out of the loop
}
else grid_step = steps; // time to bail out chaps!
grid_step += step_size;
}
}
return (matchingScore);
}
/// <summary>
/// add a new pose to the path
/// </summary>
/// <param name="pose">pose to be added</param>
public void Add(particlePose pose)
{
pose.path = this;
// set the parent of this pose
pose.parent = current_pose;
// update the list of previous path IDs
if (branch_pose != null)
{
if (current_pose == branch_pose)
{
pose.previous_paths = new List<particlePath>();
int min = branch_pose.previous_paths.Count - MAX_PATH_HISTORY;
if (min < 0) min = 0;
for (int i = min; i < branch_pose.previous_paths.Count; i++)
pose.previous_paths.Add(branch_pose.previous_paths[i]);
pose.previous_paths.Add(this);
}
else pose.previous_paths = pose.parent.previous_paths;
}
else
{
if (pose.parent == null)
{
pose.previous_paths = new List<particlePath>();
pose.previous_paths.Add(this);
}
else pose.previous_paths = pose.parent.previous_paths;
}
// set the current pose
current_pose = pose;
// add the pose to the path
path.Add(pose);
//total_score += pose.score;
total_poses++;
// ensure that the path does not exceed a maximum length
if (path.Count > max_length)
{
// remove the oldest pose in the path
path.RemoveAt(0);
}
}
/// <summary>
/// display the occupancy grid
/// </summary>
/// <param name="view_type">the angle from which to view the grid</param>
/// <param name="img">image within which to insert data</param>
/// <param name="width">width of the image</param>
/// <param name="height">height of the image</param>
/// <param name="pose">the best available robot pose hypothesis</param>
/// <param name="colour">show grid cell colour, or just occupancy values</param>
/// <param name="scalegrid">show a grid overlay which gives some idea of scale</param>
public void Show(int view_type,
byte[] img,
int width,
int height,
particlePose pose,
bool colour,
bool scalegrid)
{
switch(view_type)
{
case VIEW_ABOVE:
{
if (!colour)
Show(img, width, height, pose);
else
ShowColour(img, width, height, pose);
if (scalegrid)
{
int r = 200;
int g = 200;
int b = 255;
// draw a grid to give an indication of scale
// where the grid resolution is one metre
float dimension_mm = cellSize_mm * dimension_cells;
for (float x = 0; x < dimension_mm; x += 1000)
{
int xx = (int)(x * width / dimension_mm);
drawing.drawLine(img, width, height, xx, 0, xx, height - 1, r, g, b, 0, false);
}
for (float y = 0; y < dimension_mm; y += 1000)
{
int yy = (int)(y * height / dimension_mm);
drawing.drawLine(img, width, height, 0, yy, width - 1, yy, r, g, b, 0, false);
}
}
break;
}
case VIEW_LEFT_SIDE:
{
ShowSide(img, width, height, pose, true);
break;
}
case VIEW_RIGHT_SIDE:
{
ShowSide(img, width, height, pose, false);
break;
}
case VIEW_NAVIGABLE_SPACE:
{
ShowNavigable(img, width, height);
break;
}
}
}
/// <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>
/// show an occupancy grid from one side
/// </summary>
/// <param name="img">image in which to insert the data</param>
/// <param name="width">width of the image</param>
/// <param name="height">height of the image</param>
/// <param name="pose">best available robot pose hypothesis</param>
/// <param name="leftSide">view from the left side or the right</param>
private void ShowSide(byte[] img,
int width,
int height,
particlePose pose,
bool leftSide)
{
float[] mean_colour = new float[3];
// clear the image
for (int i = 0; i < width * height * 3; i++)
img[i] = (byte)255;
// show the left or right half of the grid
int start_x = 0;
int end_x = dimension_cells / 2;
if (!leftSide)
{
start_x = dimension_cells / 2;
end_x = dimension_cells;
}
for (int cell_x = start_x; cell_x < end_x; cell_x++)
{
for (int cell_y = 0; cell_y < dimension_cells; cell_y++)
{
if (cell[cell_x][cell_y] != null)
{
// what's the probability of there being a vertical structure here?
float mean_variance = 0;
float prob = cell[cell_x][cell_y].GetProbability(pose, cell_x, cell_y,
mean_colour, ref mean_variance);
if (prob != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
{
if (prob > 0.45f)
{
// show this vertical grid section
occupancygridCellMultiHypothesis c = cell[cell_x][cell_y];
for (int cell_z = 0; cell_z < dimension_cells_vertical; cell_z++)
{
mean_variance = 0;
prob = c.GetProbability(pose, cell_x, cell_y, cell_z, false, mean_colour, ref mean_variance);
if (prob != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
{
if ((prob > 0.0f) && (mean_variance < 0.5f))
{
int multiplier = 2;
int x = cell_y * width / dimension_cells;
int y = (cell_z * height * multiplier / dimension_cells_vertical);
if ((y >= 0) && (y < height - multiplier-2))
{
for (int yy = y; yy <= y + multiplier+2; yy++)
{
int n = ((yy * width) + x) * 3;
for (int col = 0; col < 3; col++)
img[n + 2 - col] = (byte)mean_colour[col];
}
}
}
}
}
}
}
}
}
}
}
/// <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;
}
}
}
}
/// <summary>
/// show the grid map as an image
/// </summary>
/// <param name="img">bitmap image</param>
/// <param name="width">width in pixels</param>
/// <param name="height">height in pixels</param>
/// <param name="pose">best pose hypothesis from which the map is observed</param>
private void Show(byte[] img,
int width,
int height,
particlePose pose)
{
float[] mean_colour = new float[3];
for (int y = 0; y < height; y++)
{
// get the y cell coordinate within the grid
int cell_y = y * (dimension_cells-1) / height;
for (int x = 0; x < width; x++)
{
// get the x cell coordinate within the grid
int cell_x = x * (dimension_cells - 1) / width;
int n = ((y * width) + x) * 3;
if (cell[cell_x][cell_y] == null)
{
// terra incognita
for (int c = 0; c < 3; c++)
img[n + c] = (byte)255;
}
else
{
// get the probability for this vertical column
float mean_variance = 0;
float prob = cell[cell_x][cell_y].GetProbability(pose, cell_x, cell_y,
mean_colour, ref mean_variance);
if (prob != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
{
for (int c = 0; c < 3; c++)
if (prob > 0.5f)
{
if (prob > 0.7f)
img[n + c] = (byte)0; // occupied
else
img[n + c] = (byte)100; // occupied
}
else
{
if (prob < 0.3f)
img[n + c] = (byte)230; // vacant
else
img[n + c] = (byte)200; // vacant
}
}
else
{
for (int c = 0; c < 3; c++)
img[n + c] = (byte)255; // terra incognita
}
}
}
}
}
/// <summary>
/// return the probability of occupancy for the entire cell
/// </summary>
/// <param name="pose">pose from which the observation was made</param>
/// <returns>probability as log odds</returns>
public float GetProbability(
particlePose pose,
int x, int y,
float[] mean_colour,
ref float mean_variance)
{
float probabilityLogOdds = 0;
int hits = 0;
mean_variance = 0;
mean_colour[0] = 0;
mean_colour[1] = 0;
mean_colour[2] = 0;
for (int z = Hypothesis.Length - 1; z >= 0; z--)
{
float variance = 0;
float probLO = GetProbability(pose, x, y, z, true, temp_colour, ref variance);
if (probLO != NO_OCCUPANCY_EVIDENCE)
{
probabilityLogOdds += probLO;
mean_colour[0] += temp_colour[0];
mean_colour[1] += temp_colour[1];
mean_colour[2] += temp_colour[2];
mean_variance += variance;
hits++;
}
}
if (hits > 0)
{
mean_variance /= hits;
mean_colour[0] /= hits;
mean_colour[1] /= hits;
mean_colour[2] /= hits;
}
return (probabilities.LogOddsToProbability(probabilityLogOdds));
}
/// <summary>
/// show a colour occupancy grid
/// </summary>
/// <param name="img">image in which to insert the data</param>
/// <param name="width">width of the image</param>
/// <param name="height">height of the image</param>
/// <param name="pose">best available robot pose hypothesis</param>
private void ShowColour(byte[] img,
int width,
int height,
particlePose pose)
{
float[] mean_colour = new float[3];
for (int y = 0; y < height; y++)
{
int cell_y = y * (dimension_cells - 1) / height;
for (int x = 0; x < width; x++)
{
int cell_x = x * (dimension_cells - 1) / width;
int n = ((y * width) + x) * 3;
if (cell[cell_x][cell_y] == null)
{
for (int c = 0; c < 3; c++)
img[n + c] = (byte)255; // terra incognita
}
else
{
float mean_variance = 0;
float prob = cell[cell_x][cell_y].GetProbability(pose, cell_x, cell_y,
mean_colour, ref mean_variance);
if (prob != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
{
if ((mean_variance < 0.7f) || (prob < 0.5f))
{
for (int c = 0; c < 3; c++)
if (prob > 0.6f)
{
img[n + 2 - c] = (byte)mean_colour[c]; // occupied
}
else
{
if (prob < 0.3f)
img[n + c] = (byte)200;
else
img[n + c] = (byte)255;
}
}
else
{
for (int c = 0; c < 3; c++)
img[n + c] = (byte)255;
}
}
else
{
for (int c = 0; c < 3; c++)
img[n + c] = (byte)255; // terra incognita
}
}
}
}
}
/// <summary>
/// save the occupancy grid data to file as a tile
/// it is expected that multiple tiles will be saved per grid
/// This returns a byte array, which may subsequently be
/// compressed as a zip file for extra storage efficiency
/// </summary>
/// <param name="pose">best available pose</param>
/// <param name="centre_x">centre of the tile in grid cells</param>
/// <param name="centre_y">centre of the tile in grid cells</param>
/// <param name="width_cells">width of the tile in grid cells</param>
/// <returns>byte array containing the data</returns>
public byte[] SaveTile(particlePose pose,
int centre_x,
int centre_y,
int width_cells)
{
ArrayList data = new ArrayList();
int half_width_cells = width_cells / 2;
int tx = centre_x + half_width_cells;
int ty = centre_y + half_width_cells;
int bx = centre_x - half_width_cells;
int by = centre_y - half_width_cells;
// get the bounding region within which there are actice grid cells
for (int x = centre_x - half_width_cells; x <= centre_x + half_width_cells; x++)
{
if ((x >= 0) && (x < dimension_cells))
{
for (int y = centre_y - half_width_cells; y <= centre_y + half_width_cells; y++)
{
if ((y >= 0) && (y < dimension_cells))
{
if (cell[x][y] != null)
{
if (x < tx) tx = x;
if (y < ty) ty = y;
if (x > bx) bx = x;
if (y > by) by = y;
}
}
}
}
}
bx++;
by++;
// write the bounding box dimensions to file
byte[] intbytes = BitConverter.GetBytes(tx);
for (int i = 0; i < intbytes.Length; i++)
data.Add(intbytes[i]);
intbytes = BitConverter.GetBytes(ty);
for (int i = 0; i < intbytes.Length; i++)
data.Add(intbytes[i]);
intbytes = BitConverter.GetBytes(bx);
for (int i = 0; i < intbytes.Length; i++)
data.Add(intbytes[i]);
intbytes = BitConverter.GetBytes(by);
for (int i = 0; i < intbytes.Length; i++)
data.Add(intbytes[i]);
// create a binary index
int w1 = bx - tx;
int w2 = by - ty;
bool[] binary_index = new bool[w1 * w2];
int n = 0;
int occupied_cells = 0;
for (int y = ty; y < by; y++)
{
for (int x = tx; x < bx; x++)
{
if (cell[x][y] != null)
{
occupied_cells++;
binary_index[n] = true;
}
n++;
}
}
// convert the binary index to a byte array for later storage
byte[] indexbytes = ArrayConversions.ToByteArray(binary_index);
for (int i = 0; i < indexbytes.Length; i++)
data.Add(indexbytes[i]);
// dummy variables needed by GetProbability
float[] colour = new float[3];
if (occupied_cells > 0)
{
float[] occupancy = new float[occupied_cells * dimension_cells_vertical];
byte[] colourData = new byte[occupied_cells * dimension_cells_vertical * 3];
float mean_variance = 0;
n = 0;
for (int y = ty; y < by; y++)
{
for (int x = tx; x < bx; x++)
{
if (cell[x][y] != null)
{
for (int z = 0; z < dimension_cells_vertical; z++)
{
float prob = cell[x][y].GetProbability(pose,x, y, z,
true, colour, ref mean_variance);
int index = (n * dimension_cells_vertical) + z;
occupancy[index] = prob;
if (prob != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
for (int col = 0; col < 3; col++)
colourData[(index * 3) + col] = (byte)colour[col];
}
n++;
}
}
}
// store the occupancy and colour data as byte arrays
byte[] occupancybytes = ArrayConversions.ToByteArray(occupancy);
for (int i = 0; i < occupancybytes.Length; i++)
data.Add(occupancybytes[i]);
for (int i = 0; i < colourData.Length; i++)
data.Add(colourData[i]);
}
byte[] result = (byte[])data.ToArray(typeof(byte));
return(result);
}
/// <summary>
/// save the entire grid as a byte array, suitable for subsequent compression
/// </summary>
/// <param name="pose"></param>
/// <returns></returns>
public byte[] Save(particlePose pose)
{
return(SaveTile(pose, dimension_cells / 2, dimension_cells / 2, dimension_cells));
}
/// <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>
/// export the occupancy grid data to IFrIT basic particle file format for visualisation
/// </summary>
/// <param name="filename">name of the file to save as</param>
/// <param name="pose">best available pose</param>
public void ExportToIFrIT(string filename,
particlePose pose)
{
ExportToIFrIT(filename, pose, dimension_cells / 2, dimension_cells / 2, dimension_cells);
}
