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);
}
/// <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);
}
private void loadBackgroundImage(String filename)
{
if (File.Exists(filename))
{
Bitmap background = new Bitmap(filename);
Byte[] back_bmp = new Byte[background.Width * background.Height * 3];
BitmapArrayConversions.updatebitmapslow(background, back_bmp);
background_bmp = new Byte[global_variables.standard_width * global_variables.standard_height * 3];
int n = 0;
for (int y = 0; y < global_variables.standard_height; y++)
{
int yy = y * background.Height / global_variables.standard_height;
for (int x = 0; x < global_variables.standard_width; x++)
{
int xx = x * background.Width / global_variables.standard_width;
int n2 = ((yy * background.Width) + xx) * 3;
for (int c = 0; c < 3; c++)
{
background_bmp[n] = back_bmp[n2 + c];
n++;
}
}
}
}
}
/// <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 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);
}
}
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);
}
}
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);
}
/// <summary>
/// returns the given image as a byte array
/// </summary>
/// <param name="filename"></param>
/// <param name="image_width"></param>
/// <param name="image_height"></param>
/// <returns></returns>
private static byte[] GetImageData(string filename,
ref int image_width,
ref int image_height)
{
byte[] img = null;
if (filename.ToLower().EndsWith(".pgm"))
{
img = image.loadFromPGM(filename, ref image_width, ref image_height, 1);
}
else
{
Bitmap bmp = (Bitmap)Bitmap.FromFile(filename);
if (bmp != null)
{
image_width = bmp.Width;
image_height = bmp.Height;
img = new byte[image_width * image_height * 3];
// get the as a byte array
BitmapArrayConversions.updatebitmap(bmp, img);
// convert the image to mono format (1 byte per pixel)
img = image.monoImage(img, image_width, image_height);
}
}
return(img);
}
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);
}
}
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);
}
}
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>
/// updates the given image object from a bitmap object
/// </summary>
/// <param name="bmp"></param>
/// <param name="img"></param>
public static void setBitmap(Bitmap bmp, Gtk.Image img, ref byte[] bmp_data)
{
if (bmp_data == null)
{
bmp_data = new byte[bmp.Width * bmp.Height * 3];
}
BitmapArrayConversions.updatebitmap(bmp, bmp_data);
setBitmap(bmp_data, bmp.Width, bmp.Height, 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);
}
public static void setBitmap(Bitmap bmp, Gtk.Image img)
{
try
{
byte[] bmp_data = new byte[bmp.Width * bmp.Height * 3];
BitmapArrayConversions.updatebitmap(bmp, bmp_data);
setBitmap(bmp_data, bmp.Width, bmp.Height, img);
}
catch
{
}
}
/// <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);
}
/// <summary>
/// main update for stereo correspondence
/// </summary>
/// <param name="rectified_left">left image bitmap object</param>
/// <param name="rectified_right">right image bitmap object</param>
/// <param name="calibration_offset_x">horizontal offset from calibration, correcting for non-parallel alignment of the cameras</param>
/// <param name="calibration_offset_y">vertical offset from calibration, correcting for non-parallel alignment of the cameras</param>
public void Update(
Bitmap rectified_left,
Bitmap rectified_right,
float calibration_offset_x,
float calibration_offset_y)
{
image_width = rectified_left.Width;
image_height = rectified_left.Height;
int pixels = image_width * image_height * 3;
if (img[0] == null)
{
for (int i = 0; i < 4; i++)
{
img[i] = new byte[pixels];
}
}
else
{
if (img[0].Length != pixels)
{
for (int i = 0; i < 4; i++)
{
img[i] = new byte[pixels];
}
}
}
BitmapArrayConversions.updatebitmap(rectified_left, img[0]);
BitmapArrayConversions.updatebitmap(rectified_right, img[1]);
// convert colour images to mono
monoImage(img[0], image_width, image_height, 1, ref img[2]);
monoImage(img[1], image_width, image_height, 1, ref img[3]);
// main stereo correspondence routine
Update(img[0], img[1],
img[2], img[3],
rectified_left.Width, rectified_left.Height,
calibration_offset_x, calibration_offset_y);
if (BroadcastStereoFeatureColours)
{
// assign a colour to each feature
UpdateFeatureColours(img[0], img[1]);
}
// send stereo features to connected clients
BroadcastStereoFeatures();
}
public void updateVision(Image input_img, bool leftImage)
{
int cameraIndex = 0;
Byte[] disp_bmp_data = bmp_data_left;
if (!leftImage)
{
cameraIndex = 1;
disp_bmp_data = bmp_data_right;
}
Byte[] ary;
try
{
if (captureState[cameraIndex] == 1)
{
if (disp_bmp_data == null)
{
if (leftImage)
{
bmp_data_left = new Byte[input_img.Width * input_img.Height * 3];
disp_bmp_data = bmp_data_left;
}
else
{
bmp_data_right = new Byte[input_img.Width * input_img.Height * 3];
disp_bmp_data = bmp_data_right;
}
}
ary = disp_bmp_data;
BitmapArrayConversions.updatebitmap((Bitmap)(input_img.Clone()), ary);
captureState[cameraIndex] = 2;
}
if (leftImage)
{
left_camera_running = true;
}
else
{
right_camera_running = true;
}
}
catch
{
}
}
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 loadSimulationImages()
{
String left_filename = System.AppDomain.CurrentDomain.BaseDirectory + "\\left_" + Convert.ToString(simulation_file_index) + ".jpg";
String right_filename = System.AppDomain.CurrentDomain.BaseDirectory + "\\right_" + Convert.ToString(simulation_file_index) + ".jpg";
captureState[0] = 0;
captureState[1] = 0;
if ((File.Exists(left_filename)) && (File.Exists(right_filename)))
{
picLeftImage.Image = new Bitmap(left_filename);
picLeftImage.Refresh();
left_image = (Bitmap)picLeftImage.Image.Clone();
if (global_variables.left_bmp == null)
{
global_variables.left_bmp = new Byte[left_image.Width * left_image.Height * 3];
}
BitmapArrayConversions.updatebitmap(left_image, global_variables.left_bmp);
captureState[0] = 2;
picRightImage.Image = new Bitmap(right_filename);
picRightImage.Refresh();
right_image = (Bitmap)picRightImage.Image.Clone();
if (global_variables.right_bmp == null)
{
global_variables.right_bmp = new Byte[right_image.Width * right_image.Height * 3];
}
BitmapArrayConversions.updatebitmap(right_image, global_variables.right_bmp);
captureState[1] = 2;
}
// we need to slow things down, otherwise the simulation runs at lightning speed
delay_counter++;
if (delay_counter > 2)
{
delay_counter = 0;
simulation_file_index++;
if (simulation_file_index >= output_file_index)
{
simulation_file_index = 1;
}
}
}
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;
}
}
}
public void updateVisionRight(Image input_img, bool leftImage)
{
try
{
if (captureState[1] == 1)
{
if (global_variables.right_bmp == null)
global_variables.right_bmp = new Byte[input_img.Width * input_img.Height * 3];
right_image = (Bitmap)input_img.Clone();
BitmapArrayConversions.updatebitmap(right_image, global_variables.right_bmp);
captureState[1] = 2;
}
right_camera_running = true;
}
catch //(Exception ex)
{
//MessageBox.Show("updateVisionRight/" + ex.Message);
}
}
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);
}