public void AddPoints()
{
pos3D[] point = new pos3D[3];
for (int i = 0; i < 2; i++)
{
point[i] = new pos3D(0, 0, 0);
}
point[0].x = 10;
point[0].y = 20;
point[0].z = 30;
point[0].pan = 1.4f;
point[0].tilt = 0.01f;
point[0].roll = 0.002f;
point[1].x = 40;
point[1].y = 50;
point[1].z = 60;
point[1].pan = 2.1f;
point[1].tilt = 0.03f;
point[1].roll = 0.001f;
point[2] = point[0].add(point[1]);
Assert.AreEqual(10 + 40, point[2].x, "x");
Assert.AreEqual(20 + 50, point[2].y, "y");
Assert.AreEqual(30 + 60, point[2].z, "z");
Assert.AreEqual(1.4f + 2.1f, point[2].pan, "pan");
Assert.AreEqual(0.01f + 0.03f, point[2].tilt, "tilt");
Assert.AreEqual(0.002f + 0.001f, point[2].roll, "roll");
}
public void SubtractPoints()
{
pos3D[] point = new pos3D[3];
for (int i = 0; i < 2; i++)
{
point[i] = new pos3D(0, 0, 0);
}
point[0].x = 10;
point[0].y = 20;
point[0].z = 30;
point[0].pan = 1.4f;
point[0].tilt = 0.01f;
point[0].roll = 0.002f;
point[1].x = 40;
point[1].y = 50;
point[1].z = 60;
point[1].pan = 2.1f;
point[1].tilt = 0.03f;
point[1].roll = 0.001f;
point[2] = point[0].subtract(point[1]);
Assert.AreEqual(10 - 40, point[2].x, "x");
Assert.AreEqual(20 - 50, point[2].y, "y");
Assert.AreEqual(30 - 60, point[2].z, "z");
Assert.AreEqual(1.4f - 2.1f, point[2].pan, "pan");
Assert.AreEqual(0.01f - 0.03f, point[2].tilt, "tilt");
Assert.AreEqual(0.002f - 0.001f, point[2].roll, "roll");
}
public void Tilt()
{
int tilt_angle1 = 30;
float tilt1 = tilt_angle1 * (float)Math.PI / 180.0f;
pos3D pos1 = new pos3D(0, 50, 0);
pos3D pos2 = pos1.rotate_old(0, tilt1, 0);
pos3D pos3 = pos1.rotate(0, 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);
}
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 ProbeView()
{
int map_dim = 14;
byte[] map =
{
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 1, 1, 1, 1, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 1, 1, 1, 0, 1, 1, 1, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};
dpslam sim = CreateSimulation(map_dim, map, 50);
int image_width = 320;
int image_height = 240;
float FOV_degrees = 90;
float max_range_mm = 2000;
pos3D camPose = new pos3D(0, 0, 0);
float x0_mm = 0;
float y0_mm = 0;
float z0_mm = 100;
float x1_mm = 0;
float y1_mm = 1500;
float z1_mm = -300;
bool use_ground_plane = true;
float range = sim.ProbeRange(
null,
x0_mm, y0_mm, z0_mm,
x1_mm, y1_mm, z1_mm,
use_ground_plane);
Assert.IsTrue(range > -1, "Ground plane was not detected");
Assert.IsTrue(range > 550);
Assert.IsTrue(range < 650);
int step_size = 10;
float[] range_buffer = new float[(image_width / step_size) * (image_height / step_size)];
sim.ProbeView(
null,
camPose.x, camPose.y, camPose.z,
camPose.pan, camPose.tilt, camPose.roll,
FOV_degrees,
image_width, image_height,
step_size,
max_range_mm, false,
range_buffer);
int ctr = 0;
for (int i = 0; i < range_buffer.Length; i++)
{
if (range_buffer[i] > -1)
{
ctr++;
}
//Console.WriteLine("Range: " + range_buffer[i].ToString());
}
Assert.IsTrue(ctr > 0, "No objects were ranged within the simulation");
}
public void FindBestPose()
{
int debug_img_width = 640;
int debug_img_height = 480;
byte[] debug_img_poses = new byte[debug_img_width * debug_img_height * 3];
byte[] debug_img_graph = new byte[debug_img_width * debug_img_height * 3];
Bitmap bmp = new Bitmap(debug_img_width, debug_img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
int no_of_poses = 400;
float sampling_radius_major_mm = 100;
float sampling_radius_minor_mm = 80;
float pan = 0;
float tilt = 0;
float roll = 0;
float max_orientation_variance = 5 * (float)Math.PI / 180.0f;
float max_tilt_variance = 0 * (float)Math.PI / 180.0f;
float max_roll_variance = 0 * (float)Math.PI / 180.0f;
Random rnd = new Random(81);
List <pos3D> poses = new List <pos3D>();
float ideal_best_pose_x = ((float)rnd.NextDouble() - 0.5f) * sampling_radius_minor_mm * 2 * 0.8f;
float ideal_best_pose_y = ((float)rnd.NextDouble() - 0.5f) * sampling_radius_major_mm * 2 * 0.8f;
float ideal_pan = ((float)rnd.NextDouble() - 0.5f) * (max_orientation_variance * 2);
gridCells.CreateMoireGrid(
sampling_radius_major_mm,
sampling_radius_minor_mm,
no_of_poses,
pan, tilt, roll,
max_orientation_variance,
max_tilt_variance,
max_roll_variance,
rnd,
ref poses,
null, null,
0, 0);
const int steps = 10;
for (int s = 0; s < steps; s++)
{
ideal_best_pose_x = sampling_radius_minor_mm * 0.3f;
ideal_best_pose_y = sampling_radius_major_mm * (-0.7f + (s * 1.4f / steps));
ideal_pan = 0;
List <float> scores = new List <float>();
for (int i = 0; i < poses.Count; i++)
{
float score = (float)rnd.NextDouble();
scores.Add(score);
float dx = poses[i].x - ideal_best_pose_x;
float dy = poses[i].y - ideal_best_pose_y;
float dp = (poses[i].pan - ideal_pan) * sampling_radius_major_mm / max_orientation_variance;
float dist = (float)Math.Sqrt(dx * dx + dy * dy + dp * dp) * 0.01f;
scores[i] += 2.0f / (1.0f + dist * dist);
}
pos3D best_pose = null;
gridCells.FindBestPose(
poses,
scores,
ref best_pose,
sampling_radius_major_mm,
debug_img_poses,
debug_img_graph,
debug_img_width,
debug_img_height,
ideal_best_pose_x,
ideal_best_pose_y);
float pan_angle_error = (ideal_pan - best_pose.pan) * 180 / (float)Math.PI;
Console.WriteLine("Target Pan angle: " + (ideal_pan * 180 / (float)Math.PI).ToString() + " degrees");
Console.WriteLine("Estimated Pan angle: " + (best_pose.pan * 180 / (float)Math.PI).ToString() + " degrees");
Console.WriteLine("Pan error: " + pan_angle_error.ToString() + " degrees");
float position_error_x = Math.Abs(ideal_best_pose_x - best_pose.x);
float position_error_y = Math.Abs(ideal_best_pose_y - best_pose.y);
Assert.Less(pan_angle_error, 1);
Assert.Less(position_error_x, sampling_radius_major_mm * 0.3f);
Assert.Less(position_error_y, sampling_radius_major_mm * 0.3f);
BitmapArrayConversions.updatebitmap_unsafe(debug_img_poses, bmp);
bmp.Save("tests_gridCells_FindBestPose" + s.ToString() + ".gif", System.Drawing.Imaging.ImageFormat.Gif);
BitmapArrayConversions.updatebitmap_unsafe(debug_img_graph, bmp);
bmp.Save("tests_gridCells_FindBestPose_graph" + s.ToString() + ".bmp", System.Drawing.Imaging.ImageFormat.Bmp);
}
}
public void MoveToNextLocalGrid()
{
int no_of_grids = 2;
int grid_type = metagrid.TYPE_SIMPLE;
int dimension_mm = 3000;
int dimension_vertical_mm = 2000;
int cellSize_mm = 32;
int localisationRadius_mm = 2000;
int maxMappingRange_mm = 2000;
float vacancyWeighting = 0.5f;
int overall_img_width = 640;
int overall_img_height = 480;
int current_grid_index = 0;
int current_disparity_index = 0;
pos3D robot_pose = new pos3D(0, 0, 0);
metagrid[] buffer = new metagrid[2];
int current_buffer_index = 0;
List <float> grid_centres = new List <float>();
bool update_map = false;
float grid_centre_x_mm;
float grid_centre_y_mm;
float grid_centre_z_mm;
// create some grid centres along a straight line path
float path_length_mm = 10000;
int steps = (int)(path_length_mm / (dimension_mm / 2));
for (int i = 0; i < steps; i++)
{
grid_centre_x_mm = 0;
grid_centre_y_mm = i * path_length_mm / steps;
grid_centre_z_mm = 0;
grid_centres.Add(grid_centre_x_mm);
grid_centres.Add(grid_centre_y_mm);
grid_centres.Add(grid_centre_z_mm);
}
// create the buffer
for (int i = 0; i < 2; i++)
{
buffer[i] = new metagrid(
no_of_grids,
grid_type,
dimension_mm,
dimension_vertical_mm,
cellSize_mm,
localisationRadius_mm,
maxMappingRange_mm,
vacancyWeighting);
}
// move along a straight line path
int transitions = 0;
for (int y = 0; y < path_length_mm; y += 100)
{
robot_pose.y = y;
byte[] overall_map_img = null;
if (metagridBuffer.MoveToNextLocalGrid(
ref current_grid_index,
ref current_disparity_index,
robot_pose,
buffer,
ref current_buffer_index,
grid_centres,
ref update_map,
null,
null,
ref overall_map_img,
overall_img_width,
overall_img_height,
0, 0, 0))
{
;
}
{
transitions++;
}
current_disparity_index = 1;
}
Assert.AreEqual(steps - 1, transitions, "Incorrect number of local grid transitions");
Assert.AreEqual(steps - 1, current_grid_index, "Did not reach the final grid");
}
public void LocaliseAlongPath()
{
// systematic bias
float bias_x_mm = -200;
float bias_y_mm = 0;
// number of stereo cameras on the robot's head
int no_of_stereo_cameras = 2;
// diameter of the robot's head
float head_diameter_mm = 100;
// number of stereo features per step during mapping
int no_of_mapping_stereo_features = 300;
// number of stereo features observed per localisation step
int no_of_localisation_stereo_features = 100;
string filename = "localise_along_path.dat";
float path_length_mm = 20000;
float start_orientation = 0;
float end_orientation = 0; // 90 * (float)Math.PI / 180.0f;
float distance_between_poses_mm = 100;
float disparity = 15;
string overall_map_filename = "overall_map.jpg";
byte[] overall_map_img = null;
int overall_img_width = 640;
int overall_img_height = 480;
int overall_map_dimension_mm = 0;
int overall_map_centre_x_mm = 0;
int overall_map_centre_y_mm = 0;
string[] str = filename.Split('.');
List <OdometryData> path = null;
SavePath(
filename,
path_length_mm,
start_orientation,
end_orientation,
distance_between_poses_mm,
disparity,
no_of_mapping_stereo_features,
no_of_stereo_cameras,
ref path);
Assert.AreEqual(true, File.Exists(filename));
Assert.AreEqual(true, File.Exists(str[0] + "_disparities_index.dat"));
Assert.AreEqual(true, File.Exists(str[0] + "_disparities.dat"));
int no_of_grids = 1;
int grid_type = metagrid.TYPE_SIMPLE;
int dimension_mm = 8000;
int dimension_vertical_mm = 2000;
int cellSize_mm = 50;
int localisationRadius_mm = 8000;
int maxMappingRange_mm = 10000;
float vacancyWeighting = 0.5f;
metagridBuffer buffer =
new metagridBuffer(
no_of_grids,
grid_type,
dimension_mm,
dimension_vertical_mm,
cellSize_mm,
localisationRadius_mm,
maxMappingRange_mm,
vacancyWeighting);
buffer.LoadPath(
filename,
str[0] + "_disparities_index.dat",
str[0] + "_disparities.dat",
ref overall_map_dimension_mm,
ref overall_map_centre_x_mm,
ref overall_map_centre_y_mm);
int img_width = 640;
int img_height = 640;
byte[] img = new byte[img_width * img_height * 3];
Bitmap bmp = new Bitmap(img_width, img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
buffer.ShowPath(img, img_width, img_height, true, true);
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
bmp.Save("localise_along_path.jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
robotGeometry geom = new robotGeometry();
geom.CreateStereoCameras(
no_of_stereo_cameras, 120, 0,
320, 240,
65,
head_diameter_mm,
0);
geom.CreateSensorModels(buffer);
Random rnd = new Random(0);
pos3D pose_offset = null;
bool buffer_transition = false;
float[][] stereo_features = new float[no_of_stereo_cameras][];
byte[][,] stereo_features_colour = new byte[no_of_stereo_cameras][, ];
float[][] stereo_features_uncertainties = new float[no_of_stereo_cameras][];
for (int i = 0; i < no_of_stereo_cameras; i++)
{
stereo_features_uncertainties[i] = new float[no_of_localisation_stereo_features];
for (int j = 0; j < no_of_localisation_stereo_features; j++)
{
stereo_features_uncertainties[i][j] = 1;
}
}
float average_offset_x_mm = 0;
float average_offset_y_mm = 0;
List <OdometryData> estimated_path = new List <OdometryData>();
int no_of_localisation_failures = 0;
for (int i = 0; i < path.Count - 1; i += 5)
{
string debug_mapping_filename = "localise_along_path_map_" + i.ToString() + ".jpg";
OdometryData p0 = path[i];
OdometryData p1 = path[i + 1];
// create an intermediate pose
for (int cam = 0; cam < no_of_stereo_cameras; cam++)
{
geom.pose[cam].x = p0.x + ((p1.x - p0.x) / 2) + bias_x_mm;
geom.pose[cam].y = p0.y + ((p1.y - p0.y) / 2) + bias_y_mm;
geom.pose[cam].z = 0;
geom.pose[cam].pan = p0.orientation + ((p1.orientation - p0.orientation) / 2);
// create stereo features
int ctr = 0;
stereo_features[cam] = new float[no_of_localisation_stereo_features * 3];
stereo_features_colour[cam] = new byte[no_of_localisation_stereo_features, 3];
for (int f = 0; f < no_of_localisation_stereo_features; f += 5)
{
if (f < no_of_localisation_stereo_features / 2)
{
stereo_features[cam][ctr++] = 20;
stereo_features[cam][ctr++] = rnd.Next(239);
}
else
{
stereo_features[cam][ctr++] = geom.image_width[cam] - 20;
stereo_features[cam][ctr++] = rnd.Next(239);
}
stereo_features[cam][ctr++] = disparity;
}
}
float matching_score = buffer.Localise(
geom,
stereo_features,
stereo_features_colour,
stereo_features_uncertainties,
rnd,
ref pose_offset,
ref buffer_transition,
debug_mapping_filename,
bias_x_mm, bias_y_mm,
overall_map_filename,
ref overall_map_img,
overall_img_width,
overall_img_height,
overall_map_dimension_mm,
overall_map_centre_x_mm,
overall_map_centre_y_mm);
if (matching_score != occupancygridBase.NO_OCCUPANCY_EVIDENCE)
{
Console.WriteLine("pose_offset (mm): " + pose_offset.x.ToString() + ", " + pose_offset.y.ToString() + ", " + pose_offset.pan.ToString());
OdometryData estimated_pose = new OdometryData();
estimated_pose.x = geom.pose[0].x + pose_offset.x;
estimated_pose.y = geom.pose[0].y + pose_offset.y;
estimated_pose.orientation = geom.pose[0].pan + pose_offset.pan;
estimated_path.Add(estimated_pose);
average_offset_x_mm += pose_offset.x;
average_offset_y_mm += pose_offset.y;
}
else
{
// fail!
no_of_localisation_failures++;
Console.WriteLine("Localisation failure");
}
}
buffer.ShowPath(img, img_width, img_height, true, true);
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
bmp.Save("localisations_along_path.jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
average_offset_x_mm /= estimated_path.Count;
average_offset_y_mm /= estimated_path.Count;
Console.WriteLine("Average offsets: " + average_offset_x_mm.ToString() + ", " + average_offset_y_mm.ToString());
float diff_x_mm = Math.Abs(average_offset_x_mm - bias_x_mm);
float diff_y_mm = Math.Abs(average_offset_y_mm - bias_y_mm);
Assert.Less(diff_x_mm, cellSize_mm * 3 / 2, "x bias not detected");
Assert.Less(diff_y_mm, cellSize_mm * 3 / 2, "y bias not detected");
if (no_of_localisation_failures > 0)
{
Console.WriteLine("Localisation failures: " + no_of_localisation_failures.ToString());
}
else
{
Console.WriteLine("No localisation failures!");
}
Assert.Less(no_of_localisation_failures, 4, "Too many localisation failures");
}
public void CreateObservation()
{
float baseline = 120;
int image_width = 320;
int image_height = 240;
float FOV_degrees = 68;
int no_of_stereo_features = 10;
float[] stereo_features = new float[no_of_stereo_features * 4];
byte[] stereo_features_colour = new byte[no_of_stereo_features * 3];
bool translate = false;
for (int i = 0; i < no_of_stereo_features; i++)
{
stereo_features[i * 4] = 1;
stereo_features[i * 4 + 1] = i * image_width / no_of_stereo_features;
stereo_features[i * 4 + 2] = image_height / 2;
stereo_features[i * 4 + 3] = 1;
stereo_features_colour[i * 3] = 200;
stereo_features_colour[i * 3 + 1] = 200;
stereo_features_colour[i * 3 + 2] = 200;
}
for (int rotation_degrees = 0; rotation_degrees < 360; rotation_degrees += 90)
{
stereoModel model = new stereoModel();
pos3D observer = new pos3D(0, 0, 0);
observer = observer.rotate(rotation_degrees / 180.0f * (float)Math.PI, 0, 0);
List <evidenceRay> rays = model.createObservation(
observer,
baseline,
image_width,
image_height,
FOV_degrees,
stereo_features,
stereo_features_colour,
translate);
float tx = float.MaxValue;
float ty = float.MaxValue;
float bx = float.MinValue;
float by = float.MinValue;
for (int i = 0; i < no_of_stereo_features; i++)
{
//float pan_degrees = rays[i].pan_angle * 180 / (float)Math.PI;
//Console.WriteLine(pan_degrees.ToString());
for (int j = 0; j < rays[i].vertices.Length; j++)
{
Console.WriteLine("Vertex " + j.ToString());
Console.WriteLine("xyz: " + rays[i].vertices[j].x.ToString() + " " + rays[i].vertices[j].y.ToString() + " " + rays[i].vertices[j].z.ToString());
if (rays[i].vertices[j].x < tx)
{
tx = rays[i].vertices[j].x;
}
if (rays[i].vertices[j].x > bx)
{
bx = rays[i].vertices[j].x;
}
if (rays[i].vertices[j].y < ty)
{
ty = rays[i].vertices[j].y;
}
if (rays[i].vertices[j].y > by)
{
by = rays[i].vertices[j].y;
}
}
}
int img_width = 640;
Bitmap bmp = new Bitmap(img_width, img_width, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
byte[] img = new byte[img_width * img_width * 3];
for (int i = 0; i < img.Length; i++)
{
img[i] = 255;
}
for (int i = 0; i < no_of_stereo_features; i++)
{
int x0 = (int)((rays[i].vertices[0].x - tx) * img_width / (bx - tx));
int y0 = (int)((rays[i].vertices[0].y - ty) * img_width / (by - ty));
int x1 = (int)((rays[i].vertices[1].x - tx) * img_width / (bx - tx));
int y1 = (int)((rays[i].vertices[1].y - ty) * img_width / (by - ty));
drawing.drawLine(img, img_width, img_width, x0, y0, x1, y1, 0, 0, 0, 0, false);
}
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
bmp.Save("dpslam_tests_createobservation_" + rotation_degrees.ToString() + ".bmp", System.Drawing.Imaging.ImageFormat.Bmp);
Console.WriteLine("dpslam_tests_createobservation_" + rotation_degrees.ToString() + ".bmp");
}
}
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);
}
public void EvidenceRayRotation()
{
int debug_img_width = 640;
int debug_img_height = 480;
byte[] debug_img = new byte[debug_img_width * debug_img_height * 3];
for (int i = (debug_img_width * debug_img_height * 3) - 1; i >= 0; i--)
{
debug_img[i] = 255;
}
Bitmap bmp = new Bitmap(debug_img_width, debug_img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
int cellSize_mm = 32;
int image_width = 320;
int image_height = 240;
Console.WriteLine("Creating sensor models");
stereoModel inverseSensorModel = new stereoModel();
inverseSensorModel.createLookupTable(cellSize_mm, image_width, image_height);
// create a ray
float FOV_horizontal = 78 * (float)Math.PI / 180.0f;
inverseSensorModel.FOV_horizontal = FOV_horizontal;
inverseSensorModel.FOV_vertical = FOV_horizontal * image_height / image_width;
evidenceRay ray =
inverseSensorModel.createRay(
image_width / 2, image_height / 2, 4,
0, 255, 255, 255);
Assert.AreNotEqual(null, ray, "No ray was created");
Assert.AreNotEqual(null, ray.vertices, "No ray vertices were created");
pos3D[] start_vertices = (pos3D[])ray.vertices.Clone();
Console.WriteLine("x,y,z: " + start_vertices[0].x.ToString() + ", " + start_vertices[0].y.ToString() + ", " + start_vertices[0].z.ToString());
for (int i = 0; i < ray.vertices.Length; i++)
{
int j = i + 1;
if (j == ray.vertices.Length)
{
j = 0;
}
int x0 = (debug_img_width / 2) + (int)ray.vertices[i].x / 50;
int y0 = (debug_img_height / 2) + (int)ray.vertices[i].y / 50;
int x1 = (debug_img_width / 2) + (int)ray.vertices[j].x / 50;
int y1 = (debug_img_height / 2) + (int)ray.vertices[j].y / 50;
drawing.drawLine(debug_img, debug_img_width, debug_img_height, x0, y0, x1, y1, 0, 255, 0, 0, false);
}
float angle_degrees = 30;
float angle_radians = angle_degrees / 180.0f * (float)Math.PI;
pos3D rotation = new pos3D(0, 0, 0);
rotation.pan = angle_degrees;
ray.translateRotate(rotation);
Console.WriteLine("x,y,z: " + ray.vertices[0].x.ToString() + ", " + ray.vertices[0].y.ToString() + ", " + ray.vertices[0].z.ToString());
for (int i = 0; i < ray.vertices.Length; i++)
{
int j = i + 1;
if (j == ray.vertices.Length)
{
j = 0;
}
int x0 = (debug_img_width / 2) + (int)ray.vertices[i].x / 50;
int y0 = (debug_img_height / 2) + (int)ray.vertices[i].y / 50;
int x1 = (debug_img_width / 2) + (int)ray.vertices[j].x / 50;
int y1 = (debug_img_height / 2) + (int)ray.vertices[j].y / 50;
drawing.drawLine(debug_img, debug_img_width, debug_img_height, x0, y0, x1, y1, 255, 0, 0, 0, false);
}
BitmapArrayConversions.updatebitmap_unsafe(debug_img, bmp);
bmp.Save("tests_occupancygrid_simple_EvidenceRayRotation.jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
}
public void LocaliseAlongPath()
{
// systematic bias
float bias_x_mm = -200;
float bias_y_mm = 0;
// number of stereo features per step during mapping
int no_of_mapping_stereo_features = 300;
// number of stereo features observed per localisation step
int no_of_localisation_stereo_features = 50;
string filename = "steersman_localise_along_path.dat";
float path_length_mm = 10000;
float start_orientation = 0;
float end_orientation = 0; //90 * (float)Math.PI / 180.0f;
float distance_between_poses_mm = 100;
float disparity = 15;
string steersman_filename = "tests_steersman_LocaliseAlongPath.xml";
int body_width_mm = 465;
int body_length_mm = 380;
int body_height_mm = 1660;
int centre_of_rotation_x = body_width_mm / 2;
int centre_of_rotation_y = body_length_mm / 2;
int centre_of_rotation_z = 10;
int head_centroid_x = body_width_mm / 2;
int head_centroid_y = 65;
int head_centroid_z = 1600;
string sensormodels_filename = "";
int no_of_stereo_cameras = 2;
float baseline_mm = 120;
float dist_to_centre_of_tilt_mm = 0;
int image_width = 320;
int image_height = 240;
float FOV_degrees = 65;
float head_diameter_mm = 160;
float default_head_orientation_degrees = 0;
int no_of_grid_levels = 1;
int dimension_mm = 8000;
int dimension_vertical_mm = 2000;
int cellSize_mm = 50;
string[] str = filename.Split('.');
List <OdometryData> path = null;
tests_metagridbuffer.SavePath(
filename,
path_length_mm,
start_orientation,
end_orientation,
distance_between_poses_mm,
disparity,
no_of_mapping_stereo_features,
no_of_stereo_cameras,
ref path);
Assert.AreEqual(true, File.Exists(filename));
Assert.AreEqual(true, File.Exists(str[0] + "_disparities_index.dat"));
Assert.AreEqual(true, File.Exists(str[0] + "_disparities.dat"));
steersman visual_guidance = null;
if (File.Exists(steersman_filename))
{
visual_guidance = new steersman();
visual_guidance.Load(steersman_filename);
}
else
{
visual_guidance = new steersman(
body_width_mm,
body_length_mm,
body_height_mm,
centre_of_rotation_x,
centre_of_rotation_y,
centre_of_rotation_z,
head_centroid_x,
head_centroid_y,
head_centroid_z,
sensormodels_filename,
no_of_stereo_cameras,
baseline_mm,
dist_to_centre_of_tilt_mm,
image_width,
image_height,
FOV_degrees,
head_diameter_mm,
default_head_orientation_degrees,
no_of_grid_levels,
dimension_mm,
dimension_vertical_mm,
cellSize_mm);
visual_guidance.Save(steersman_filename);
}
visual_guidance.LoadPath(filename, str[0] + "_disparities_index.dat", str[0] + "_disparities.dat");
visual_guidance.ShowLocalisations("steersman_localise_along_path.jpg", 640, 480);
Random rnd = new Random(0);
pos3D pose_offset = null;
bool buffer_transition = false;
float[][] stereo_features = new float[no_of_stereo_cameras][];
byte[][,] stereo_features_colour = new byte[no_of_stereo_cameras][, ];
float[][] stereo_features_uncertainties = new float[no_of_stereo_cameras][];
for (int i = 0; i < no_of_stereo_cameras; i++)
{
stereo_features_uncertainties[i] = new float[no_of_localisation_stereo_features];
for (int j = 0; j < no_of_localisation_stereo_features; j++)
{
stereo_features_uncertainties[i][j] = 1;
}
}
float average_offset_x_mm = 0;
float average_offset_y_mm = 0;
List <OdometryData> estimated_path = new List <OdometryData>();
int no_of_localisation_failures = 0;
DateTime start_time = DateTime.Now;
int no_of_localisations = 0;
for (int i = 0; i < path.Count - 1; i += 5, no_of_localisations++)
{
string debug_mapping_filename = "steersman_localise_along_path_map_" + i.ToString() + ".jpg";
OdometryData p0 = path[i];
OdometryData p1 = path[i + 1];
float current_x_mm = p0.x + ((p1.x - p0.x) / 2) + bias_x_mm;
float current_y_mm = p0.y + ((p1.y - p0.y) / 2) + bias_y_mm;
float current_pan = p0.orientation + ((p1.orientation - p0.orientation) / 2);
// create an intermediate pose
for (int cam = 0; cam < no_of_stereo_cameras; cam++)
{
// set the current pose
visual_guidance.SetCurrentPosition(
cam,
current_x_mm,
current_y_mm,
0,
current_pan,
0, 0);
// create stereo features
int ctr = 0;
stereo_features[cam] = new float[no_of_localisation_stereo_features * 3];
stereo_features_colour[cam] = new byte[no_of_localisation_stereo_features, 3];
for (int f = 0; f < no_of_localisation_stereo_features; f += 5)
{
if (f < no_of_localisation_stereo_features / 2)
{
stereo_features[cam][ctr++] = 20;
stereo_features[cam][ctr++] = rnd.Next(239);
}
else
{
stereo_features[cam][ctr++] = image_width - 20;
stereo_features[cam][ctr++] = rnd.Next(239);
}
stereo_features[cam][ctr++] = disparity;
}
}
float offset_x_mm = 0;
float offset_y_mm = 0;
float offset_z_mm = 0;
float offset_pan = 0;
float offset_tilt = 0;
float offset_roll = 0;
bool valid_localisation = visual_guidance.Localise(
stereo_features,
stereo_features_colour,
stereo_features_uncertainties,
debug_mapping_filename,
bias_x_mm, bias_y_mm,
ref offset_x_mm,
ref offset_y_mm,
ref offset_z_mm,
ref offset_pan,
ref offset_tilt,
ref offset_roll);
if (valid_localisation)
{
Console.WriteLine("pose_offset (mm): " + offset_x_mm.ToString() + ", " + offset_y_mm.ToString() + ", " + offset_pan.ToString());
OdometryData estimated_pose = new OdometryData();
estimated_pose.x = current_x_mm + offset_x_mm;
estimated_pose.y = current_y_mm + offset_y_mm;
estimated_pose.orientation = current_pan + offset_pan;
estimated_path.Add(estimated_pose);
average_offset_x_mm += offset_x_mm;
average_offset_y_mm += offset_y_mm;
}
else
{
// fail!
no_of_localisation_failures++;
Console.WriteLine("Localisation failure");
}
}
TimeSpan diff = DateTime.Now.Subtract(start_time);
float time_per_localisation_sec = (float)diff.TotalSeconds / no_of_localisations;
Console.WriteLine("Time per localisation: " + time_per_localisation_sec.ToString() + " sec");
visual_guidance.ShowLocalisations("steersman_localisations_along_path.jpg", 640, 480);
average_offset_x_mm /= estimated_path.Count;
average_offset_y_mm /= estimated_path.Count;
Console.WriteLine("Average offsets: " + average_offset_x_mm.ToString() + ", " + average_offset_y_mm.ToString());
float diff_x_mm = Math.Abs(average_offset_x_mm - bias_x_mm);
float diff_y_mm = Math.Abs(average_offset_y_mm - bias_y_mm);
Assert.Less(diff_x_mm, cellSize_mm * 3 / 2, "x bias not detected");
Assert.Less(diff_y_mm, cellSize_mm * 3 / 2, "y bias not detected");
if (no_of_localisation_failures > 0)
{
Console.WriteLine("Localisation failures: " + no_of_localisation_failures.ToString());
}
else
{
Console.WriteLine("No localisation failures!");
}
Assert.Less(no_of_localisation_failures, 4, "Too many localisation failures");
}
