public static void ShowGaborFilter(string filename, int image_width, int image_height,
double a, double b, double frequency,
double phase_degrees, double orientation_degrees)
{
Bitmap bmp = new Bitmap(image_width, image_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
byte[] img = new byte[image_width * image_height * 3];
ShowGaborFilter(img, image_width, image_height,
0, 0, image_width - 1, image_height - 1,
a, b, frequency, phase_degrees, orientation_degrees);
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
if (filename.ToLower().EndsWith("jpg"))
{
bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Jpeg);
}
if (filename.ToLower().EndsWith("bmp"))
{
bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Bmp);
}
if (filename.ToLower().EndsWith("png"))
{
bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Png);
}
if (filename.ToLower().EndsWith("gif"))
{
bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Gif);
}
}
/// <summary>
/// retrieves a set of stereo images for the given time step
/// filenames should be in the format "camera_0_left_2.jpg" where
/// the first number is the index of the stereo camera and the second
/// number is the time step upon which the image was taken
/// </summary>
/// <param name="time_step"></param>
/// <returns></returns>
private ArrayList getStereoImages(int time_step)
{
ArrayList images = new ArrayList();
// get the file names for the left and right images for this time step
String[] left_file = Directory.GetFiles(sim.ImagesPath, "*left_" + Convert.ToString(time_step + 1) + ".jpg");
String[] right_file = Directory.GetFiles(sim.ImagesPath, "*right_" + Convert.ToString(time_step + 1) + ".jpg");
for (int i = 0; i < left_file.Length; i++)
{
// load the images as bitmap objects
Bitmap left_image = new Bitmap(left_file[i]);
Bitmap right_image = new Bitmap(right_file[i]);
// extract the raw byte arrays
Byte[] left_bmp = BitmapArrayConversions.updatebitmap_unsafe(left_image);
Byte[] right_bmp = BitmapArrayConversions.updatebitmap_unsafe(right_image);
// put the byte arrays into the list of results
images.Add(left_bmp);
images.Add(right_bmp);
// show the current images
// this is a good way of checking that updatebitmap returned a valid byte array
picLeftImage.Image = new Bitmap(left_image.Width, left_image.Height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
picRightImage.Image = new Bitmap(right_image.Width, right_image.Height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
BitmapArrayConversions.updatebitmap_unsafe(left_bmp, (Bitmap)(picLeftImage.Image));
BitmapArrayConversions.updatebitmap_unsafe(right_bmp, (Bitmap)(picRightImage.Image));
}
return(images);
}
/// <summary>
/// display the path segments within the list view
/// </summary>
private void showPathSegments()
{
// add items to the list
lstPathSegments.Items.Clear();
for (int i = 0; i < sim.pathSegments.Count; i++)
{
simulationPathSegment segment = (simulationPathSegment)sim.pathSegments[i];
ListViewItem result = new ListViewItem(new string[]
{ Convert.ToString(segment.x),
Convert.ToString(segment.y),
Convert.ToString(segment.pan * 180.0f / (float)Math.PI),
Convert.ToString(segment.no_of_steps),
Convert.ToString(segment.distance_per_step_mm),
Convert.ToString(segment.pan_per_step * 180.0f / (float)Math.PI) });
lstPathSegments.Items.Add(result);
}
// create an image to display the path
picPath.Image = new Bitmap(sim.results_image_width, sim.results_image_height,
System.Drawing.Imaging.PixelFormat.Format24bppRgb);
Byte[] path_img = new Byte[sim.results_image_width * sim.results_image_height * 3];
sim.ShowPath(path_img, sim.results_image_width, sim.results_image_height);
BitmapArrayConversions.updatebitmap_unsafe(path_img, (Bitmap)(picPath.Image));
}
public void Show(
string filename)
{
int image_width = 320;
int image_height = 240;
byte[] img = new byte[image_width * image_height * 3];
Show(image_width, image_height, img);
Bitmap bmp = new Bitmap(image_width, image_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
if (filename.EndsWith("bmp"))
{
bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Bmp);
}
if (filename.EndsWith("png"))
{
bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Png);
}
if (filename.EndsWith("gif"))
{
bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Gif);
}
if (filename.EndsWith("jpg"))
{
bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Jpeg);
}
}
public void ShowLocalisations(
string image_filename,
int img_width,
int img_height)
{
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);
if (image_filename.ToLower().EndsWith("gif"))
{
bmp.Save(image_filename, System.Drawing.Imaging.ImageFormat.Gif);
}
if (image_filename.ToLower().EndsWith("png"))
{
bmp.Save(image_filename, System.Drawing.Imaging.ImageFormat.Png);
}
if (image_filename.ToLower().EndsWith("bmp"))
{
bmp.Save(image_filename, System.Drawing.Imaging.ImageFormat.Bmp);
}
if (image_filename.ToLower().EndsWith("jpg"))
{
bmp.Save(image_filename, System.Drawing.Imaging.ImageFormat.Jpeg);
}
}
private Bitmap OverlayImage(Bitmap bmp, Bitmap overlay, int overlay_width)
{
usage.Update("Overlay image, SurveyorVisionStereoGtk, OverlayImage");
Bitmap result = (Bitmap)bmp.Clone();
byte[] image_data = new byte[result.Width * result.Height * 3];
BitmapArrayConversions.updatebitmap(result, image_data);
byte[] overlay_data = new byte[overlay.Width * overlay.Height * 3];
BitmapArrayConversions.updatebitmap(overlay, overlay_data);
int overlay_height = overlay.Height * overlay_width / overlay.Width;
for (int y = 0; y < overlay_height; y++)
{
int yy = y * overlay.Height / overlay_height;
for (int x = 0; x < overlay_width; x++)
{
int xx = x * overlay.Width / overlay_width;
int n1 = ((yy * overlay.Width) + xx) * 3;
int n2 = ((y * result.Width) + x) * 3;
for (int col = 0; col < 3; col++)
{
image_data[n2 + col] = overlay_data[n1 + col];
}
}
}
BitmapArrayConversions.updatebitmap_unsafe(image_data, result);
return(result);
}
public void VacancyFunction()
{
float min_probability = 0;
float max_probability = 1;
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 = 0; i < debug_img.Length; i++)
{
debug_img[i] = 255;
}
Bitmap bmp = new Bitmap(debug_img_width, debug_img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
int prev_y = debug_img_height - 1;
for (int x = 0; x < debug_img_width; x++)
{
float prob = occupancygridBase.vacancyFunction(x / (float)debug_img_width, min_probability, max_probability);
Console.WriteLine(prob.ToString());
int y = debug_img_height - (int)(prob * (debug_img_height - 1) / max_probability);
if (x > 0)
{
drawing.drawLine(debug_img, debug_img_width, debug_img_height, x - 1, prev_y, x, y, 0, 0, 0, 0, false);
}
prev_y = y;
}
BitmapArrayConversions.updatebitmap_unsafe(debug_img, bmp);
bmp.Save("tests_occupancygrid_simple_VacancyFunction.jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
}
private void SaveImages(
string left_image_filename,
string right_image_filename)
{
byte[] left = new byte[image_width * image_height * 3];
byte[] right = new byte[image_width * image_height * 3];
GtkBitmap.getBitmap(leftimage, left);
GtkBitmap.getBitmap(rightimage, right);
Bitmap left_bmp = new Bitmap(image_width, image_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
Bitmap right_bmp = new Bitmap(image_width, image_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
BitmapArrayConversions.updatebitmap_unsafe(left, left_bmp);
BitmapArrayConversions.updatebitmap_unsafe(right, right_bmp);
if (left_image_filename.ToLower().EndsWith("png"))
{
left_bmp.Save(left_image_filename, System.Drawing.Imaging.ImageFormat.Png);
right_bmp.Save(right_image_filename, System.Drawing.Imaging.ImageFormat.Png);
}
if (left_image_filename.ToLower().EndsWith("bmp"))
{
left_bmp.Save(left_image_filename, System.Drawing.Imaging.ImageFormat.Bmp);
right_bmp.Save(right_image_filename, System.Drawing.Imaging.ImageFormat.Bmp);
}
if (left_image_filename.ToLower().EndsWith("gif"))
{
left_bmp.Save(left_image_filename, System.Drawing.Imaging.ImageFormat.Gif);
right_bmp.Save(right_image_filename, System.Drawing.Imaging.ImageFormat.Gif);
}
if (left_image_filename.ToLower().EndsWith("jpg"))
{
left_bmp.Save(left_image_filename, System.Drawing.Imaging.ImageFormat.Jpeg);
right_bmp.Save(right_image_filename, System.Drawing.Imaging.ImageFormat.Jpeg);
}
}
private void showPathTree()
{
picPathTree.Image = new Bitmap(640, 480,
System.Drawing.Imaging.PixelFormat.Format24bppRgb);
Byte[] tree_img = new Byte[640 * 480 * 3];
sim.ShowPathTree(tree_img, 640, 480);
BitmapArrayConversions.updatebitmap_unsafe(tree_img, (Bitmap)(picPathTree.Image));
}
/*
* private Bitmap OverlayImage(Bitmap bmp, Bitmap overlay, int overlay_width)
* {
* Bitmap result = (Bitmap)bmp.Clone();
* byte[] image_data = new byte[result.Width * result.Height * 3];
* BitmapArrayConversions.updatebitmap(result, image_data);
*
* byte[] overlay_data = new byte[overlay.Width * overlay.Height * 3];
* BitmapArrayConversions.updatebitmap(overlay, overlay_data);
*
* int overlay_height = overlay.Height * overlay_width / overlay.Width;
*
* for (int y = 0; y < overlay_height; y++)
* {
* int yy = y * overlay.Height / overlay_height;
* for (int x = 0; x < overlay_width; x++)
* {
* int xx = x * overlay.Width / overlay_width;
* int n1 = ((yy * overlay.Width) + xx) * 3;
* int n2 = ((y * result.Width) + x) * 3;
* for (int col = 0; col < 3; col++)
* image_data[n2 + col] = overlay_data[n1 + col];
* }
* }
*
* BitmapArrayConversions.updatebitmap_unsafe(image_data, result);
* return (result);
* }
*/
private void DisplayImage(Bitmap img, Bitmap default_image, bool is_left)
{
Bitmap disp_image = null;
switch (display_type)
{
case DISPLAY_RAW: { disp_image = default_image; break; }
case DISPLAY_CALIBRATION_DOTS: { disp_image = edges; break; }
case DISPLAY_CALIBRATION_GRID: { disp_image = linked_dots; break; }
case DISPLAY_CALIBRATION_DIFF: { disp_image = grid_diff; break; }
case DISPLAY_RECTIFIED: { if (is_left)
{
disp_image = rectified[0];
}
else
{
disp_image = rectified[1];
} break; }
}
if ((calibration_pattern == null) || (disp_image == null))
{
disp_image = default_image;
}
if (calibration_pattern == null)
{
if (is_left)
{
if (calibration_map[0] != null)
{
if (stereo_features == null)
{
disp_image = rectified[0];
}
else
{
disp_image = stereo_features;
}
}
}
else
{
if (calibration_map[1] != null)
{
disp_image = rectified[1];
}
}
}
byte[] image_data = new byte[disp_image.Width * disp_image.Height * 3];
BitmapArrayConversions.updatebitmap(disp_image, image_data);
BitmapArrayConversions.updatebitmap_unsafe(image_data, img);
}
/// <summary>
/// displays the occupancy grid
/// </summary>
private void showOccupancyGrid()
{
occupancygridMultiHypothesis grid = sim.rob.GetBestGrid();
picGridMap.Image = new Bitmap(grid.dimension_cells, grid.dimension_cells,
System.Drawing.Imaging.PixelFormat.Format24bppRgb);
Byte[] grid_img = new Byte[grid.dimension_cells * grid.dimension_cells * 3];
sim.ShowGrid(occupancygridMultiHypothesis.VIEW_ABOVE, grid_img, grid.dimension_cells, grid.dimension_cells, true);
BitmapArrayConversions.updatebitmap_unsafe(grid_img, (Bitmap)(picGridMap.Image));
}
/// <summary>
/// show the current best pose
/// </summary>
private void showBestPose()
{
int dimension_mm = sim.rob.GetBestGrid().dimension_cells;
picBestPose.Image = new Bitmap(dimension_mm, dimension_mm,
System.Drawing.Imaging.PixelFormat.Format24bppRgb);
Byte[] best_pose_img = new Byte[dimension_mm * dimension_mm * 3];
sim.ShowBestPose(best_pose_img, dimension_mm, dimension_mm, true);
BitmapArrayConversions.updatebitmap_unsafe(best_pose_img, (Bitmap)(picBestPose.Image));
}
/// <summary>
/// show stereo features or depth map
/// </summary>
/// <param name="output">bitmap object into which to insert the image</param>
public virtual void Show(ref Bitmap output)
{
byte[] output_img = new byte[image_width * image_height * 3];
if (output == null)
{
output = new Bitmap(image_width, image_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
}
Show(img[0], image_width, image_height, output_img);
BitmapArrayConversions.updatebitmap_unsafe(output_img, output);
}
public void MapUpdate()
{
int max_disparity_percent = 40;
int map_x = 60;
int map_y = 60;
int cell_size_mm = 100;
byte[] cartesian_map = new byte[map_x * map_y * 3];
int stereo_matches = 0;
CartesianMap map = new CartesianMap();
map.imgWidth = 320;
map.imgHeight = 240;
map.footline = new ushort[(int)map.imgWidth / map.SVS_HORIZONTAL_SAMPLING];
map.footline_dist_mm = new ushort[(int)map.imgWidth / map.SVS_HORIZONTAL_SAMPLING];
// create a footline
for (int x = 0; x < (int)map.imgWidth / map.SVS_HORIZONTAL_SAMPLING; x++)
{
map.footline[x] = (ushort)(map.imgHeight / 2);
}
map.FootlineUpdate(max_disparity_percent);
/* create a map */
drawing.drawLine(cartesian_map, map_x, map_y, (map_x / 2) - 2, (map_y / 2) - 6, (map_x / 2) - 2, (map_y / 2) - 29, 255, 0, 0, 0, false);
drawing.drawLine(cartesian_map, map_x, map_y, (map_x / 2) + 2, (map_y / 2) - 6, (map_x / 2) + 2, (map_y / 2) - 29, 255, 0, 0, 0, false);
drawing.drawLine(cartesian_map, map_x, map_y, (map_x / 2) - 2, (map_y / 2) - 6, (map_x / 2) - 10, (map_y / 2) - 6, 255, 0, 0, 0, false);
drawing.drawLine(cartesian_map, map_x, map_y, (map_x / 2) + 2, (map_y / 2) - 6, (map_x / 2) + 10, (map_y / 2) - 6, 255, 0, 0, 0, false);
Bitmap bmp = new Bitmap(map_x, map_y, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
BitmapArrayConversions.updatebitmap_unsafe(cartesian_map, bmp);
bmp.Save("CartesianMapTests_MapUpdate0.bmp", System.Drawing.Imaging.ImageFormat.Bmp);
/* convert map to stereo view */
map.SimulateStereoView(
max_disparity_percent,
map_x, map_y,
cell_size_mm,
cartesian_map,
ref stereo_matches);
map.SaveDisparities(
"CartesianMapTests_MapUpdate1.bmp",
stereo_matches);
map.MapUpdate(stereo_matches, 40);
map.Show("CartesianMapTests_MapUpdate2.bmp");
Assert.IsTrue(stereo_matches > 0);
}
/// <summary>
/// simple function used to test grid creation
/// </summary>
/// <param name="filename">filename of the test image to be created</param>
public static void Test(string filename)
{
int image_width = 640;
int image_height = 480;
int dimension_x = 10;
int dimension_y = 10;
Bitmap bmp = new Bitmap(image_width, image_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
byte[] img = new byte[image_width * image_height * 3];
polygon2D test_poly;
grid2D grid;
test_poly = new polygon2D();
test_poly.Add(image_width * 15 / 100, image_height * 20 / 100);
test_poly.Add(image_width * 40 / 100, image_height * 20 / 100);
test_poly.Add(image_width * 40 / 100, image_height * 40 / 100);
test_poly.Add(image_width * 15 / 100, image_height * 40 / 100);
grid = new grid2D(dimension_x, dimension_y, test_poly, 0, false);
grid.ShowLines(img, image_width, image_height, 0, 255, 0, 0);
int half_width = image_width / 2;
test_poly = new polygon2D();
test_poly.Add(half_width + (image_width * 20 / 100), image_height * 20 / 100);
test_poly.Add(half_width + (image_width * 35 / 100), image_height * 20 / 100);
test_poly.Add(half_width + (image_width * 40 / 100), image_height * 40 / 100);
test_poly.Add(half_width + (image_width * 15 / 100), image_height * 40 / 100);
grid = new grid2D(dimension_x, dimension_y, test_poly, 0, false);
grid.ShowLines(img, image_width, image_height, 0, 255, 0, 0);
int half_height = image_height / 2;
test_poly = new polygon2D();
test_poly.Add(image_width * 15 / 100, half_height + (image_height * 24 / 100));
test_poly.Add(image_width * 40 / 100, half_height + (image_height * 20 / 100));
test_poly.Add(image_width * 40 / 100, half_height + (image_height * 40 / 100));
test_poly.Add(image_width * 15 / 100, half_height + (image_height * 36 / 100));
grid = new grid2D(dimension_x, dimension_y, test_poly, 0, false);
grid.ShowLines(img, image_width, image_height, 0, 255, 0, 0);
test_poly = new polygon2D();
test_poly.Add(half_width + (image_width * 20 / 100), half_height + (image_height * 24 / 100));
test_poly.Add(half_width + (image_width * 35 / 100), half_height + (image_height * 20 / 100));
test_poly.Add(half_width + (image_width * 40 / 100), half_height + (image_height * 40 / 100));
test_poly.Add(half_width + (image_width * 15 / 100), half_height + (image_height * 36 / 100));
grid = new grid2D(dimension_x, dimension_y, test_poly, 0, false);
grid.ShowLines(img, image_width, image_height, 0, 255, 0, 0);
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
bmp.Save(filename);
}
public void CreateMoireGrid()
{
int no_of_poses = 200;
float sampling_radius_major_mm = 100;
float sampling_radius_minor_mm = 100;
float pan = 0;
float tilt = 0;
float roll = 0;
float max_orientation_variance = 5 * (float)Math.PI / 180;
float max_tilt_variance = 0 * (float)Math.PI / 180;
float max_roll_variance = 0 * (float)Math.PI / 180;
Random rnd = new Random(0);
List <pos3D> cells = new List <pos3D>();
int debug_img_width = 5000; //640;
int debug_img_height = 5000; //480;
byte[] debug_img = new byte[debug_img_width * debug_img_height * 3];
byte[] debug_img_moire = new byte[debug_img_width * debug_img_height * 3];
Bitmap bmp = new Bitmap(debug_img_width, debug_img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
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 cells,
debug_img,
debug_img_moire,
debug_img_width,
debug_img_height);
Assert.Greater(cells.Count, no_of_poses * 90 / 100);
Assert.Less(cells.Count, no_of_poses * 110 / 100);
BitmapArrayConversions.updatebitmap_unsafe(debug_img, bmp);
bmp.Save("tests_gridCells_CreateMoireGrid.bmp", System.Drawing.Imaging.ImageFormat.Bmp);
BitmapArrayConversions.updatebitmap_unsafe(debug_img_moire, bmp);
bmp.Save("tests_gridCells_CreateMoireGrid_moire.bmp", System.Drawing.Imaging.ImageFormat.Bmp);
}
public void SpeedControl()
{
int image_width = 640;
byte[] img_rays = new byte[image_width * image_width * 3];
Bitmap bmp = new Bitmap(image_width, image_width, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
bool closed_loop = false;
robot rob = new robot(1);
int min_x_mm = 0;
int min_y_mm = 0;
int max_x_mm = 1000;
int max_y_mm = 1200;
int step_size = (max_y_mm - min_y_mm) / 15;
int x = min_x_mm + ((max_x_mm - min_x_mm) / 2);
bool initial = true;
float pan = 0; // (float)Math.PI / 4;
rob.x = x;
rob.y = 0;
for (int y = min_y_mm; y <= max_y_mm; y += step_size)
{
if (closed_loop)
{
surveyPosesDummy(rob);
}
List <ushort[]> stereo_matches = new List <ushort[]>();
float forward_velocity = step_size;
float angular_velocity_pan = 0 * (float)Math.PI / 180.0f;
rob.updateFromVelocities(stereo_matches, forward_velocity, angular_velocity_pan, 0, 0, 1.0f);
rob.motion.Show(
img_rays, image_width, image_width,
min_x_mm, min_y_mm, max_x_mm, max_y_mm,
true, false,
initial);
initial = false;
}
BitmapArrayConversions.updatebitmap_unsafe(img_rays, bmp);
bmp.Save("motionmodel_tests_SpeedControl.bmp", System.Drawing.Imaging.ImageFormat.Bmp);
Assert.IsTrue(rob.y > max_y_mm - 50, "The robot did not move far enough " + rob.y.ToString());
}
public void OpenLoopSpeed()
{
int image_width = 640;
byte[] img_rays = new byte[image_width * image_width * 3];
Bitmap bmp = new Bitmap(image_width, image_width, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
bool closed_loop = false;
robot rob = new robot(1);
rob.motion.uncertainty_type = motionModel.SPEED;
rob.motion.speed_uncertainty_forward = 4;
rob.motion.speed_uncertainty_angular = 0.5f / 180.0f * (float)Math.PI;
int min_x_mm = 0;
int min_y_mm = 0;
int max_x_mm = 1000;
int max_y_mm = 1000;
int step_size = (max_y_mm - min_y_mm) / 15;
int x = min_x_mm + ((max_x_mm - min_x_mm) / 2);
bool initial = true;
float pan = 0; // (float)Math.PI / 4;
for (int y = min_y_mm; y <= max_y_mm; y += step_size)
{
if (closed_loop)
{
surveyPosesDummy(rob);
}
List <ushort[]> stereo_matches = new List <ushort[]>();
rob.updateFromKnownPosition(stereo_matches, x, y, 0, pan, 0, 0);
rob.motion.Show(
img_rays, image_width, image_width,
min_x_mm, min_y_mm, max_x_mm, max_y_mm,
true, false,
initial);
initial = false;
}
BitmapArrayConversions.updatebitmap_unsafe(img_rays, bmp);
bmp.Save("motionmodel_tests_OpenLoopSpeed.bmp", System.Drawing.Imaging.ImageFormat.Bmp);
Assert.IsTrue(rob.y > max_y_mm - 50, "The robot did not move far enough " + rob.y.ToString());
}
private void update()
{
// load calibration data
stereointerface.loadCalibration(calibration_filename);
// get image data from bitmaps
int bytes_per_pixel = 3;
int image_width = picLeftImage.Image.Width;
int image_height = picLeftImage.Image.Height;
byte[] fullres_left = new byte[image_width * image_height *
bytes_per_pixel];
byte[] fullres_right = new byte[fullres_left.Length];
BitmapArrayConversions.updatebitmap((Bitmap)picLeftImage.Image, fullres_left);
BitmapArrayConversions.updatebitmap((Bitmap)picRightImage.Image, fullres_right);
// load images into the correspondence object
stereointerface.loadImage(fullres_left, image_width, image_height, true, bytes_per_pixel);
stereointerface.loadImage(fullres_right, image_width, image_height, false, bytes_per_pixel);
// set the quality of the disparity map
stereointerface.setDisparityMapCompression(horizontal_compression, vertical_compression);
clock.Start();
// perform stereo matching
stereointerface.stereoMatchRun(0, 8, correspondence_algorithm_type);
long correspondence_time_mS = clock.Stop();
txtStereoCorrespondenceTime.Text = correspondence_time_mS.ToString();
// make a bitmap
Bitmap depthmap_bmp = new Bitmap(image_width, image_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
picDepthMap.Image = depthmap_bmp;
byte[] depthmap = new byte[image_width * image_height * 3];
stereointerface.getDisparityMap(depthmap, image_width, image_height, 0);
BitmapArrayConversions.updatebitmap_unsafe(depthmap, depthmap_bmp);
picDepthMap.Refresh();
}
private void timAnimation_Tick(object sender, EventArgs e)
{
if ((File.Exists(processed_left_image_filename)) &&
(File.Exists(processed_right_image_filename)))
{
if (picAnimation.Image != null)
{
picAnimation.Image.Dispose();
}
if (animation_state)
{
Bitmap bmp = (Bitmap)Bitmap.FromFile(processed_left_image_filename);
if (reverse_colours)
{
byte[] img = new byte[bmp.Width * bmp.Height * 3];
BitmapArrayConversions.updatebitmap(bmp, img);
BitmapArrayConversions.RGB_BGR(img);
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
}
picAnimation.Image = bmp;
}
else
{
Bitmap bmp = (Bitmap)Bitmap.FromFile(processed_right_image_filename);
if (reverse_colours)
{
byte[] img = new byte[bmp.Width * bmp.Height * 3];
BitmapArrayConversions.updatebitmap(bmp, img);
BitmapArrayConversions.RGB_BGR(img);
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
}
picAnimation.Image = bmp;
}
picAnimation.Refresh();
if (chkAnimate.Checked)
{
animation_state = !animation_state;
}
}
}
private void gaussianFunctionToolStripMenuItem_Click(object sender, EventArgs e)
{
grid_layer = new float[grid_dimension, grid_dimension, 3];
pos3D_x = new float[4];
pos3D_y = new float[4];
stereo_model = new stereoModel();
robot_head = new stereoHead(4);
stereo_features = new float[900];
stereo_uncertainties = new float[900];
img_rays = new Byte[standard_width * standard_height * 3];
rays = new Bitmap(standard_width, standard_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
picRays.Image = rays;
stereo_model.showDistribution(img_rays, standard_width, standard_height);
BitmapArrayConversions.updatebitmap_unsafe(img_rays, (Bitmap)picRays.Image);
}
private void multipleStereoRaysToolStripMenuItem_Click(object sender, EventArgs e)
{
grid_layer = new float[grid_dimension, grid_dimension, 3];
pos3D_x = new float[4];
pos3D_y = new float[4];
stereo_model = new stereoModel();
robot_head = new stereoHead(4);
stereo_features = new float[900];
stereo_uncertainties = new float[900];
img_rays = new Byte[standard_width * standard_height * 3];
rays = new Bitmap(standard_width, standard_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
picRays.Image = rays;
bool mirror = false;
stereo_model.showProbabilities(grid_layer, grid_dimension, img_rays, standard_width, standard_height, false, true, mirror);
BitmapArrayConversions.updatebitmap_unsafe(img_rays, (Bitmap)picRays.Image);
}
public void SampleNormalDistribution()
{
int image_width = 640;
robot rob = new robot(1);
motionModel mm = new motionModel(rob, rob.LocalGrid, 1);
byte[] img_rays = new byte[image_width * image_width * 3];
Bitmap bmp = new Bitmap(image_width, image_width, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
int[] hist = new int[20];
int max = 1;
int max_index = 0;
for (int sample = 0; sample < 2000; sample++)
{
float v = mm.sample_normal_distribution(20);
int index = (hist.Length / 2) + ((int)v / (hist.Length / 2));
if ((index > -1) && (index < hist.Length))
{
hist[index]++;
if (hist[index] > max)
{
max = hist[index];
max_index = index;
}
}
}
max += 5;
for (int x = 0; x < image_width; x++)
{
int index = x * hist.Length / image_width;
drawing.drawLine(img_rays, image_width, image_width, x, image_width - 1, x, image_width - 1 - (hist[index] * image_width / max), 255, 255, 255, 0, false);
}
BitmapArrayConversions.updatebitmap_unsafe(img_rays, bmp);
bmp.Save("motionmodel_tests_SampleNormalDistribution.bmp", System.Drawing.Imaging.ImageFormat.Bmp);
Assert.IsTrue(max_index == hist.Length / 2, "Peak of normal distribution is offset");
}
public void SaveDisparities(
string filename,
int stereo_matches)
{
int max_disp_pixels = 40 * (int)imgWidth / 100;
byte[] img = new byte[imgWidth * imgHeight * 3];
Bitmap bmp = new Bitmap((int)imgWidth, (int)imgHeight, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
for (int i = 0; i < stereo_matches; i++)
{
int x = svs_matches[i * 4 + 1];
int y = svs_matches[i * 4 + 2];
int disp = svs_matches[i * 4 + 3];
int radius = 1 + (disp / 8);
int v = disp * 255 / max_disp_pixels;
if (v > 255)
{
v = 255;
}
drawing.drawSpot(img, (int)imgWidth, (int)imgHeight, x, y, radius, v, v, v);
}
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
if (filename.EndsWith("bmp"))
{
bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Bmp);
}
if (filename.EndsWith("jpg"))
{
bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Jpeg);
}
if (filename.EndsWith("gif"))
{
bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Gif);
}
if (filename.EndsWith("png"))
{
bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Png);
}
}
/// <summary>
/// add some text to the given image
/// </summary>
/// <param name="img">colour image into which to insert th text</param>
/// <param name="img_width">width of the image</param>
/// <param name="img_height">height of the image</param>
/// <param name="text">text to be added</param>
/// <param name="font">font style</param>
/// <param name="font_size">font size</param>
/// <param name="r">red</param>
/// <param name="g">green</param>
/// <param name="b">blue</param>
/// <param name="position_x">x coordinate at which to insert the text</param>
/// <param name="position_y">y coordinate at which to insert the text</param>
public static void AddText(byte[] img, int img_width, int img_height,
String text,
String font, int font_size,
int r, int g, int b,
float position_x, float position_y)
{
Bitmap screen_bmp = new Bitmap(img_width, img_height,
System.Drawing.Imaging.PixelFormat.Format24bppRgb);
// insert the existing image into the bitmap
BitmapArrayConversions.updatebitmap_unsafe(img, screen_bmp);
Font drawFont = new Font(font, font_size);
SolidBrush drawBrush = new SolidBrush(Color.FromArgb(r, g, b));
Graphics grph = Graphics.FromImage(screen_bmp);
grph.DrawString(text, drawFont, drawBrush, position_x, position_y);
grph.Dispose();
// extract the bitmap data
BitmapArrayConversions.updatebitmap(screen_bmp, img);
}
public override void Show(ref Bitmap output)
{
byte[] output_img = null;
if (img[0].Length == image_width * image_height * 3)
{
output_img = new byte[image_width * image_height];
}
else
{
output_img = new byte[image_width * image_height * 3];
}
if (output == null)
{
output = new Bitmap(image_width, image_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
}
if (disparity_map != null)
{
BitmapArrayConversions.updatebitmap_unsafe(disparity_map, output);
}
}
public MainWindow() : base(Gtk.WindowType.Toplevel)
{
Build();
byte[] img = new byte[image_width * image_height * 3];
Bitmap left_bmp = new Bitmap(image_width, image_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
Bitmap right_bmp = new Bitmap(image_width, image_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
BitmapArrayConversions.updatebitmap_unsafe(img, left_bmp);
BitmapArrayConversions.updatebitmap_unsafe(img, right_bmp);
GtkBitmap.setBitmap(left_bmp, leftimage);
GtkBitmap.setBitmap(right_bmp, rightimage);
stereo_camera = new WebcamVisionStereoGtk(left_camera_device, right_camera_device, broadcast_port, fps);
stereo_camera.temporary_files_path = temporary_files_path;
stereo_camera.recorded_images_path = recorded_images_path;
stereo_camera.window = this;
stereo_camera.display_image[0] = leftimage;
stereo_camera.display_image[1] = rightimage;
//stereo_camera.Load(calibration_filename);
stereo_camera.image_width = image_width;
stereo_camera.image_height = image_height;
stereo_camera.min_exposure = minimum_brightness;
stereo_camera.max_exposure = maximum_brightness;
stereo_camera.flip_left_image = flip_left_image;
stereo_camera.flip_right_image = flip_right_image;
stereo_camera.baseline_mm = baseline_mm;
stereo_camera.Run();
stereo_camera.disable_rectification = disable_rectification;
stereo_camera.disable_radial_correction = disable_radial_correction;
chkRectification.Active = !disable_rectification;
chkRadialCorrection.Active = !disable_radial_correction;
chkFlipLeft.Active = stereo_camera.flip_left_image;
chkFlipRight.Active = stereo_camera.flip_right_image;
txtBaseline.Text = stereo_camera.baseline_mm.ToString();
}
private void showDisparityMap()
{
stereo.getDisparityMap(disp_bmp_data, picOutput.Image.Width, picOutput.Image.Height, 0);
BitmapArrayConversions.updatebitmap_unsafe(disp_bmp_data, (Bitmap)picOutput.Image);
picOutput.Refresh();
}
// run the simulation one step forwards
private void Simulation_RunOneStep()
{
busy = true;
int prev_time_step = sim.current_time_step;
// update the simulation
ArrayList images = getStereoImages(sim.current_time_step);
sim.RunOneStep(images);
// show the occupancy grid, from the perspective of the best available pose
showOccupancyGrid();
// show the possible paths
showPathTree();
// show the best robot pose
showBestPose();
// show the benchmarks
ArrayList benchmarks = sim.GetBenchmarks();
lstBenchmarks.View = View.List;
lstBenchmarks.Items.Clear();
for (int i = 0; i < benchmarks.Count; i++)
{
lstBenchmarks.Items.Add((String)benchmarks[i]);
}
// save the images, so that they may be used to produce an animation
if (!optimiser_running)
{
picGridMap.Image.Save("Simulation_step_" + Convert.ToString(sim.current_time_step) + ".jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
picPathTree.Image.Save("Path_tree_step_" + Convert.ToString(sim.current_time_step) + ".jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
}
if (prev_time_step == sim.current_time_step)
{
if (optimiser_running)
{
// get the score for this simulation run
float score = sim.position_error_mm;
// show the position error
txtPositionError.Text = Convert.ToString(sim.position_error_mm);
// show the average colour variance
txtMeanColourVariance.Text = Convert.ToString((int)(sim.GetMeanColourVariance() * 1000000) / 1000000.0f);
// update colour variance point graphs
sim.updateGraphs();
if (picColourVariance.Image == null)
{
picColourVariance.Image = new Bitmap(sim.graph_colour_variance.screen_width, sim.graph_colour_variance.screen_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
picNoOfParticles.Image = new Bitmap(sim.graph_no_of_particles.screen_width, sim.graph_colour_variance.screen_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
}
BitmapArrayConversions.updatebitmap_unsafe(sim.graph_colour_variance.image, (Bitmap)(picColourVariance.Image));
BitmapArrayConversions.updatebitmap_unsafe(sim.graph_no_of_particles.image, (Bitmap)(picNoOfParticles.Image));
picColourVariance.Refresh();
picNoOfParticles.Refresh();
// set the score for this run
autotuner.setScore(score);
// if this is the best score save the result
if ((score == autotuner.best_score) &&
(score < sim.rob.MinimumPositionError_mm))
{
showSideViews();
// set the minimum position error
sim.rob.MinimumPositionError_mm = score;
// save the robot design file with these tuning parameters
if (txtRobotDefinitionFile.Text != "")
{
sim.rob.Save(txtRobotDefinitionFile.Text);
}
// display the parameters
txtTuningParameters.Text = sim.GetTuningParameters();
// show the best score
txtBestScore.Text = Convert.ToString((int)(autotuner.best_score * 1000000) / 1000000.0f);
}
// show the score graph
//picGridSideView.Image = new Bitmap(640, 200, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
//Byte[] score_img = new Byte[640 * 200 * 3];
//autotuner.showHistory(score_img, 640, 200);
//BitmapArrayConversions.updatebitmap_unsafe(score_img, (Bitmap)(picGridSideView.Image));
// reset the simulation
Simulation_Reset();
// load the next instance
nextAutotunerInstance();
}
else
{
simulation_running = false;
StopSimulation();
showSideViews();
// test storage of the grid data
SaveGrid("testgrid.grd");
LoadGrid("testgrid.grd");
// show the grid
showOccupancyGrid();
showSideViews();
// export data in IFrIT format
Simulation_Export();
// reset the simulation
Simulation_Reset();
}
}
busy = false;
}
public frmManualOffsetCalibration(
string left_image_filename,
string right_image_filename,
float offset_x,
float offset_y,
float scale,
float rotation_degrees,
bool parameters_exist,
bool reverse_colours)
{
InitializeComponent();
this.left_image_filename = left_image_filename;
this.right_image_filename = right_image_filename;
this.offset_x = offset_x;
this.offset_y = offset_y;
this.scale = scale;
this.rotation_degrees = rotation_degrees;
this.reverse_colours = reverse_colours;
if (!parameters_exist)
{
LoadPreviousParameters();
if (left_image_filename != "")
{
this.left_image_filename = left_image_filename;
}
if (right_image_filename != "")
{
this.right_image_filename = right_image_filename;
}
}
if ((this.left_image_filename != null) &&
(this.left_image_filename != ""))
{
Console.WriteLine(this.left_image_filename);
Bitmap bmp = (Bitmap)Bitmap.FromFile(this.left_image_filename);
if (reverse_colours)
{
byte[] img = new byte[bmp.Width * bmp.Height * 3];
BitmapArrayConversions.updatebitmap(bmp, img);
BitmapArrayConversions.RGB_BGR(img);
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
}
picLeftImage.Image = bmp;
}
if ((this.right_image_filename != null) &&
(this.right_image_filename != ""))
{
Bitmap bmp = (Bitmap)Bitmap.FromFile(this.right_image_filename);
if (reverse_colours)
{
byte[] img = new byte[bmp.Width * bmp.Height * 3];
BitmapArrayConversions.updatebitmap(bmp, img);
BitmapArrayConversions.RGB_BGR(img);
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
}
picRightImage.Image = bmp;
}
txtOffsetX.Text = this.offset_x.ToString();
txtOffsetY.Text = this.offset_y.ToString();
txtScale.Text = this.scale.ToString();
txtRotation.Text = this.rotation_degrees.ToString();
Update();
}