/// <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 show_uncertainty_ellipse,
bool show_path_tree,
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, show_uncertainty_ellipse, show_path_tree, 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,
dpslam 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>
/// 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>
/// remove the mapping particles associated with this path
/// </summary>
public bool Remove(dpslam 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>
/// 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.z, format) + "," +
Convert.ToString(pose.pan * 180 / Math.PI, format) + "," +
Convert.ToString(pose.tilt * 180 / Math.PI, format) + "," +
Convert.ToString(pose.roll * 180 / Math.PI, format));
}
return(nodePath);
}
/// <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>
/// 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="show_uncertainty_ellipse">whether to show the uncertainty ellipse</param>
/// <param name="show_path_tree">whether to show the path tree</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 show_uncertainty_ellipse,
bool show_path_tree,
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;
}
}
}
}
}
if (show_uncertainty_ellipse)
{
ShowUncertaintyEllipse(
img, width, height,
min_x_mm, min_y_mm,
max_x_mm, max_y_mm,
255, 0, 0);
}
// show the path
if (show_path_tree)
{
ShowPath(img, width, height, 0, 255, 0, 0, min_x_mm, min_y_mm, max_x_mm, max_y_mm, false);
}
}
/// <summary>
/// distill all grid particles within this path
/// </summary>
/// <param name="grid"></param>
public void Distill(dpslam 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>
/// 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_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>
/// 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 z,
float pan,
float tilt,
float roll,
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, z, pan, tilt, roll, this);
pose.time_step = time_step;
Enabled = true;
Add(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>
/// 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>
/// 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>
/// 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),
Convert.ToSingle(poseStr[3], format) * (float)Math.PI / 180.0f,
Convert.ToSingle(poseStr[4], format) * (float)Math.PI / 180.0f,
Convert.ToSingle(poseStr[5], 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>
/// 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>
/// 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
//Console.WriteLine("occupancy_probability = " + occupancy_probability.ToString());
if (occupancy_probability > 1) occupancy_probability = 1;
prob_log_odds = LogOdds[(int)(occupancy_probability * colour_probability * LOG_ODDS_LOOKUP_LEVELS)];
}
return (prob_log_odds);
}
/// <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>
/// probes the grid and returns a set of ranges
/// </summary>
/// <param name="pose">current best pose estimate</param>
/// <param name="camera_x_mm">camera sensor x coordinate</param>
/// <param name="camera_y_mm">camera sensor y coordinate</param>
/// <param name="camera_z_mm">camera sensor z coordinate</param>
/// <param name="camera_pan">camera pan in radians</param>
/// <param name="camera_tilt">camera tilt in radians</param>
/// <param name="camera_roll">camera roll in radians</param>
/// <param name="camera_FOV_degrees">camera field of view in degrees</param>
/// <param name="camera_resolution_width">camera resolution pixels horizontal</param>
/// <param name="camera_resolution_height">camera resolution pixels vertical</param>
/// <param name="step_size">sampling step size in pixels</param>
/// <param name="max_range_mm">maximum range</param>
/// <param name="use_ground_plane">whether to detect the ground plane, z=0</param>
/// <param name="range_buffer">buffer storing returned ranges (-1 = range unknown)</param>
public void ProbeView(
particlePose pose,
float camera_x_mm,
float camera_y_mm,
float camera_z_mm,
float camera_pan,
float camera_tilt,
float camera_roll,
float camera_FOV_degrees,
int camera_resolution_width,
int camera_resolution_height,
int step_size,
float max_range_mm,
bool use_ground_plane,
float[] range_buffer)
{
float camera_FOV_radians = camera_FOV_degrees * (float)Math.PI / 180.0f;
float camera_FOV_radians2 = camera_FOV_radians * camera_resolution_height / camera_resolution_width;
int n = 0;
for (int camy = 0; camy < camera_resolution_height; camy+=step_size)
{
float tilt = (camy * camera_FOV_radians2 / camera_resolution_height) - (camera_FOV_radians2*0.5f);
float z = max_range_mm*(float)Math.Tan(tilt);
for (int camx = 0; camx < camera_resolution_width; camx+=step_size, n++)
{
float pan = (camx * camera_FOV_radians / camera_resolution_width) - (camera_FOV_radians*0.5f);
float x = max_range_mm*(float)Math.Tan(pan);
pos3D p = new pos3D(x,max_range_mm,z);
pos3D p2 = p.rotate(camera_pan, camera_tilt, camera_roll);
range_buffer[n] = ProbeRange(
pose,
camera_x_mm,
camera_y_mm,
camera_z_mm,
camera_x_mm + p2.x,
camera_y_mm + p2.y,
camera_z_mm + p2.z,
use_ground_plane);
}
}
}
/// <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);
}
/// <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 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>
/// 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 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>
/// 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>
/// 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="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 z,
float pan,
float tilt,
float roll,
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, z, pan, tilt, roll, this);
pose.time_step = time_step;
Enabled = true;
Add(pose);
}
/// <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>
/// remove all poses along the path until a branch point is located
/// </summary>
/// <param name="pose"></param>
/// <returns></returns>
private particlePose CollapseBranch(
particlePose pose,
dpslam 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>
/// 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),
Convert.ToSingle(poseStr[3], format) * (float)Math.PI/180.0f,
Convert.ToSingle(poseStr[4], format) * (float)Math.PI/180.0f,
Convert.ToSingle(poseStr[5], 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>
/// 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>
/// 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 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>
/// 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 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 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>
/// 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>
/// 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>
/// 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>
/// 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,
bool use_ground_plane)
{
float range_mm = -1;
int half_dimension_cells = dimension_cells/2;
int half_dimension_cells_vertical = dimension_cells_vertical/2;
int ground_plane_cells = half_dimension_cells_vertical - (int)(z / cellSize_mm);
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_vertical + (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_vertical + (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.0f / (float)dist_cells;
float fraction = 0;
float[] colour = new float[3];
float mean_variance = 0;
float prob, peak_prob = 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))
{
if ((z_cell < ground_plane_cells) && (use_ground_plane))
{
range_mm = dist * cellSize_mm;
//break;
}
else
{
occupancygridCellMultiHypothesis c = cell[x_cell][y_cell];
if (c != null)
{
prob = c.GetProbability(pose, x_cell, y_cell, z_cell, false, colour, ref mean_variance);
if (prob > 0)
{
if (prob > peak_prob)
{
peak_prob = prob;
range_mm = dist * cellSize_mm;
}
if ((peak_prob > 0.5f) && (prob < peak_prob)) break;
}
}
}
}
else break;
}
else break;
}
else break;
}
return(range_mm);
}
public void Show(
string filename,
int width,
int height,
particlePose pose)
{
byte[] img = new byte[width * height * 3];
Show(img, width, height, pose);
Bitmap bmp = new Bitmap(width, height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
if (filename.EndsWith("bmp")) bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Bmp);
if (filename.EndsWith("jpg")) bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Jpeg);
if (filename.EndsWith("gif")) bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Gif);
if (filename.EndsWith("png")) bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Png);
}
/// <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>
/// 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>
/// 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
float ray_model_interval_pixels = sensormodel_lookup.ray_model_interval_pixels;
int sensormodel_index = (int)Math.Round(ray.disparity / ray_model_interval_pixels);
// 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_lookup == null) Console.WriteLine("Sensor models are missing");
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 = ray_model_interval_pixels + (sensormodel_lookup_probability[sensormodel_index][grid_step] * ray_model_interval_pixels);
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>
/// 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>
/// 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];
}
}
}
}
}
}
}
}
}
}
}
public void AveragePose(
ref float x,
ref float y,
ref float z,
ref float pan,
ref float tilt,
ref float roll,
ref float deviation,
ref float deviation_forward,
ref float deviation_perp,
ref float deviation_vertical)
{
x = 0;
y = 0;
z = 0;
pan = 0;
tilt = 0;
roll = 0;
deviation = 0;
deviation_forward = 0;
deviation_perp = 0;
deviation_vertical = 0;
for (int p = 0; p < Poses.Count; p++)
{
particlePose pose = Poses[p].current_pose;
x += pose.x;
y += pose.y;
z += pose.z;
float pan2 = pose.pan;
if (pan2 < -Math.PI)
{
pan2 = -(2 * (float)Math.PI) - pan2;
}
if (pan2 > Math.PI)
{
pan2 = (2 * (float)Math.PI) - pan2;
}
pan += pan2;
float tilt2 = pose.tilt;
if (tilt2 < -Math.PI)
{
tilt2 = -(2 * (float)Math.PI) - tilt2;
}
if (tilt2 > Math.PI)
{
tilt2 = (2 * (float)Math.PI) - tilt2;
}
tilt += tilt2;
roll += pose.roll;
}
if (Poses.Count > 0)
{
x /= Poses.Count;
y /= Poses.Count;
z /= Poses.Count;
pan /= Poses.Count;
tilt /= Poses.Count;
roll /= Poses.Count;
float axis_x0 = x + (10 * (float)Math.Sin(pan));
float axis_y0 = y + (10 * (float)Math.Cos(pan));
float axis_x1 = x + (10 * (float)Math.Sin(pan + (Math.PI / 2)));
float axis_y1 = y + (10 * (float)Math.Cos(pan + (Math.PI / 2)));
for (int p = 0; p < Poses.Count; p++)
{
particlePose pose = Poses[p].current_pose;
float dx = x - pose.x;
float dy = y - pose.y;
float dz = z - pose.z;
float dev = (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
deviation += dev;
float d = Math.Abs(geometry.circleDistanceFromLine(x, y, axis_x0, axis_y0, pose.x, pose.y, 0));
deviation_forward += d;
d = Math.Abs(geometry.circleDistanceFromLine(x, y, axis_x1, axis_y1, pose.x, pose.y, 0));
deviation_perp += d;
deviation_vertical += Math.Abs(dz);
}
deviation /= Poses.Count;
deviation *= 4;
deviation_forward /= Poses.Count;
deviation_forward *= 4;
deviation_perp /= Poses.Count;
deviation_perp *= 4;
deviation_vertical /= Poses.Count;
deviation_vertical *= 4;
}
}