/// <summary>
/// calculate the position of the robots head and cameras for this pose
/// </summary>
/// <param name="rob">robot object</param>
/// <param name="head_location">location of the centre of the head</param>
/// <param name="camera_centre_location">location of the centre of each stereo camera</param>
/// <param name="left_camera_location">location of the left camera within each stereo camera</param>
/// <param name="right_camera_location">location of the right camera within each stereo camera</param>
protected void calculateCameraPositions(
robot rob,
ref pos3D head_location,
ref pos3D[] camera_centre_location,
ref pos3D[] left_camera_location,
ref pos3D[] right_camera_location)
{
// calculate the position of the centre of the head relative to
// the centre of rotation of the robots body
pos3D head_centroid = new pos3D(-(rob.BodyWidth_mm / 2) + rob.head.x,
-(rob.BodyLength_mm / 2) + rob.head.y,
rob.head.z);
// location of the centre of the head on the grid map
// adjusted for the robot pose and the head pan and tilt angle.
// Note that the positions and orientations of individual cameras
// on the head have already been accounted for within stereoModel.createObservation
pos3D head_locn = head_centroid.rotate(rob.head.pan + pan, rob.head.tilt, 0);
head_locn = head_locn.translate(x, y, 0);
head_location.copyFrom(head_locn);
for (int cam = 0; cam < rob.head.no_of_stereo_cameras; cam++)
{
// calculate the position of the centre of the stereo camera
// (baseline centre point)
pos3D camera_centre_locn = new pos3D(rob.head.calibration[cam].positionOrientation.x, rob.head.calibration[cam].positionOrientation.y, rob.head.calibration[cam].positionOrientation.y);
camera_centre_locn = camera_centre_locn.rotate(rob.head.calibration[cam].positionOrientation.pan + rob.head.pan + pan, rob.head.calibration[cam].positionOrientation.tilt, rob.head.calibration[cam].positionOrientation.roll);
camera_centre_location[cam] = camera_centre_locn.translate(head_location.x, head_location.y, head_location.z);
// where are the left and right cameras?
// we need to know this for the origins of the vacancy models
float half_baseline_length = rob.head.calibration[cam].baseline / 2;
pos3D left_camera_locn = new pos3D(-half_baseline_length, 0, 0);
left_camera_locn = left_camera_locn.rotate(rob.head.calibration[cam].positionOrientation.pan + rob.head.pan + pan, rob.head.calibration[cam].positionOrientation.tilt, rob.head.calibration[cam].positionOrientation.roll);
pos3D right_camera_locn = new pos3D(-left_camera_locn.x, -left_camera_locn.y, -left_camera_locn.z);
left_camera_location[cam] = left_camera_locn.translate(camera_centre_location[cam].x, camera_centre_location[cam].y, camera_centre_location[cam].z);
right_camera_location[cam] = right_camera_locn.translate(camera_centre_location[cam].x, camera_centre_location[cam].y, camera_centre_location[cam].z);
right_camera_location[cam].pan = left_camera_location[cam].pan;
}
}
public void InsertRays()
{
int no_of_stereo_features = 2000;
int image_width = 640;
int image_height = 480;
int no_of_stereo_cameras = 1;
int localisationRadius_mm = 16000;
int maxMappingRange_mm = 16000;
int cellSize_mm = 32;
int dimension_cells = 16000 / cellSize_mm;
int dimension_cells_vertical = dimension_cells/2;
float vacancyWeighting = 0.5f;
float FOV_horizontal = 78 * (float)Math.PI / 180.0f;
// create a grid
Console.WriteLine("Creating grid");
occupancygridSimple grid =
new occupancygridSimple(
dimension_cells,
dimension_cells_vertical,
cellSize_mm,
localisationRadius_mm,
maxMappingRange_mm,
vacancyWeighting);
Assert.AreNotEqual(grid, null, "object occupancygridSimple was not created");
Console.WriteLine("Creating sensor models");
stereoModel inverseSensorModel = new stereoModel();
inverseSensorModel.FOV_horizontal = FOV_horizontal;
inverseSensorModel.FOV_vertical = FOV_horizontal * image_height / image_width;
inverseSensorModel.createLookupTable(cellSize_mm, image_width, image_height);
//Assert.AreNotEqual(0, inverseSensorModel.ray_model.probability[1][5], "Ray model probabilities not updated");
// observer parameters
int pan_angle_degrees = 0;
pos3D observer = new pos3D(0,0,0);
observer.pan = pan_angle_degrees * (float)Math.PI / 180.0f;
float stereo_camera_baseline_mm = 100;
pos3D left_camera_location = new pos3D(stereo_camera_baseline_mm*0.5f,0,0);
pos3D right_camera_location = new pos3D(-stereo_camera_baseline_mm*0.5f,0,0);
left_camera_location = left_camera_location.rotate(observer.pan, observer.tilt, observer.roll);
right_camera_location = right_camera_location.rotate(observer.pan, observer.tilt, observer.roll);
left_camera_location = left_camera_location.translate(observer.x, observer.y, observer.z);
right_camera_location = right_camera_location.translate(observer.x, observer.y, observer.z);
float FOV_degrees = 78;
float[] stereo_features = new float[no_of_stereo_features * 3];
byte[,] stereo_features_colour = new byte[no_of_stereo_features, 3];
float[] stereo_features_uncertainties = new float[no_of_stereo_features];
// create some stereo disparities within the field of view
Console.WriteLine("Adding disparities");
//MersenneTwister rnd = new MersenneTwister(0);
Random rnd = new Random(0);
for (int correspondence = 0; correspondence < no_of_stereo_features; correspondence++)
{
float x = rnd.Next(image_width-1);
float y = rnd.Next(image_height/50) + (image_height/2);
float disparity = 7;
if ((x < image_width/5) || (x > image_width * 4/5))
{
disparity = 7; //15;
}
byte colour_red = (byte)rnd.Next(255);
byte colour_green = (byte)rnd.Next(255);
byte colour_blue = (byte)rnd.Next(255);
stereo_features[correspondence*3] = x;
stereo_features[(correspondence*3)+1] = y;
stereo_features[(correspondence*3)+2] = disparity;
stereo_features_colour[correspondence, 0] = colour_red;
stereo_features_colour[correspondence, 1] = colour_green;
stereo_features_colour[correspondence, 2] = colour_blue;
stereo_features_uncertainties[correspondence] = 0;
}
// create an observation as a set of rays from the stereo correspondence results
List<evidenceRay>[] stereo_rays = new List<evidenceRay>[no_of_stereo_cameras];
for (int cam = 0; cam < no_of_stereo_cameras; cam++)
{
Console.WriteLine("Creating rays");
stereo_rays[cam] =
inverseSensorModel.createObservation(
observer,
stereo_camera_baseline_mm,
image_width,
image_height,
FOV_degrees,
stereo_features,
stereo_features_colour,
stereo_features_uncertainties,
true);
// insert rays into the grid
Console.WriteLine("Throwing rays");
for (int ray = 0; ray < stereo_rays[cam].Count; ray++)
{
grid.Insert(stereo_rays[cam][ray], inverseSensorModel.ray_model, left_camera_location, right_camera_location, false);
}
}
// save the result as an image
Console.WriteLine("Saving grid");
int debug_img_width = 640;
int debug_img_height = 480;
byte[] debug_img = new byte[debug_img_width * debug_img_height * 3];
Bitmap bmp = new Bitmap(debug_img_width, debug_img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
grid.Show(debug_img, debug_img_width, debug_img_height, false, false);
BitmapArrayConversions.updatebitmap_unsafe(debug_img, bmp);
bmp.Save("tests_occupancygrid_simple_InsertRays_overhead.jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
grid.ShowFront(debug_img, debug_img_width, debug_img_height, true);
BitmapArrayConversions.updatebitmap_unsafe(debug_img, bmp);
bmp.Save("tests_occupancygrid_simple_InsertRays_front.jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
// side view of the probabilities
float max_prob = -1;
float min_prob = 1;
float[] probs = new float[dimension_cells/2];
float[] mean_colour = new float[3];
for (int y = dimension_cells / 2; y < dimension_cells; y++)
{
float p = grid.GetProbability(dimension_cells / 2, y, mean_colour);
probs[y - (dimension_cells / 2)] = p;
if (p != occupancygridSimple.NO_OCCUPANCY_EVIDENCE)
{
if (p < min_prob) min_prob = p;
if (p > max_prob) max_prob = p;
}
}
for (int i = 0; i < debug_img.Length; i++) debug_img[i] = 255;
int prev_x = -1;
int prev_y = debug_img_height / 2;
for (int i = 0; i < probs.Length; i++)
{
if (probs[i] != occupancygridSimple.NO_OCCUPANCY_EVIDENCE)
{
int x = i * (debug_img_width - 1) / probs.Length;
int y = debug_img_height - 1 - (int)((probs[i] - min_prob) / (max_prob - min_prob) * (debug_img_height - 1));
int n = ((y * debug_img_width) + x) * 3;
if (prev_x > -1)
{
int r = 255;
int g = 0;
int b = 0;
if (probs[i] > 0.5f)
{
r = 0;
g = 255;
b = 0;
}
drawing.drawLine(debug_img, debug_img_width, debug_img_height, prev_x, prev_y, x, y, r, g, b, 0, false);
}
prev_x = x;
prev_y = y;
}
}
int y_zero = debug_img_height - 1 - (int)((0.5f-min_prob) / (max_prob - min_prob) * (debug_img_height - 1));
drawing.drawLine(debug_img, debug_img_width, debug_img_height, 0, y_zero, debug_img_width - 1, y_zero, 0, 0, 0, 0, false);
BitmapArrayConversions.updatebitmap_unsafe(debug_img, bmp);
bmp.Save("tests_occupancygrid_simple_InsertRays_probs.jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
}
/// <summary>
/// Calculate the position of the robots head and cameras for this pose
/// the positions returned are relative to the robots centre of rotation
/// </summary>
/// <param name="body_width_mm">width of the robot body in millimetres</param>
/// <param name="body_length_mm">length of the robot body in millimetres</param>
/// <param name="body_centre_of_rotation_x">x centre of rotation of the robot, relative to the top left corner</param>
/// <param name="body_centre_of_rotation_y">y centre of rotation of the robot, relative to the top left corner</param>
/// <param name="body_centre_of_rotation_z">z centre of rotation of the robot, relative to the top left corner</param>
/// <param name="head_centroid_x">head centroid x position in millimetres relative to the top left corner of the body</param>
/// <param name="head_centroid_y">head centroid y position in millimetres relative to the top left corner of the body</param>
/// <param name="head_centroid_z">head centroid z position in millimetres relative to the top left corner of the body</param>
/// <param name="head_pan">head pan angle in radians</param>
/// <param name="head_tilt">head tilt angle in radians</param>
/// <param name="head_roll">head roll angle in radians</param>
/// <param name="baseline_mm">stereo camera baseline in millimetres</param>
/// <param name="stereo_camera_position_x">stereo camera x position in millimetres relative to the head centroid</param>
/// <param name="stereo_camera_position_y">stereo camera y position in millimetres relative to the head centroid</param>
/// <param name="stereo_camera_position_z">stereo camera z position in millimetres relative to the head centroid</param>
/// <param name="stereo_camera_pan">stereo camera pan in radians relative to the head</param>
/// <param name="stereo_camera_tilt">stereo camera tilt in radians relative to the head</param>
/// <param name="stereo_camera_roll">stereo camera roll in radians relative to the head</param>
/// <param name="head_location">returned location/orientation of the robot head</param>
/// <param name="camera_centre_location">returned stereo camera centre position/orientation</param>
/// <param name="left_camera_location">returned left camera position/orientation</param>
/// <param name="right_camera_location">returned right camera position/orientation</param>
public static void calculateCameraPositions(
float body_width_mm,
float body_length_mm,
float body_centre_of_rotation_x,
float body_centre_of_rotation_y,
float body_centre_of_rotation_z,
float head_centroid_x,
float head_centroid_y,
float head_centroid_z,
float head_pan,
float head_tilt,
float head_roll,
float baseline_mm,
float stereo_camera_position_x,
float stereo_camera_position_y,
float stereo_camera_position_z,
float stereo_camera_pan,
float stereo_camera_tilt,
float stereo_camera_roll,
ref pos3D head_location,
ref pos3D camera_centre_location,
ref pos3D left_camera_location,
ref pos3D right_camera_location)
{
// calculate the position of the centre of the head relative to
// the centre of rotation of the robots body
pos3D head_centroid =
new pos3D(
-(body_width_mm * 0.5f) + (body_centre_of_rotation_x - (body_width_mm * 0.5f)) + head_centroid_x,
-(body_length_mm * 0.5f) + (body_centre_of_rotation_y - (body_length_mm * 0.5f)) + head_centroid_y,
head_centroid_z);
// location of the centre of the head
// adjusted for the robot pose and the head pan and tilt angle.
// Note that the positions and orientations of individual cameras
// on the head have already been accounted for within stereoModel.createObservation
pos3D head_locn =
head_centroid.rotate(
head_pan, head_tilt, 0);
head_location.copyFrom(head_locn);
// calculate the position of the centre of the stereo camera
// (baseline centre point)
pos3D camera_centre_locn =
new pos3D(
stereo_camera_position_x,
stereo_camera_position_y,
stereo_camera_position_z);
// rotate the stereo camera
camera_centre_locn =
camera_centre_locn.rotate(
stereo_camera_pan + head_pan,
stereo_camera_tilt + head_tilt,
stereo_camera_roll + head_roll);
// move the stereo camera relative to the head position
camera_centre_location =
camera_centre_locn.translate(
head_location.x,
head_location.y,
head_location.z);
// where are the left and right cameras?
// we need to know this for the origins of the vacancy models
float half_baseline_length = baseline_mm * 0.5f;
pos3D left_camera_locn = new pos3D(-half_baseline_length, 0, 0);
left_camera_locn = left_camera_locn.rotate(stereo_camera_pan + head_pan, stereo_camera_tilt + head_tilt, stereo_camera_roll + head_roll);
pos3D right_camera_locn = new pos3D(-left_camera_locn.x, -left_camera_locn.y, -left_camera_locn.z);
left_camera_location = left_camera_locn.translate(camera_centre_location.x, camera_centre_location.y, camera_centre_location.z);
right_camera_location = right_camera_locn.translate(camera_centre_location.x, camera_centre_location.y, camera_centre_location.z);
right_camera_location.pan = left_camera_location.pan;
}
/// <summary>
/// calculate the position of the robots head and cameras for this pose
/// </summary>
/// <param name="rob">robot object</param>
/// <param name="head_location">location of the centre of the head</param>
/// <param name="camera_centre_location">location of the centre of each stereo camera</param>
/// <param name="left_camera_location">location of the left camera within each stereo camera</param>
/// <param name="right_camera_location">location of the right camera within each stereo camera</param>
protected void calculateCameraPositions(
robot rob,
ref pos3D head_location,
ref pos3D[] camera_centre_location,
ref pos3D[] left_camera_location,
ref pos3D[] right_camera_location)
{
// calculate the position of the centre of the head relative to
// the centre of rotation of the robots body
pos3D head_centroid = new pos3D(-(rob.BodyWidth_mm / 2) + rob.head.x,
-(rob.BodyLength_mm / 2) + rob.head.y,
rob.head.z);
// location of the centre of the head on the grid map
// adjusted for the robot pose and the head pan and tilt angle.
// Note that the positions and orientations of individual cameras
// on the head have already been accounted for within stereoModel.createObservation
pos3D head_locn = head_centroid.rotate(rob.head.pan + pan, rob.head.tilt, 0);
head_locn = head_locn.translate(x, y, 0);
head_location.copyFrom(head_locn);
for (int cam = 0; cam < rob.head.no_of_stereo_cameras; cam++)
{
// calculate the position of the centre of the stereo camera
// (baseline centre point)
pos3D camera_centre_locn = new pos3D(rob.head.calibration[cam].positionOrientation.x, rob.head.calibration[cam].positionOrientation.y, rob.head.calibration[cam].positionOrientation.y);
camera_centre_locn = camera_centre_locn.rotate(rob.head.calibration[cam].positionOrientation.pan + rob.head.pan + pan, rob.head.calibration[cam].positionOrientation.tilt, rob.head.calibration[cam].positionOrientation.roll);
camera_centre_location[cam] = camera_centre_locn.translate(head_location.x, head_location.y, head_location.z);
// where are the left and right cameras?
// we need to know this for the origins of the vacancy models
float half_baseline_length = rob.head.calibration[cam].baseline / 2;
pos3D left_camera_locn = new pos3D(-half_baseline_length, 0, 0);
left_camera_locn = left_camera_locn.rotate(rob.head.calibration[cam].positionOrientation.pan + rob.head.pan + pan, rob.head.calibration[cam].positionOrientation.tilt, rob.head.calibration[cam].positionOrientation.roll);
pos3D right_camera_locn = new pos3D(-left_camera_locn.x, -left_camera_locn.y, -left_camera_locn.z);
left_camera_location[cam] = left_camera_locn.translate(camera_centre_location[cam].x, camera_centre_location[cam].y, camera_centre_location[cam].z);
right_camera_location[cam] = right_camera_locn.translate(camera_centre_location[cam].x, camera_centre_location[cam].y, camera_centre_location[cam].z);
right_camera_location[cam].pan = left_camera_location[cam].pan;
}
}
public void RotatePoint()
{
pos3D point = new pos3D(0, 100, 0);
point.pan = DegreesToRadians(10);
pos3D rotated = point.rotate(DegreesToRadians(30), 0, 0);
int pan = (int)Math.Round(RadiansToDegrees(rotated.pan));
Assert.AreEqual(40, pan, "pan positive");
Assert.AreEqual(50, (int)Math.Round(rotated.x), "pan positive x");
Assert.AreEqual(87, (int)Math.Round(rotated.y), "pan positive y");
rotated = point.rotate(DegreesToRadians(-30), 0, 0);
pan = (int)Math.Round(RadiansToDegrees(rotated.pan));
Assert.AreEqual(-20, pan, "pan negative");
Assert.AreEqual(-50, (int)Math.Round(rotated.x), "pan negative x");
Assert.AreEqual(87, (int)Math.Round(rotated.y), "pan negative y");
point.pan = 0;
point.tilt = DegreesToRadians(5);
pos3D tilted = point.rotate(0, DegreesToRadians(30), 0);
int tilt = (int)Math.Round(RadiansToDegrees(tilted.tilt));
Assert.AreEqual(35, tilt, "tilt positive");
point.pan = 0;
point.tilt = 0;
point.roll = DegreesToRadians(2);
pos3D rolled = point.rotate(0, 0, DegreesToRadians(-20));
int roll = (int)Math.Round(RadiansToDegrees(rolled.roll));
Assert.AreEqual(-18, roll, "roll negative");
//Console.WriteLine("x = " + rotated.x.ToString());
//Console.WriteLine("y = " + rotated.y.ToString());
}
public void PanTilt()
{
int pan_angle1 = -40;
float pan1 = pan_angle1 * (float)Math.PI / 180.0f;
int tilt_angle1 = 20;
float tilt1 = tilt_angle1 * (float)Math.PI / 180.0f;
pos3D pos1 = new pos3D(0, 50, 0);
pos3D pos2 = pos1.rotate_old(pan1,tilt1,0);
pos3D pos3 = pos1.rotate(pan1,tilt1,0);
float dx = Math.Abs(pos2.x - pos3.x);
float dy = Math.Abs(pos2.y - pos3.y);
float dz = Math.Abs(pos2.z - pos3.z);
Console.WriteLine("pos old: " + pos2.x.ToString() + ", " + pos2.y.ToString() + ", " + pos2.z.ToString());
Console.WriteLine("pos new: " + pos3.x.ToString() + ", " + pos3.y.ToString() + ", " + pos3.z.ToString());
Assert.Less(dx, 1);
Assert.Less(dy, 1);
Assert.Less(dz, 1);
}
public void Roll()
{
int roll_angle1 = 20;
float roll1 = roll_angle1 * (float)Math.PI / 180.0f;
pos3D pos1 = new pos3D(50, 0, 0);
pos3D pos2 = pos1.rotate_old(0,0,roll1);
pos3D pos3 = pos1.rotate(0,0,roll1);
float dx = Math.Abs(pos2.x - pos3.x);
float dy = Math.Abs(pos2.y - pos3.y);
float dz = Math.Abs(pos2.z - pos3.z);
Console.WriteLine("pos old: " + pos2.x.ToString() + ", " + pos2.y.ToString() + ", " + pos2.z.ToString());
Console.WriteLine("pos new: " + pos3.x.ToString() + ", " + pos3.y.ToString() + ", " + pos3.z.ToString());
Assert.Less(dx, 1);
Assert.Less(dy, 1);
Assert.Less(dz, 1);
}
/// <summary>
/// Localisation
/// </summary>
/// <param name="body_width_mm">width of the robot body in millimetres</param>
/// <param name="body_length_mm">length of the robot body in millimetres</param>
/// <param name="body_centre_of_rotation_x">x centre of rotation of the robot, relative to the top left corner</param>
/// <param name="body_centre_of_rotation_y">y centre of rotation of the robot, relative to the top left corner</param>
/// <param name="body_centre_of_rotation_z">z centre of rotation of the robot, relative to the top left corner</param>
/// <param name="head_centroid_x">head centroid x position in millimetres relative to the top left corner of the body</param>
/// <param name="head_centroid_y">head centroid y position in millimetres relative to the top left corner of the body</param>
/// <param name="head_centroid_z">head centroid z position in millimetres relative to the top left corner of the body</param>
/// <param name="head_pan">head pan angle in radians</param>
/// <param name="head_tilt">head tilt angle in radians</param>
/// <param name="head_roll">head roll angle in radians</param>
/// <param name="baseline_mm">stereo camera baseline in millimetres</param>
/// <param name="stereo_camera_position_x">stereo camera x position in millimetres relative to the head centroid</param>
/// <param name="stereo_camera_position_y">stereo camera y position in millimetres relative to the head centroid</param>
/// <param name="stereo_camera_position_z">stereo camera z position in millimetres relative to the head centroid</param>
/// <param name="stereo_camera_pan">stereo camera pan in radians relative to the head</param>
/// <param name="stereo_camera_tilt">stereo camera tilt in radians relative to the head</param>
/// <param name="stereo_camera_roll">stereo camera roll in radians relative to the head</param>
/// <param name="image_width">image width for each stereo camera</param>
/// <param name="image_height">image height for each stereo camera</param>
/// <param name="FOV_degrees">field of view for each stereo camera in degrees</param>
/// <param name="stereo_features">stereo features (disparities) for each stereo camera</param>
/// <param name="stereo_features_colour">stereo feature colours for each stereo camera</param>
/// <param name="stereo_features_uncertainties">stereo feature uncertainties (priors) for each stereo camera</param>
/// <param name="sensormodel">sensor model for each stereo camera</param>
/// <param name="left_camera_location">returned position and orientation of the left camera on each stereo camera</param>
/// <param name="right_camera_location">returned position and orientation of the right camera on each stereo camera</param>
/// <param name="no_of_samples">number of sample poses</param>
/// <param name="sampling_radius_major_mm">major radius for samples, in the direction of robot movement</param>
/// <param name="sampling_radius_minor_mm">minor radius for samples, perpendicular to the direction of robot movement</param>
/// <param name="robot_pose">current estimated position and orientation of the robots centre of rotation, from which each stereo camera took its observations</param>
/// <param name="max_orientation_variance">maximum variance in orientation in radians, used to create sample poses</param>
/// <param name="max_tilt_variance">maximum variance in tilt angle in radians, used to create sample poses</param>
/// <param name="max_roll_variance">maximum variance in roll angle in radians, used to create sample poses</param>
/// <param name="poses">list of poses tried</param>
/// <param name="pose_score">list of pose matching scores</param>
/// <param name="pose_offset">offset of the best pose from the current one</param>
/// <param name="rnd">random number generator</param>
/// <param name="pose_offset">returned pose offset</param>
/// <param name="img_poses">optional image within which to show poses</param>
/// <param name="img_poses_width">width of the poses image</param>
/// <param name="img_poses_height">height of the poses image</param>
/// <returns>best localisation matching score</returns>
public float Localise(
float body_width_mm,
float body_length_mm,
float body_centre_of_rotation_x,
float body_centre_of_rotation_y,
float body_centre_of_rotation_z,
float head_centroid_x,
float head_centroid_y,
float head_centroid_z,
float head_pan,
float head_tilt,
float head_roll,
float[] baseline_mm,
float[] stereo_camera_position_x,
float[] stereo_camera_position_y,
float[] stereo_camera_position_z,
float[] stereo_camera_pan,
float[] stereo_camera_tilt,
float[] stereo_camera_roll,
int[] image_width,
int[] image_height,
float[] FOV_degrees,
float[][] stereo_features,
byte[][,] stereo_features_colour,
float[][] stereo_features_uncertainties,
stereoModel[][] sensormodel,
ref pos3D[] left_camera_location,
ref pos3D[] right_camera_location,
int no_of_samples,
float sampling_radius_major_mm,
float sampling_radius_minor_mm,
pos3D[] robot_pose,
float max_orientation_variance,
float max_tilt_variance,
float max_roll_variance,
List<pos3D> poses,
List<float> pose_score,
Random rnd,
ref pos3D pose_offset,
byte[] img_poses,
int img_poses_width,
int img_poses_height,
float known_best_pose_x_mm,
float known_best_pose_y_mm)
{
float best_matching_score = float.MinValue;
poses.Clear();
pose_score.Clear();
gridCells.CreateMoireGrid(
sampling_radius_major_mm,
sampling_radius_minor_mm,
no_of_samples,
robot_pose[0].pan,
robot_pose[0].tilt,
robot_pose[0].roll,
max_orientation_variance,
max_tilt_variance,
max_roll_variance,
rnd,
ref poses,
null, null, 0, 0);
// positions of the left and right camera relative to the robots centre of rotation
pos3D head_location = new pos3D(0, 0, 0);
left_camera_location = new pos3D[baseline_mm.Length];
right_camera_location = new pos3D[baseline_mm.Length];
pos3D[] camera_centre_location = new pos3D[baseline_mm.Length];
pos3D[] relative_left_cam = new pos3D[baseline_mm.Length];
pos3D[] relative_right_cam = new pos3D[baseline_mm.Length];
for (int cam = 0; cam < baseline_mm.Length; cam++)
{
occupancygridBase.calculateCameraPositions(
body_width_mm,
body_length_mm,
body_centre_of_rotation_x,
body_centre_of_rotation_y,
body_centre_of_rotation_z,
head_centroid_x,
head_centroid_y,
head_centroid_z,
head_pan,
head_tilt,
head_roll,
baseline_mm[cam],
stereo_camera_position_x[cam],
stereo_camera_position_y[cam],
stereo_camera_position_z[cam],
stereo_camera_pan[cam],
stereo_camera_tilt[cam],
stereo_camera_roll[cam],
ref head_location,
ref camera_centre_location[cam],
ref relative_left_cam[cam],
ref relative_right_cam[cam]);
left_camera_location[cam] = relative_left_cam[cam].translate(robot_pose[cam].x, robot_pose[cam].y, robot_pose[cam].z);
right_camera_location[cam] = relative_right_cam[cam].translate(robot_pose[cam].x, robot_pose[cam].y, robot_pose[cam].z);
}
pose_score.Clear();
for (int p = 0; p < poses.Count; p++)
{
pose_score.Add(0);
}
int no_of_zero_probabilities = 0;
// try a number of random poses
// we can do this in parallel
Parallel.For(0, poses.Count, delegate(int i)
//for (int i = 0; i < poses.Count; i++)
{
pos3D sample_pose = poses[i];
float matching_score = 0;
for (int cam = 0; cam < baseline_mm.Length; cam++)
{
// update the position of the left camera for this pose
pos3D sample_pose_left_cam = relative_left_cam[cam].add(sample_pose);
sample_pose_left_cam.pan = 0;
sample_pose_left_cam.tilt = 0;
sample_pose_left_cam.roll = 0;
sample_pose_left_cam = sample_pose_left_cam.rotate(sample_pose.pan, sample_pose.tilt, sample_pose.roll);
sample_pose_left_cam.x += robot_pose[cam].x;
sample_pose_left_cam.y += robot_pose[cam].y;
sample_pose_left_cam.z += robot_pose[cam].z;
// update the position of the right camera for this pose
pos3D sample_pose_right_cam = relative_right_cam[cam].add(sample_pose);
sample_pose_right_cam.pan = 0;
sample_pose_right_cam.tilt = 0;
sample_pose_right_cam.roll = 0;
sample_pose_right_cam = sample_pose_right_cam.rotate(sample_pose.pan, sample_pose.tilt, sample_pose.roll);
sample_pose_right_cam.x += robot_pose[cam].x;
sample_pose_right_cam.y += robot_pose[cam].y;
sample_pose_right_cam.z += robot_pose[cam].z;
// centre position between the left and right cameras
pos3D stereo_camera_centre =
new pos3D(
sample_pose_left_cam.x + ((sample_pose_right_cam.x - sample_pose_left_cam.x) * 0.5f),
sample_pose_left_cam.y + ((sample_pose_right_cam.y - sample_pose_left_cam.y) * 0.5f),
sample_pose_left_cam.z + ((sample_pose_right_cam.z - sample_pose_left_cam.z) * 0.5f));
stereo_camera_centre.pan = head_pan + stereo_camera_pan[cam] + sample_pose.pan;
stereo_camera_centre.tilt = head_tilt + stereo_camera_tilt[cam] + sample_pose.tilt;
stereo_camera_centre.roll = head_roll + stereo_camera_roll[cam] + sample_pose.roll;
// create a set of stereo rays as observed from this pose
List<evidenceRay> rays = sensormodel[cam][0].createObservation(
stereo_camera_centre,
baseline_mm[cam],
image_width[cam],
image_height[cam],
FOV_degrees[cam],
stereo_features[cam],
stereo_features_colour[cam],
stereo_features_uncertainties[cam],
true);
// insert rays into the occupancy grid
for (int r = 0; r < rays.Count; r++)
{
float score = Insert(rays[r], sensormodel[cam], sample_pose_left_cam, sample_pose_right_cam, true);
if (score != occupancygridBase.NO_OCCUPANCY_EVIDENCE)
{
if (matching_score == occupancygridBase.NO_OCCUPANCY_EVIDENCE) matching_score = 0;
matching_score += score;
}
}
}
// add the pose to the list
sample_pose.pan -= robot_pose[0].pan;
sample_pose.tilt -= robot_pose[0].tilt;
sample_pose.roll -= robot_pose[0].roll;
if (Math.Abs(sample_pose.pan) > max_orientation_variance)
{
Console.WriteLine("Pose variance out of range");
}
if (matching_score != occupancygridBase.NO_OCCUPANCY_EVIDENCE)
{
float prob = probabilities.LogOddsToProbability(matching_score);
if (prob == 0) no_of_zero_probabilities++;
pose_score[i] = prob;
//pose_score[i] = matching_score;
}
} );
if (no_of_zero_probabilities == poses.Count)
{
Console.WriteLine("Localisation failure");
pose_offset = new pos3D(0, 0, 0);
best_matching_score = occupancygridBase.NO_OCCUPANCY_EVIDENCE;
}
else
{
// locate the best possible pose
pos3D best_robot_pose = new pos3D(0, 0, 0);
gridCells.FindBestPose(poses, pose_score, ref best_robot_pose, sampling_radius_major_mm, img_poses, null, img_poses_width, img_poses_height, known_best_pose_x_mm, known_best_pose_y_mm);
// rotate the best pose to the robot's current orientation
// this becomes an offset, which may be used for course correction
pose_offset = best_robot_pose.rotate(robot_pose[0].pan, robot_pose[0].tilt, robot_pose[0].roll);
// orientation relative to the current heading
pose_offset.pan = best_robot_pose.pan;
pose_offset.tilt = best_robot_pose.tilt;
pose_offset.roll = best_robot_pose.roll;
// range checks
if (Math.Abs(pose_offset.pan) > max_orientation_variance) Console.WriteLine("pose_offset pan out of range");
if (Math.Abs(pose_offset.tilt) > max_tilt_variance) Console.WriteLine("pose_offset tilt out of range");
if (Math.Abs(pose_offset.roll) > max_roll_variance) Console.WriteLine("pose_offset roll out of range");
}
return (best_matching_score);
}
/// <summary>
/// Returns positions of grid cells corresponding to a moire grid
/// produced by the interference of a pair of hexagonal grids (theta waves)
/// </summary>
/// <param name="sampling_radius_major_mm">radius of the major axis of the bounding ellipse</param>
/// <param name="sampling_radius_minor_mm">radius of the minor axis of the bounding ellipse</param>
/// <param name="first_grid_spacing">spacing of the first hexagonal grid (theta wavelength 1)</param>
/// <param name="second_grid_spacing">spacing of the second hexagonal grid (theta wavelength 2)</param>
/// <param name="first_grid_rotation_degrees">rotation of the first grid (theta field 1)</param>
/// <param name="second_grid_rotation_degrees">rotation of the second grid (theta field 2)</param>
/// <param name="dimension_x_cells">number of grid cells in the x axis</param>
/// <param name="dimension_y_cells">number of grid cells in the y axis</param>
/// <param name="img">image data</param>
/// <param name="img_width">image width</param>
/// <param name="img_height">image height</param>
public static void ShowMoireGrid(
float sampling_radius_major_mm,
float sampling_radius_minor_mm,
float first_grid_spacing,
float second_grid_spacing,
float first_grid_rotation_degrees,
float second_grid_rotation_degrees,
int dimension_x_cells,
int dimension_y_cells,
float scaling_factor,
byte[] img,
int img_width,
int img_height)
{
float[,,] grid1 = new float[dimension_x_cells, dimension_y_cells, 2];
float[,,] grid2 = new float[dimension_x_cells, dimension_y_cells, 2];
float rotation = 0;
float min_x = float.MaxValue;
float max_x = float.MinValue;
float min_y = float.MaxValue;
float max_y = float.MinValue;
float radius_sqr = sampling_radius_minor_mm * sampling_radius_minor_mm;
for (int grd = 0; grd < 2; grd++)
{
float spacing = first_grid_spacing;
if (grd == 1)
{
spacing = second_grid_spacing;
}
float half_grid_spacing = spacing / 2;
float half_x = spacing * dimension_x_cells / 2;
float half_y = spacing * dimension_y_cells / 2;
if (grd == 0)
{
rotation = first_grid_rotation_degrees * (float)Math.PI / 180;
}
else
{
rotation = second_grid_rotation_degrees * (float)Math.PI / 180;
}
for (int cell_x = 0; cell_x < dimension_x_cells; cell_x++)
{
for (int cell_y = 0; cell_y < dimension_y_cells; cell_y++)
{
float x = (cell_x * spacing) - half_x;
if (cell_y % 2 == 0)
{
x += half_grid_spacing;
}
float y = (cell_y * spacing) - half_y;
x *= scaling_factor;
y *= scaling_factor;
pos3D p = new pos3D(x, y, 0);
p = p.rotate(rotation, 0, 0);
if (grd == 0)
{
grid1[cell_x, cell_y, 0] = p.x;
grid1[cell_x, cell_y, 1] = p.y;
}
else
{
grid2[cell_x, cell_y, 0] = p.x;
grid2[cell_x, cell_y, 1] = p.y;
}
if (p.x < min_x)
{
min_x = p.x;
}
if (p.y < min_y)
{
min_y = p.y;
}
if (p.x > max_x)
{
max_x = p.x;
}
if (p.y > max_y)
{
max_y = p.y;
}
}
}
}
for (int i = (img_width * img_height * 3) - 1; i >= 0; i--)
{
img[i] = 0;
}
int radius = img_width / (dimension_x_cells * 350 / 100);
if (radius < 2)
{
radius = 2;
}
int r, g, b;
for (int grd = 0; grd < 2; grd++)
{
float[,,] grid = grid1;
r = 5;
g = -1;
b = -1;
if (grd == 1)
{
grid = grid2;
r = -1;
g = 5;
b = -1;
}
for (int cell_x = 0; cell_x < dimension_x_cells; cell_x++)
{
for (int cell_y = 0; cell_y < dimension_y_cells; cell_y++)
{
int x = ((img_width - img_height) / 2) + (int)((grid[cell_x, cell_y, 0] - min_x) * img_height / (max_x - min_x));
int y = (int)((grid[cell_x, cell_y, 1] - min_y) * img_height / (max_y - min_y));
float dx = grid[cell_x, cell_y, 0];
float dy = (grid[cell_x, cell_y, 1] * sampling_radius_minor_mm) / sampling_radius_major_mm;
float dist = dx * dx + dy * dy;
if (dist <= radius_sqr)
{
drawing.drawSpotOverlay(img, img_width, img_height, x, y, radius, r, g, b);
}
}
}
for (int i = (img_width * img_height * 3) - 3; i >= 2; i -= 3)
{
if ((img[i + 2] > 0) && (img[i + 1] > 0))
{
img[i + 2] = 255;
img[i + 1] = 255;
}
}
}
}
/// <summary>
/// Returns positions of grid cells corresponding to a moire grid
/// produced by the interference of a pair of hexagonal grids (theta waves)
/// </summary>
/// <param name="first_grid_spacing">spacing of the first hexagonal grid (theta wavelength 1)</param>
/// <param name="second_grid_spacing">spacing of the second hexagonal grid (theta wavelength 2)</param>
/// <param name="phase_major_degrees">phase precession in the direction of motion in degrees</param>
/// <param name="phase_minor_degrees">phase precession perpendicular to the direction of motion in degrees</param>
/// <param name="first_grid_rotation_degrees">rotation of the first grid (theta field 1)</param>
/// <param name="second_grid_rotation_degrees">rotation of the second grid (theta field 2)</param>
/// <param name="dimension_x_cells">number of grid cells in the x axis</param>
/// <param name="dimension_y_cells">number of grid cells in the y axis</param>
/// <param name="scaling_factor">scaling factor (k)</param>
/// <param name="cells">returned grid cell positions</param>
public static void CreateMoireGrid(
float sampling_radius_major_mm,
float sampling_radius_minor_mm,
float first_grid_spacing,
float second_grid_spacing,
float first_grid_rotation_degrees,
float second_grid_rotation_degrees,
int dimension_x_cells,
int dimension_y_cells,
float scaling_factor,
ref List <pos3D> cells)
{
cells.Clear();
float scaling = 0;
float orientation = 0;
float dx, dy;
// compute scaling and orientation of the grid
MoireGrid(
first_grid_spacing,
second_grid_spacing,
first_grid_rotation_degrees,
second_grid_rotation_degrees,
scaling_factor,
ref scaling,
ref orientation);
float moire_grid_spacing = first_grid_spacing * scaling;
float half_grid_spacing = moire_grid_spacing / 2;
float radius_sqr = sampling_radius_minor_mm * sampling_radius_minor_mm;
Console.WriteLine("moire_grid_spacing: " + moire_grid_spacing.ToString());
Console.WriteLine("radius_sqr: " + radius_sqr.ToString());
float half_x = dimension_x_cells * moire_grid_spacing / 2.0f;
float half_y = dimension_y_cells * moire_grid_spacing / 2.0f;
for (int cell_x = 0; cell_x < dimension_x_cells; cell_x++)
{
for (int cell_y = 0; cell_y < dimension_y_cells; cell_y++)
{
float x = (cell_x * moire_grid_spacing) - half_x;
if (cell_y % 2 == 0)
{
x += half_grid_spacing;
}
float y = (cell_y * moire_grid_spacing) - half_y;
pos3D grid_cell = new pos3D(x, y, 0);
grid_cell = grid_cell.rotate(-orientation, 0, 0);
dx = grid_cell.x;
dy = (grid_cell.y * sampling_radius_minor_mm) / sampling_radius_major_mm;
float dist = dx * dx + dy * dy;
if (dist <= radius_sqr)
{
cells.Add(grid_cell);
}
}
}
}
/// <summary>
/// Calculate the position of the robots head and cameras for this pose
/// the positions returned are relative to the robots centre of rotation
/// </summary>
/// <param name="body_width_mm">width of the robot body in millimetres</param>
/// <param name="body_length_mm">length of the robot body in millimetres</param>
/// <param name="body_centre_of_rotation_x">x centre of rotation of the robot, relative to the top left corner</param>
/// <param name="body_centre_of_rotation_y">y centre of rotation of the robot, relative to the top left corner</param>
/// <param name="body_centre_of_rotation_z">z centre of rotation of the robot, relative to the top left corner</param>
/// <param name="head_centroid_x">head centroid x position in millimetres relative to the top left corner of the body</param>
/// <param name="head_centroid_y">head centroid y position in millimetres relative to the top left corner of the body</param>
/// <param name="head_centroid_z">head centroid z position in millimetres relative to the top left corner of the body</param>
/// <param name="head_pan">head pan angle in radians</param>
/// <param name="head_tilt">head tilt angle in radians</param>
/// <param name="head_roll">head roll angle in radians</param>
/// <param name="baseline_mm">stereo camera baseline in millimetres</param>
/// <param name="stereo_camera_position_x">stereo camera x position in millimetres relative to the head centroid</param>
/// <param name="stereo_camera_position_y">stereo camera y position in millimetres relative to the head centroid</param>
/// <param name="stereo_camera_position_z">stereo camera z position in millimetres relative to the head centroid</param>
/// <param name="stereo_camera_pan">stereo camera pan in radians relative to the head</param>
/// <param name="stereo_camera_tilt">stereo camera tilt in radians relative to the head</param>
/// <param name="stereo_camera_roll">stereo camera roll in radians relative to the head</param>
/// <param name="head_location">returned location/orientation of the robot head</param>
/// <param name="camera_centre_location">returned stereo camera centre position/orientation</param>
/// <param name="left_camera_location">returned left camera position/orientation</param>
/// <param name="right_camera_location">returned right camera position/orientation</param>
public static void calculateCameraPositions(
float body_width_mm,
float body_length_mm,
float body_centre_of_rotation_x,
float body_centre_of_rotation_y,
float body_centre_of_rotation_z,
float head_centroid_x,
float head_centroid_y,
float head_centroid_z,
float head_pan,
float head_tilt,
float head_roll,
float baseline_mm,
float stereo_camera_position_x,
float stereo_camera_position_y,
float stereo_camera_position_z,
float stereo_camera_pan,
float stereo_camera_tilt,
float stereo_camera_roll,
ref pos3D head_location,
ref pos3D camera_centre_location,
ref pos3D left_camera_location,
ref pos3D right_camera_location)
{
// calculate the position of the centre of the head relative to
// the centre of rotation of the robots body
pos3D head_centroid =
new pos3D(
-(body_width_mm * 0.5f) + (body_centre_of_rotation_x - (body_width_mm * 0.5f)) + head_centroid_x,
-(body_length_mm * 0.5f) + (body_centre_of_rotation_y - (body_length_mm * 0.5f)) + head_centroid_y,
head_centroid_z);
// location of the centre of the head
// adjusted for the robot pose and the head pan and tilt angle.
// Note that the positions and orientations of individual cameras
// on the head have already been accounted for within stereoModel.createObservation
pos3D head_locn =
head_centroid.rotate(
head_pan, head_tilt, 0);
head_location.copyFrom(head_locn);
// calculate the position of the centre of the stereo camera
// (baseline centre point)
pos3D camera_centre_locn =
new pos3D(
stereo_camera_position_x,
stereo_camera_position_y,
stereo_camera_position_z);
// rotate the stereo camera
camera_centre_locn =
camera_centre_locn.rotate(
stereo_camera_pan + head_pan,
stereo_camera_tilt + head_tilt,
stereo_camera_roll + head_roll);
// move the stereo camera relative to the head position
camera_centre_location =
camera_centre_locn.translate(
head_location.x,
head_location.y,
head_location.z);
// where are the left and right cameras?
// we need to know this for the origins of the vacancy models
float half_baseline_length = baseline_mm * 0.5f;
pos3D left_camera_locn = new pos3D(-half_baseline_length, 0, 0);
left_camera_locn = left_camera_locn.rotate(stereo_camera_pan + head_pan, stereo_camera_tilt + head_tilt, stereo_camera_roll + head_roll);
pos3D right_camera_locn = new pos3D(-left_camera_locn.x, -left_camera_locn.y, -left_camera_locn.z);
left_camera_location = left_camera_locn.translate(camera_centre_location.x, camera_centre_location.y, camera_centre_location.z);
right_camera_location = right_camera_locn.translate(camera_centre_location.x, camera_centre_location.y, camera_centre_location.z);
right_camera_location.pan = left_camera_location.pan;
}
/// <summary>
/// Returns positions of grid cells corresponding to a moire grid
/// produced by the interference of a pair of hexagonal grids (theta waves)
/// </summary>
/// <param name="sampling_radius_major_mm">radius of the major axis of the bounding ellipse</param>
/// <param name="sampling_radius_minor_mm">radius of the minor axis of the bounding ellipse</param>
/// <param name="first_grid_spacing">spacing of the first hexagonal grid (theta wavelength 1)</param>
/// <param name="second_grid_spacing">spacing of the second hexagonal grid (theta wavelength 2)</param>
/// <param name="first_grid_rotation_degrees">rotation of the first grid (theta field 1)</param>
/// <param name="second_grid_rotation_degrees">rotation of the second grid (theta field 2)</param>
/// <param name="dimension_x_cells">number of grid cells in the x axis</param>
/// <param name="dimension_y_cells">number of grid cells in the y axis</param>
/// <param name="img">image data</param>
/// <param name="img_width">image width</param>
/// <param name="img_height">image height</param>
public static void ShowMoireGrid(
float sampling_radius_major_mm,
float sampling_radius_minor_mm,
float first_grid_spacing,
float second_grid_spacing,
float first_grid_rotation_degrees,
float second_grid_rotation_degrees,
int dimension_x_cells,
int dimension_y_cells,
float scaling_factor,
byte[] img,
int img_width,
int img_height)
{
float[,,] grid1 = new float[dimension_x_cells, dimension_y_cells, 2];
float[,,] grid2 = new float[dimension_x_cells, dimension_y_cells, 2];
float rotation = 0;
float min_x = float.MaxValue;
float max_x = float.MinValue;
float min_y = float.MaxValue;
float max_y = float.MinValue;
float radius_sqr = sampling_radius_minor_mm*sampling_radius_minor_mm;
for (int grd = 0; grd < 2; grd++)
{
float spacing = first_grid_spacing;
if (grd == 1) spacing = second_grid_spacing;
float half_grid_spacing = spacing/2;
float half_x = spacing * dimension_x_cells / 2;
float half_y = spacing * dimension_y_cells / 2;
if (grd == 0)
rotation = first_grid_rotation_degrees * (float)Math.PI / 180;
else
rotation = second_grid_rotation_degrees * (float)Math.PI / 180;
for (int cell_x = 0; cell_x < dimension_x_cells; cell_x++)
{
for (int cell_y = 0; cell_y < dimension_y_cells; cell_y++)
{
float x = (cell_x * spacing) - half_x;
if (cell_y % 2 == 0) x += half_grid_spacing;
float y = (cell_y * spacing) - half_y;
x *= scaling_factor;
y *= scaling_factor;
pos3D p = new pos3D(x,y,0);
p = p.rotate(rotation, 0, 0);
if (grd == 0)
{
grid1[cell_x, cell_y, 0] = p.x;
grid1[cell_x, cell_y, 1] = p.y;
}
else
{
grid2[cell_x, cell_y, 0] = p.x;
grid2[cell_x, cell_y, 1] = p.y;
}
if (p.x < min_x) min_x = p.x;
if (p.y < min_y) min_y = p.y;
if (p.x > max_x) max_x = p.x;
if (p.y > max_y) max_y = p.y;
}
}
}
for (int i = (img_width * img_height * 3)-1; i >= 0; i--) img[i] = 0;
int radius = img_width / (dimension_x_cells*350/100);
if (radius < 2) radius = 2;
int r,g,b;
for (int grd = 0; grd < 2; grd++)
{
float[,,] grid = grid1;
r = 5;
g = -1;
b = -1;
if (grd == 1)
{
grid = grid2;
r = -1;
g = 5;
b = -1;
}
for (int cell_x = 0; cell_x < dimension_x_cells; cell_x++)
{
for (int cell_y = 0; cell_y < dimension_y_cells; cell_y++)
{
int x = ((img_width-img_height)/2) + (int)((grid[cell_x, cell_y, 0] - min_x) * img_height / (max_x - min_x));
int y = (int)((grid[cell_x, cell_y, 1] - min_y) * img_height / (max_y - min_y));
float dx = grid[cell_x, cell_y, 0];
float dy = (grid[cell_x, cell_y, 1] * sampling_radius_minor_mm) / sampling_radius_major_mm;
float dist = dx*dx + dy*dy;
if (dist <= radius_sqr)
{
drawing.drawSpotOverlay(img, img_width, img_height, x,y,radius,r,g,b);
}
}
}
for (int i = (img_width * img_height * 3)-3; i >= 2; i-=3)
{
if ((img[i+2] > 0) && (img[i+1] > 0))
{
img[i+2] = 255;
img[i+1] = 255;
}
}
}
}
/// <summary>
/// Returns positions of grid cells corresponding to a moire grid
/// produced by the interference of a pair of hexagonal grids (theta waves)
/// </summary>
/// <param name="first_grid_spacing">spacing of the first hexagonal grid (theta wavelength 1)</param>
/// <param name="second_grid_spacing">spacing of the second hexagonal grid (theta wavelength 2)</param>
/// <param name="phase_major_degrees">phase precession in the direction of motion in degrees</param>
/// <param name="phase_minor_degrees">phase precession perpendicular to the direction of motion in degrees</param>
/// <param name="first_grid_rotation_degrees">rotation of the first grid (theta field 1)</param>
/// <param name="second_grid_rotation_degrees">rotation of the second grid (theta field 2)</param>
/// <param name="dimension_x_cells">number of grid cells in the x axis</param>
/// <param name="dimension_y_cells">number of grid cells in the y axis</param>
/// <param name="scaling_factor">scaling factor (k)</param>
/// <param name="cells">returned grid cell positions</param>
public static void CreateMoireGrid(
float sampling_radius_major_mm,
float sampling_radius_minor_mm,
float first_grid_spacing,
float second_grid_spacing,
float first_grid_rotation_degrees,
float second_grid_rotation_degrees,
int dimension_x_cells,
int dimension_y_cells,
float scaling_factor,
ref List<pos3D> cells)
{
cells.Clear();
float scaling = 0;
float orientation = 0;
float dx, dy;
// compute scaling and orientation of the grid
MoireGrid(
first_grid_spacing,
second_grid_spacing,
first_grid_rotation_degrees,
second_grid_rotation_degrees,
scaling_factor,
ref scaling,
ref orientation);
float moire_grid_spacing = first_grid_spacing * scaling;
float half_grid_spacing = moire_grid_spacing/2;
float radius_sqr = sampling_radius_minor_mm*sampling_radius_minor_mm;
Console.WriteLine("moire_grid_spacing: " + moire_grid_spacing.ToString());
Console.WriteLine("radius_sqr: " + radius_sqr.ToString());
float half_x = dimension_x_cells * moire_grid_spacing / 2.0f;
float half_y = dimension_y_cells * moire_grid_spacing / 2.0f;
for (int cell_x = 0; cell_x < dimension_x_cells; cell_x++)
{
for (int cell_y = 0; cell_y < dimension_y_cells; cell_y++)
{
float x = (cell_x * moire_grid_spacing) - half_x;
if (cell_y % 2 == 0) x += half_grid_spacing;
float y = (cell_y * moire_grid_spacing) - half_y;
pos3D grid_cell = new pos3D(x, y, 0);
grid_cell = grid_cell.rotate(-orientation, 0, 0);
dx = grid_cell.x;
dy = (grid_cell.y * sampling_radius_minor_mm) / sampling_radius_major_mm;
float dist = dx*dx + dy*dy;
if (dist <= radius_sqr)
{
cells.Add(grid_cell);
}
}
}
}
/// <summary>
/// Localisation
/// </summary>
/// <param name="body_width_mm">width of the robot body in millimetres</param>
/// <param name="body_length_mm">length of the robot body in millimetres</param>
/// <param name="body_centre_of_rotation_x">x centre of rotation of the robot, relative to the top left corner</param>
/// <param name="body_centre_of_rotation_y">y centre of rotation of the robot, relative to the top left corner</param>
/// <param name="body_centre_of_rotation_z">z centre of rotation of the robot, relative to the top left corner</param>
/// <param name="head_centroid_x">head centroid x position in millimetres relative to the top left corner of the body</param>
/// <param name="head_centroid_y">head centroid y position in millimetres relative to the top left corner of the body</param>
/// <param name="head_centroid_z">head centroid z position in millimetres relative to the top left corner of the body</param>
/// <param name="head_pan">head pan angle in radians</param>
/// <param name="head_tilt">head tilt angle in radians</param>
/// <param name="head_roll">head roll angle in radians</param>
/// <param name="baseline_mm">stereo camera baseline in millimetres</param>
/// <param name="stereo_camera_position_x">stereo camera x position in millimetres relative to the head centroid</param>
/// <param name="stereo_camera_position_y">stereo camera y position in millimetres relative to the head centroid</param>
/// <param name="stereo_camera_position_z">stereo camera z position in millimetres relative to the head centroid</param>
/// <param name="stereo_camera_pan">stereo camera pan in radians relative to the head</param>
/// <param name="stereo_camera_tilt">stereo camera tilt in radians relative to the head</param>
/// <param name="stereo_camera_roll">stereo camera roll in radians relative to the head</param>
/// <param name="image_width">image width for each stereo camera</param>
/// <param name="image_height">image height for each stereo camera</param>
/// <param name="FOV_degrees">field of view for each stereo camera in degrees</param>
/// <param name="stereo_features">stereo features (disparities) for each stereo camera</param>
/// <param name="stereo_features_colour">stereo feature colours for each stereo camera</param>
/// <param name="stereo_features_uncertainties">stereo feature uncertainties (priors) for each stereo camera</param>
/// <param name="sensormodel">sensor model for each stereo camera</param>
/// <param name="left_camera_location">returned position and orientation of the left camera on each stereo camera</param>
/// <param name="right_camera_location">returned position and orientation of the right camera on each stereo camera</param>
/// <param name="no_of_samples">number of sample poses</param>
/// <param name="sampling_radius_major_mm">major radius for samples, in the direction of robot movement</param>
/// <param name="sampling_radius_minor_mm">minor radius for samples, perpendicular to the direction of robot movement</param>
/// <param name="robot_pose">current estimated position and orientation of the robots centre of rotation, from which each stereo camera took its observations</param>
/// <param name="max_orientation_variance">maximum variance in orientation in radians, used to create sample poses</param>
/// <param name="max_tilt_variance">maximum variance in tilt angle in radians, used to create sample poses</param>
/// <param name="max_roll_variance">maximum variance in roll angle in radians, used to create sample poses</param>
/// <param name="poses">list of poses tried</param>
/// <param name="pose_score">list of pose matching scores</param>
/// <param name="pose_offset">offset of the best pose from the current one</param>
/// <param name="rnd">random number generator</param>
/// <param name="pose_offset">returned pose offset</param>
/// <param name="img_poses">optional image within which to show poses</param>
/// <param name="img_poses_width">width of the poses image</param>
/// <param name="img_poses_height">height of the poses image</param>
/// <returns>best localisation matching score</returns>
public float Localise(
float body_width_mm,
float body_length_mm,
float body_centre_of_rotation_x,
float body_centre_of_rotation_y,
float body_centre_of_rotation_z,
float head_centroid_x,
float head_centroid_y,
float head_centroid_z,
float head_pan,
float head_tilt,
float head_roll,
float[] baseline_mm,
float[] stereo_camera_position_x,
float[] stereo_camera_position_y,
float[] stereo_camera_position_z,
float[] stereo_camera_pan,
float[] stereo_camera_tilt,
float[] stereo_camera_roll,
int[] image_width,
int[] image_height,
float[] FOV_degrees,
float[][] stereo_features,
byte[][,] stereo_features_colour,
float[][] stereo_features_uncertainties,
stereoModel[][] sensormodel,
ref pos3D[] left_camera_location,
ref pos3D[] right_camera_location,
int no_of_samples,
float sampling_radius_major_mm,
float sampling_radius_minor_mm,
pos3D[] robot_pose,
float max_orientation_variance,
float max_tilt_variance,
float max_roll_variance,
List <pos3D> poses,
List <float> pose_score,
Random rnd,
ref pos3D pose_offset,
byte[] img_poses,
int img_poses_width,
int img_poses_height,
float known_best_pose_x_mm,
float known_best_pose_y_mm)
{
float best_matching_score = float.MinValue;
poses.Clear();
pose_score.Clear();
gridCells.CreateMoireGrid(
sampling_radius_major_mm,
sampling_radius_minor_mm,
no_of_samples,
robot_pose[0].pan,
robot_pose[0].tilt,
robot_pose[0].roll,
max_orientation_variance,
max_tilt_variance,
max_roll_variance,
rnd,
ref poses,
null, null, 0, 0);
// positions of the left and right camera relative to the robots centre of rotation
pos3D head_location = new pos3D(0, 0, 0);
left_camera_location = new pos3D[baseline_mm.Length];
right_camera_location = new pos3D[baseline_mm.Length];
pos3D[] camera_centre_location = new pos3D[baseline_mm.Length];
pos3D[] relative_left_cam = new pos3D[baseline_mm.Length];
pos3D[] relative_right_cam = new pos3D[baseline_mm.Length];
for (int cam = 0; cam < baseline_mm.Length; cam++)
{
occupancygridBase.calculateCameraPositions(
body_width_mm,
body_length_mm,
body_centre_of_rotation_x,
body_centre_of_rotation_y,
body_centre_of_rotation_z,
head_centroid_x,
head_centroid_y,
head_centroid_z,
head_pan,
head_tilt,
head_roll,
baseline_mm[cam],
stereo_camera_position_x[cam],
stereo_camera_position_y[cam],
stereo_camera_position_z[cam],
stereo_camera_pan[cam],
stereo_camera_tilt[cam],
stereo_camera_roll[cam],
ref head_location,
ref camera_centre_location[cam],
ref relative_left_cam[cam],
ref relative_right_cam[cam]);
left_camera_location[cam] = relative_left_cam[cam].translate(robot_pose[cam].x, robot_pose[cam].y, robot_pose[cam].z);
right_camera_location[cam] = relative_right_cam[cam].translate(robot_pose[cam].x, robot_pose[cam].y, robot_pose[cam].z);
}
pose_score.Clear();
for (int p = 0; p < poses.Count; p++)
{
pose_score.Add(0);
}
int no_of_zero_probabilities = 0;
// try a number of random poses
// we can do this in parallel
Parallel.For(0, poses.Count, delegate(int i)
//for (int i = 0; i < poses.Count; i++)
{
pos3D sample_pose = poses[i];
float matching_score = 0;
for (int cam = 0; cam < baseline_mm.Length; cam++)
{
// update the position of the left camera for this pose
pos3D sample_pose_left_cam = relative_left_cam[cam].add(sample_pose);
sample_pose_left_cam.pan = 0;
sample_pose_left_cam.tilt = 0;
sample_pose_left_cam.roll = 0;
sample_pose_left_cam = sample_pose_left_cam.rotate(sample_pose.pan, sample_pose.tilt, sample_pose.roll);
sample_pose_left_cam.x += robot_pose[cam].x;
sample_pose_left_cam.y += robot_pose[cam].y;
sample_pose_left_cam.z += robot_pose[cam].z;
// update the position of the right camera for this pose
pos3D sample_pose_right_cam = relative_right_cam[cam].add(sample_pose);
sample_pose_right_cam.pan = 0;
sample_pose_right_cam.tilt = 0;
sample_pose_right_cam.roll = 0;
sample_pose_right_cam = sample_pose_right_cam.rotate(sample_pose.pan, sample_pose.tilt, sample_pose.roll);
sample_pose_right_cam.x += robot_pose[cam].x;
sample_pose_right_cam.y += robot_pose[cam].y;
sample_pose_right_cam.z += robot_pose[cam].z;
// centre position between the left and right cameras
pos3D stereo_camera_centre =
new pos3D(
sample_pose_left_cam.x + ((sample_pose_right_cam.x - sample_pose_left_cam.x) * 0.5f),
sample_pose_left_cam.y + ((sample_pose_right_cam.y - sample_pose_left_cam.y) * 0.5f),
sample_pose_left_cam.z + ((sample_pose_right_cam.z - sample_pose_left_cam.z) * 0.5f));
stereo_camera_centre.pan = head_pan + stereo_camera_pan[cam] + sample_pose.pan;
stereo_camera_centre.tilt = head_tilt + stereo_camera_tilt[cam] + sample_pose.tilt;
stereo_camera_centre.roll = head_roll + stereo_camera_roll[cam] + sample_pose.roll;
// create a set of stereo rays as observed from this pose
List <evidenceRay> rays = sensormodel[cam][0].createObservation(
stereo_camera_centre,
baseline_mm[cam],
image_width[cam],
image_height[cam],
FOV_degrees[cam],
stereo_features[cam],
stereo_features_colour[cam],
stereo_features_uncertainties[cam],
true);
// insert rays into the occupancy grid
for (int r = 0; r < rays.Count; r++)
{
float score = Insert(rays[r], sensormodel[cam], sample_pose_left_cam, sample_pose_right_cam, true);
if (score != occupancygridBase.NO_OCCUPANCY_EVIDENCE)
{
if (matching_score == occupancygridBase.NO_OCCUPANCY_EVIDENCE)
{
matching_score = 0;
}
matching_score += score;
}
}
}
// add the pose to the list
sample_pose.pan -= robot_pose[0].pan;
sample_pose.tilt -= robot_pose[0].tilt;
sample_pose.roll -= robot_pose[0].roll;
if (Math.Abs(sample_pose.pan) > max_orientation_variance)
{
Console.WriteLine("Pose variance out of range");
}
if (matching_score != occupancygridBase.NO_OCCUPANCY_EVIDENCE)
{
float prob = probabilities.LogOddsToProbability(matching_score);
if (prob == 0)
{
no_of_zero_probabilities++;
}
pose_score[i] = prob;
//pose_score[i] = matching_score;
}
});
if (no_of_zero_probabilities == poses.Count)
{
Console.WriteLine("Localisation failure");
pose_offset = new pos3D(0, 0, 0);
best_matching_score = occupancygridBase.NO_OCCUPANCY_EVIDENCE;
}
else
{
// locate the best possible pose
pos3D best_robot_pose = new pos3D(0, 0, 0);
gridCells.FindBestPose(poses, pose_score, ref best_robot_pose, sampling_radius_major_mm, img_poses, null, img_poses_width, img_poses_height, known_best_pose_x_mm, known_best_pose_y_mm);
// rotate the best pose to the robot's current orientation
// this becomes an offset, which may be used for course correction
pose_offset = best_robot_pose.rotate(robot_pose[0].pan, robot_pose[0].tilt, robot_pose[0].roll);
// orientation relative to the current heading
pose_offset.pan = best_robot_pose.pan;
pose_offset.tilt = best_robot_pose.tilt;
pose_offset.roll = best_robot_pose.roll;
// range checks
if (Math.Abs(pose_offset.pan) > max_orientation_variance)
{
Console.WriteLine("pose_offset pan out of range");
}
if (Math.Abs(pose_offset.tilt) > max_tilt_variance)
{
Console.WriteLine("pose_offset tilt out of range");
}
if (Math.Abs(pose_offset.roll) > max_roll_variance)
{
Console.WriteLine("pose_offset roll out of range");
}
}
return(best_matching_score);
}
