protected override void DisplayImages(Bitmap left_image, Bitmap right_image)
{
if (display_image[0] != null)
{
if (display_image[0].Image == null)
{
display_image[0].Image = (Bitmap)left_image.Clone();
}
if ((show_left_image) &&
(calibration_pattern != null))
{
display_image[0].Image = calibration_pattern;
if (calibration_survey != null)
{
CalibrationSurvey survey = calibration_survey[1];
if (survey != null)
{
if ((survey.minimum_rms_error < 3) &&
((prev_minimum_rms_error >= 3) || (prev_minimum_rms_error == 0)))
{
BeepSound.Play("beep.wav");
}
prev_minimum_rms_error = survey.minimum_rms_error;
}
}
}
else
{
DisplayImage((Bitmap)display_image[0].Image, left_image, true);
}
if (window != null)
{
//window.UpdateLeftImage(display_image[0]);
try
{
window.Invoke((MethodInvoker) delegate
{
bool success = false;
DateTime start_time = DateTime.Now;
int time_elapsed_mS = 0;
while ((!success) && (time_elapsed_mS < 500))
{
try
{
display_image[0].Refresh();
success = true;
}
catch
{
System.Threading.Thread.Sleep(5);
TimeSpan diff = DateTime.Now.Subtract(start_time);
time_elapsed_mS = (int)diff.TotalMilliseconds;
}
}
});
}
catch
{
}
}
}
if (display_image[1] != null)
{
if (display_image[1].Image == null)
{
display_image[1].Image = (Bitmap)right_image.Clone();
}
if ((!show_left_image) &&
(calibration_pattern != null))
{
display_image[1].Image = calibration_pattern;
if (calibration_survey != null)
{
CalibrationSurvey survey = calibration_survey[0];
if (survey != null)
{
if ((survey.minimum_rms_error < 3) &&
((prev_minimum_rms_error >= 3) || (prev_minimum_rms_error == 0)))
{
BeepSound.Play("beep.wav");
}
prev_minimum_rms_error = survey.minimum_rms_error;
}
}
}
else
{
DisplayImage((Bitmap)display_image[1].Image, right_image, false);
}
if (window != null)
{
//window.UpdateRightImage(display_image[1]);
try
{
window.Invoke((MethodInvoker) delegate
{
bool success = false;
DateTime start_time = DateTime.Now;
int time_elapsed_mS = 0;
while ((!success) && (time_elapsed_mS < 500))
{
try
{
display_image[1].Refresh();
success = true;
}
catch
{
System.Threading.Thread.Sleep(5);
TimeSpan diff = DateTime.Now.Subtract(start_time);
time_elapsed_mS = (int)diff.TotalMilliseconds;
}
}
});
}
catch
{
}
}
}
}
/// <summary>
/// parse an xml node to extract camera calibration parameters
/// </summary>
/// <param name="xnod"></param>
/// <param name="level"></param>
public void LoadFromXml(XmlNode xnod, int level, ref int camera_index)
{
usage.Update("Load from Xml, BaseVisionStereo, LoadFromXml");
IFormatProvider format = new System.Globalization.CultureInfo("en-GB");
if (xnod.Name == "Camera")
{
camera_index++;
calibration_survey[camera_index] = new CalibrationSurvey();
}
if (xnod.Name == "DisableRadialCorrection")
{
disable_radial_correction = Convert.ToBoolean(xnod.InnerText);
}
if (xnod.Name == "DisableRectification")
{
disable_rectification = Convert.ToBoolean(xnod.InnerText);
}
if (xnod.Name == "Scale")
{
// do nothing
}
if (xnod.Name == "Flip")
{
if (camera_index == 0)
flip_left_image = Convert.ToBoolean(xnod.InnerText);
else
flip_right_image = Convert.ToBoolean(xnod.InnerText);
}
if (xnod.Name == "RelativeImageScale")
{
scale = Convert.ToSingle(xnod.InnerText, format);
}
if (xnod.Name == "DeviceName")
{
device_name = xnod.InnerText;
}
if (xnod.Name == "PartNumber")
{
part_number = xnod.InnerText;
}
if (xnod.Name == "SerialNumber")
{
serial_number = xnod.InnerText;
}
if (xnod.Name == "Offsets")
{
string[] offsets = xnod.InnerText.Split(' ');
offset_x = Convert.ToSingle(offsets[0], format);
offset_y = Convert.ToSingle(offsets[1], format);
}
if (xnod.Name == "FocalLengthMillimetres")
{
focal_length_mm = Convert.ToSingle(xnod.InnerText, format);
}
if (xnod.Name == "SensorDensityPixelsPerMillimetre")
{
pixels_per_mm = Convert.ToSingle(xnod.InnerText, format);
focal_length_pixels = focal_length_mm * pixels_per_mm;
}
if (xnod.Name == "BaselineMillimetres")
{
baseline_mm = Convert.ToSingle(xnod.InnerText, format);
}
if (xnod.Name == "FieldOfViewDegrees")
fov_degrees = Convert.ToSingle(xnod.InnerText, format);
if (xnod.Name == "ImageDimensions")
{
string[] dimStr = xnod.InnerText.Split('x');
image_width = Convert.ToInt32(dimStr[0]);
image_height = Convert.ToInt32(dimStr[1]);
}
if (xnod.Name == "CentreOfDistortion")
{
string[] centreStr = xnod.InnerText.Split(' ');
calibration_survey[camera_index].centre_of_distortion_x =
Convert.ToSingle(centreStr[0], format);
calibration_survey[camera_index].centre_of_distortion_y =
Convert.ToSingle(centreStr[1], format);
}
if (xnod.Name == "DistortionCoefficients")
{
if (xnod.InnerText != "")
{
string[] coeffStr = xnod.InnerText.Split(' ');
calibration_survey[camera_index].best_fit_curve = new polynomial();
calibration_survey[camera_index].best_fit_curve.SetDegree(coeffStr.Length - 1);
for (int i = 0; i < coeffStr.Length; i++)
calibration_survey[camera_index].best_fit_curve.SetCoeff(i, Convert.ToSingle(coeffStr[i], format));
}
}
if (xnod.Name == "RelativeRotationDegrees")
{
rotation = Convert.ToSingle(xnod.InnerText, format) / 180.0f * (float)Math.PI;
}
if (xnod.Name == "RMSerror")
{
if ((xnod.InnerText != "") &&
(xnod.InnerText != "Infinity"))
calibration_survey[camera_index].minimum_rms_error = Convert.ToSingle(xnod.InnerText, format);
}
// call recursively on all children of the current node
if (xnod.HasChildNodes)
{
XmlNode xnodWorking = xnod.FirstChild;
while (xnodWorking != null)
{
LoadFromXml(xnodWorking, level + 1, ref camera_index);
xnodWorking = xnodWorking.NextSibling;
}
}
}
public static hypergraph DetectDots(
Bitmap bmp,
ref EdgeDetectorCanny edge_detector,
CalibrationSurvey survey,
ref Bitmap detected_dots,
ref Bitmap linked_dots,
ref Bitmap grd,
ref Bitmap grid_diff,
ref Bitmap rectified)
{
const int minimum_links = (dots_across * (dots_across/2)) / 2;
hypergraph dots = null;
DetectEdges(bmp, ref edge_detector, ref dots, ref detected_dots);
if (dots != null)
{
// find the dots around the red centre dot
List<CalibrationDot> centre_dots = null;
polygon2D centre_square = GetCentreSquare(dots, ref centre_dots);
if (centre_square != null)
{
if (centre_square.x_points.Count == 4)
{
// find the orientation of the centre square
float angle = geometry.threePointAngle(
(float)centre_dots[1].x, 0,
(float)centre_dots[1].x, (float)centre_dots[1].y,
(float)centre_dots[2].x, (float)centre_dots[2].y);
angle = angle / (float)Math.PI * 180;
if (angle > 160)
{
angle = geometry.threePointAngle(
(float)centre_dots[2].x, 0,
(float)centre_dots[2].x, (float)centre_dots[2].y,
(float)centre_dots[3].x, (float)centre_dots[3].y);
angle = angle / (float)Math.PI * 180;
if ((angle >= 70) && (angle < 120))
{
// link dots together
List<CalibrationDot> search_regions = new List<CalibrationDot>();
double horizontal_dx = centre_dots[1].x - centre_dots[0].x;
double horizontal_dy = centre_dots[1].y - centre_dots[0].y;
double vertical_dx = centre_dots[3].x - centre_dots[0].x;
double vertical_dy = centre_dots[3].y - centre_dots[0].y;
for (int i = 0; i < centre_dots.Count; i++)
LinkDots(dots, centre_dots[i], horizontal_dx, horizontal_dy, vertical_dx, vertical_dy, 0, search_regions);
if (dots.Links.Count > minimum_links)
{
int tollerance_degrees = 10;
if (ValidGrid(dots, bmp.Width, bmp.Height, tollerance_degrees))
{
ShowLinkedDots(bmp, dots, search_regions, ref linked_dots);
// assign a grid coordinate to each dot
ApplyGrid(dots, centre_dots);
// put the dots into a 2D grid for convenient lookup
CalibrationDot[,] grid = CreateGrid(dots);
if (grid != null)
{
grid2D ideal_grid = GetIdealGrid(grid, bmp.Width, bmp.Height);
ShowIdealGrid(bmp, ideal_grid, grid, ref grd);
ShowIdealGridDiff(bmp, grid, ref grid_diff);
survey.Update(bmp.Width, bmp.Height, grid);
ShowRectifiedImage(bmp, survey.centre_of_distortion_x, survey.centre_of_distortion_y, survey.best_fit_curve, ref rectified);
}
}
}
}
}
}
}
}
return(dots);
}
/// <summary>
/// constructor
/// </summary>
/// <param name="broadcast_port">port number on which to broadcast stereo feature data to other applications</param>
/// <param name="fps">ideal frames per second</param>
public BaseVisionStereo(
int broadcast_port,
float fps)
{
this.fps = fps;
broadcast_port_number = broadcast_port;
calibration_survey = new CalibrationSurvey[2];
calibration_map = new int[2][];
calibration_map_inverse = new int[2][,,];
calibration_survey[0] = new CalibrationSurvey();
calibration_survey[1] = new CalibrationSurvey();
rectified = new Bitmap[2];
// delete and previously recorded images
string[] victims = Directory.GetFiles(".", "raw*.jpg");
if (victims != null)
{
for (int v = 0; v < victims.Length; v++)
File.Delete(victims[v]);
}
usage = new UsageGraph();
//usage.log_file = "debug.txt";
if (File.Exists(usage.log_file)) File.Delete(usage.log_file);
}
/// <summary>
/// shows both left and right camera images within the GUI
/// </summary>
/// <param name="left_image">left image bitmap</param>
/// <param name="right_image">right image bitmap</param>
protected override void DisplayImages(Bitmap left_image, Bitmap right_image)
{
usage.Update("Display Images, SurveyorVisionStereoGtk, DisplayImages");
if (display_image[0] != null)
{
if ((show_left_image) &&
(calibration_pattern != null))
{
try
{
GtkBitmap.setBitmap(calibration_pattern, display_image[0], ref buffer);
}
catch
{
}
if (calibration_survey != null)
{
CalibrationSurvey survey = calibration_survey[1];
if (survey != null)
{
if ((survey.minimum_rms_error < 3) &&
((prev_minimum_rms_error >= 3) || (prev_minimum_rms_error == 0)))
{
PlaySound("beep.wav");
}
prev_minimum_rms_error = survey.minimum_rms_error;
}
}
}
else
{
DisplayImage(display_image[0], left_image, true);
}
}
if (display_image[1] != null)
{
if ((!show_left_image) &&
(calibration_pattern != null))
{
try
{
GtkBitmap.setBitmap(calibration_pattern, display_image[1], ref buffer);
}
catch
{
}
if (calibration_survey != null)
{
CalibrationSurvey survey = calibration_survey[0];
if (survey != null)
{
if ((survey.minimum_rms_error < 3) &&
((prev_minimum_rms_error >= 3) || (prev_minimum_rms_error == 0)))
{
PlaySound("beep.wav");
}
prev_minimum_rms_error = survey.minimum_rms_error;
}
}
}
else
{
DisplayImage(display_image[1], right_image, false);
}
}
if (window != null)
{
if (!IsWindows())
{
// Here we need to update the GUI after receiving the right camera image
// Since we're running in a separate thread from the GUI we have to
// call it in a special way
RunOnMainThread.Run(this, "UpdateGUI", new object[] { window, calibration_window });
Gdk.Threads.Enter();
window.QueueDraw();
Gdk.Threads.Leave();
}
}
}
