public void Reset()
{
survey = null;
survey_updates = 0;
minimum_rms_error = double.MaxValue;
best_fit_curve = null;
}
/// <summary>
/// fits a curve to the given grid using the given centre of distortion
/// </summary>
/// <param name="grid">detected grid dots</param>
/// <param name="overlay_grid">overlayed ideal rectified grid</param>
/// <param name="centre_of_distortion">centre of lens distortion</param>
/// <param name="curve">curve to be fitted</param>
private static void FitCurve(CalibrationDot[,] grid,
grid2D overlay_grid,
CalibrationDot centre_of_distortion,
polynomial curve,
double noise, Random rnd,
int grid_offset_x, int grid_offset_y)
{
double[] prev_col = new double[grid.GetLength(1) * 2];
double[] col = new double[prev_col.Length];
double half_noise = noise / 2;
double rectified_x, rectified_y;
for (int pass = 0; pass < 1; pass++)
{
// for every detected dot
for (int grid_x = 0; grid_x < grid.GetLength(0); grid_x++)
{
double prev_rectified_radial_dist = 0;
double prev_actual_radial_dist = 0;
int prev_grid_y = -1;
for (int grid_y = 0; grid_y < grid.GetLength(1); grid_y++)
{
if (grid[grid_x, grid_y] != null)
{
if ((grid_x + grid_offset_x < overlay_grid.line_intercepts.GetLength(0)) &&
(grid_y + grid_offset_y < overlay_grid.line_intercepts.GetLength(1)) &&
(grid_x + grid_offset_x >= 0) && (grid_y + grid_offset_y >= 0))
{
// find the rectified distance of the dot from the centre of distortion
rectified_x = overlay_grid.line_intercepts[grid_x + grid_offset_x, grid_y + grid_offset_y, 0];
rectified_y = overlay_grid.line_intercepts[grid_x + grid_offset_x, grid_y + grid_offset_y, 1];
if (pass > 0)
{
rectified_x += (((rnd.NextDouble() * noise) - half_noise) * 0.1);
rectified_y += (((rnd.NextDouble() * noise) - half_noise) * 0.1);
}
//double rectified_x = overlay_grid.line_intercepts[grid_x + grid_offset_x, grid_y + grid_offset_y, 0];
//double rectified_y = overlay_grid.line_intercepts[grid_x + grid_offset_x, grid_y + grid_offset_y, 1];
double rectified_dx = rectified_x - centre_of_distortion.x;
double rectified_dy = rectified_y - centre_of_distortion.y;
double rectified_radial_dist = Math.Sqrt(rectified_dx * rectified_dx + rectified_dy * rectified_dy);
// find the actual raw image distance of the dot from the centre of distortion
//double actual_x = grid[grid_x, grid_y].x + (((rnd.NextDouble() * noise) - half_noise) * 2);
//double actual_y = grid[grid_x, grid_y].y + (((rnd.NextDouble() * noise) - half_noise) * 2);
double actual_x = grid[grid_x, grid_y].x;
double actual_y = grid[grid_x, grid_y].y;
double actual_dx = actual_x - centre_of_distortion.x;
double actual_dy = actual_y - centre_of_distortion.y;
double actual_radial_dist = Math.Sqrt(actual_dx * actual_dx + actual_dy * actual_dy);
// plot
curve.AddPoint(rectified_radial_dist, actual_radial_dist);
col[(grid_y * 2)] = rectified_radial_dist;
col[(grid_y * 2) + 1] = actual_radial_dist;
prev_rectified_radial_dist = rectified_radial_dist;
prev_actual_radial_dist = actual_radial_dist;
prev_grid_y = grid_y;
}
}
}
for (int i = 0; i < col.Length; i++)
prev_col[i] = col[i];
}
}
// find the best fit curve
curve.Solve();
}
/// <summary>
///
/// </summary>
/// <param name="filename"></param>
/// <param name="image_width"></param>
/// <param name="image_height"></param>
/// <param name="fov_degrees"></param>
/// <param name="dist_to_centre_dot_mm"></param>
/// <param name="dot_spacing_mm"></param>
/// <param name="centre_of_distortion_x"></param>
/// <param name="centre_of_distortion_y"></param>
/// <param name="lens_distortion_curve"></param>
/// <param name="camera_rotation"></param>
/// <param name="scale"></param>
/// <param name="lens_distortion_image_filename"></param>
/// <param name="curve_fit_image_filename"></param>
/// <param name="rectified_image_filename"></param>
/// <param name="centre_dot_x"></param>
/// <param name="centre_dot_y"></param>
/// <param name="minimum_error"></param>
/// <param name="calibration_map"></param>
/// <returns></returns>
private static hypergraph Detect(string filename,
ref int image_width, ref int image_height,
float fov_degrees, float dist_to_centre_dot_mm, float dot_spacing_mm,
ref double centre_of_distortion_x, ref double centre_of_distortion_y,
ref polynomial lens_distortion_curve,
ref double camera_rotation, ref double scale,
string lens_distortion_image_filename,
string curve_fit_image_filename,
string rectified_image_filename,
ref float centre_dot_x, ref float centre_dot_y,
ref double minimum_error,
ref int[] calibration_map,
int random_seed)
{
image_width = 0;
image_height = 0;
hypergraph dots = DetectDots(filename, ref image_width, ref image_height);
// how far is the centre dot from the centre of the image ?
float centre_dot_horizontal_distance = float.MaxValue;
float centre_dot_vertical_distance = 0;
GetCentreDotDisplacement(image_width, image_height, dots,
ref centre_dot_horizontal_distance,
ref centre_dot_vertical_distance);
centre_dot_x = centre_dot_horizontal_distance + (image_width / 2);
centre_dot_y = centre_dot_vertical_distance + (image_height / 2);
if ((Math.Abs(centre_dot_horizontal_distance) > image_width / 4) ||
(Math.Abs(centre_dot_vertical_distance) > image_height / 4))
{
Console.WriteLine("The calibration pattern centre dot was not detected or is too far away from the centre of the image");
}
else
{
// are the dots covering an adequate area of the image ?
int dots_at_top_of_image = 0;
int dots_at_bottom_of_image = 0;
int border_percent = 20;
for (int i = 0; i < dots.Nodes.Count; i++)
{
CalibrationDot d = (CalibrationDot)dots.Nodes[i];
if (d.y < image_height * border_percent / 100)
dots_at_top_of_image++;
if (d.y > image_height - (image_height * border_percent / 100))
dots_at_bottom_of_image++;
}
//if ((dots_at_top_of_image > 2) &&
// (dots_at_bottom_of_image > 2))
if (dots.Nodes.Count > 100)
{
// square around the centre dot
List<CalibrationDot> centre_dots = null;
polygon2D centre_square = GetCentreSquare(dots, ref centre_dots);
ShowSquare(filename, centre_square, "centre_square.jpg");
float ideal_centre_square_angular_diameter_degrees = (2.0f * (float)Math.Atan(0.5f * dot_spacing_mm / dist_to_centre_dot_mm)) * 180 / (float)Math.PI;
float ideal_centre_square_dimension_pixels = ideal_centre_square_angular_diameter_degrees * image_width / fov_degrees;
scale = 1; // ((centre_square.getSideLength(0) + centre_square.getSideLength(2)) * 0.5f) / ideal_centre_square_dimension_pixels;
//scale = 1;
//Console.WriteLine("centre_square ideal_dimension: " + ideal_centre_square_dimension_pixels.ToString());
//Console.WriteLine("centre_square actual_dimension: " + ((centre_square.getSideLength(0) + centre_square.getSideLength(2)) * 0.5f).ToString());
//Console.WriteLine("scale: " + scale.ToString());
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);
Console.WriteLine(dots.Links.Count.ToString() + " links created");
ApplyGrid(dots, centre_dots);
CalibrationDot[,] grid = CreateGrid(dots);
if (grid != null)
{
int grid_offset_x = 0, grid_offset_y = 0;
List<CalibrationDot> corners = new List<CalibrationDot>();
grid2D overlay_grid = OverlayIdealGrid(grid, corners, ref grid_offset_x, ref grid_offset_y, random_seed);
if (overlay_grid != null)
{
ShowDots(corners, filename, "corners.jpg");
List<List<double>> lines = CreateLines(dots, grid);
lens_distortion_curve = null;
CalibrationDot centre_of_distortion = null;
List<List<double>> best_rectified_lines = null;
DetectLensDistortion(image_width, image_height, grid, overlay_grid, lines,
ref lens_distortion_curve, ref centre_of_distortion,
ref best_rectified_lines,
grid_offset_x, grid_offset_y, scale,
ref minimum_error,
random_seed);
if (lens_distortion_curve != null)
{
ShowDistortionCurve(lens_distortion_curve, curve_fit_image_filename);
ShowLensDistortion(image_width, image_height, centre_of_distortion, lens_distortion_curve, fov_degrees, 10, lens_distortion_image_filename);
// detect the camera rotation
List<List<double>> rectified_centre_line = null;
camera_rotation = DetectCameraRotation(image_width, image_height,
grid, lens_distortion_curve, centre_of_distortion,
ref rectified_centre_line, scale);
double rotation_degrees = camera_rotation / Math.PI * 180;
camera_rotation = 0;
centre_of_distortion_x = centre_of_distortion.x;
centre_of_distortion_y = centre_of_distortion.y;
calibration_map = null;
int[, ,] calibration_map_inverse = null;
updateCalibrationMap(image_width, image_height,
lens_distortion_curve,
(float)scale, (float)camera_rotation,
(float)centre_of_distortion.x, (float)centre_of_distortion.y,
ref calibration_map,
ref calibration_map_inverse);
Rectify(filename, calibration_map, rectified_image_filename);
if (rectified_centre_line != null)
ShowLines(filename, rectified_centre_line, "rectified_centre_line.jpg");
if (best_rectified_lines != null)
{
ShowLines(filename, lines, "lines.jpg");
ShowLines(filename, best_rectified_lines, "rectified_lines.jpg");
}
}
}
ShowOverlayGrid(filename, overlay_grid, "grid_overlay.jpg");
ShowOverlayGridPerimeter(filename, overlay_grid, "grid_overlay_perimeter.jpg");
ShowGrid(filename, dots, search_regions, "grid.jpg");
ShowGridCoordinates(filename, dots, "coordinates.jpg");
}
}
}
return (dots);
}
/// <summary>
/// save camera calibration parameters as an xml file
/// </summary>
/// <param name="filename"></param>
/// <param name="device_name"></param>
/// <param name="focal_length_pixels"></param>
/// <param name="baseline_mm"></param>
/// <param name="fov_degrees"></param>
/// <param name="image_width"></param>
/// <param name="image_height"></param>
/// <param name="lens_distortion_curve"></param>
/// <param name="centre_of_distortion_x"></param>
/// <param name="centre_of_distortion_y"></param>
/// <param name="rotation"></param>
/// <param name="scale"></param>
/// <param name="lens_distortion_image_filename"></param>
/// <param name="curve_fit_image_filename"></param>
/// <param name="offset_x"></param>
/// <param name="offset_y"></param>
/// <param name="pan_curve"></param>
/// <param name="pan_offset_x"></param>
/// <param name="pan_offset_y"></param>
/// <param name="tilt_curve"></param>
/// <param name="tilt_offset_x"></param>
/// <param name="tilt_offset_y"></param>
private static void Save(
string filename,
string device_name,
float focal_length_pixels,
float baseline_mm,
float fov_degrees,
int image_width, int image_height,
polynomial[] lens_distortion_curve,
double[] centre_of_distortion_x, double[] centre_of_distortion_y,
double[] rotation, double[] scale,
string[] lens_distortion_image_filename,
string[] curve_fit_image_filename,
float offset_x, float offset_y,
polynomial pan_curve, float pan_offset_x, float pan_offset_y,
polynomial tilt_curve, float tilt_offset_x, float tilt_offset_y)
{
XmlDocument doc =
getXmlDocument(
device_name,
focal_length_pixels,
baseline_mm,
fov_degrees,
image_width, image_height,
lens_distortion_curve,
centre_of_distortion_x, centre_of_distortion_y,
rotation, scale,
lens_distortion_image_filename,
curve_fit_image_filename,
offset_x, offset_y,
pan_curve, pan_offset_x, pan_offset_y,
tilt_curve, tilt_offset_x, tilt_offset_y);
doc.Save(filename);
}
private static XmlElement getXml(
XmlDocument doc, XmlElement parent,
string device_name,
float focal_length_pixels,
float baseline_mm,
float fov_degrees,
int image_width, int image_height,
polynomial[] lens_distortion_curve,
double[] centre_of_distortion_x, double[] centre_of_distortion_y,
double[] rotation, double[] scale,
string[] lens_distortion_image_filename,
string[] curve_fit_image_filename,
float offset_x, float offset_y,
polynomial pan_curve, float pan_offset_x, float pan_offset_y,
polynomial tilt_curve, float tilt_offset_x, float tilt_offset_y)
{
// make sure that floating points are saved in a standard format
IFormatProvider format = new System.Globalization.CultureInfo("en-GB");
// is this a stereo or a monocular camera ?
bool stereo_camera = false;
if (lens_distortion_curve.Length == 2)
if ((lens_distortion_curve[0] != null) && (lens_distortion_curve[1] != null))
stereo_camera = true;
// pan/tilt mechanism parameters
XmlElement nodePanTilt;
nodePanTilt = doc.CreateElement("CameraPanTilt");
nodePanTilt.AppendChild(getPanTiltAxis(doc, nodePanTilt, "Pan", pan_curve, pan_offset_x, pan_offset_y));
nodePanTilt.AppendChild(getPanTiltAxis(doc, nodePanTilt, "Tilt", tilt_curve, tilt_offset_x, tilt_offset_y));
parent.AppendChild(nodePanTilt);
// camera parameters
XmlElement nodeStereoCamera;
if (stereo_camera)
nodeStereoCamera = doc.CreateElement("StereoCamera");
else
nodeStereoCamera = doc.CreateElement("MonocularCamera");
parent.AppendChild(nodeStereoCamera);
if ((device_name != null) && (device_name != ""))
{
xml.AddComment(doc, nodeStereoCamera, "Name of the camera device");
xml.AddTextElement(doc, nodeStereoCamera, "DeviceName", device_name);
}
xml.AddComment(doc, nodeStereoCamera, "Focal length in pixels");
xml.AddTextElement(doc, nodeStereoCamera, "FocalLengthPixels", Convert.ToString(focal_length_pixels, format));
if (stereo_camera)
{
xml.AddComment(doc, nodeStereoCamera, "Camera baseline distance in millimetres");
xml.AddTextElement(doc, nodeStereoCamera, "BaselineMillimetres", Convert.ToString(baseline_mm, format));
}
xml.AddComment(doc, nodeStereoCamera, "Calibration Data");
XmlElement nodeCalibration = doc.CreateElement("Calibration");
nodeStereoCamera.AppendChild(nodeCalibration);
if (stereo_camera)
{
string offsets = Convert.ToString(offset_x, format) + "," +
Convert.ToString(offset_y, format);
xml.AddComment(doc, nodeCalibration, "Image offsets in pixels due to small missalignment from parallel");
xml.AddTextElement(doc, nodeCalibration, "Offsets", offsets);
}
for (int cam = 0; cam < lens_distortion_curve.Length; cam++)
{
XmlElement elem = getCameraXml(
doc, fov_degrees,
image_width, image_height,
lens_distortion_curve[cam],
centre_of_distortion_x[cam], centre_of_distortion_y[cam],
rotation[cam], scale[cam],
lens_distortion_image_filename[cam],
curve_fit_image_filename[cam]);
nodeCalibration.AppendChild(elem);
}
return (nodeStereoCamera);
}
/// <summary>
/// displays the lens distortion curve
/// </summary>
/// <param name="curve">curve to be displayed</param>
/// <param name="output_filename">filename to save as</param>
private static void ShowDistortionCurve(polynomial curve, string output_filename)
{
int img_width = 640;
int img_height = 480;
byte[] img = new byte[img_width * img_height * 3];
curve.Show(img, img_width, img_height, "Rectified radial distance (pixels)", "Raw image radial distance (pixels)");
Bitmap output_bmp = new Bitmap(img_width, img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
BitmapArrayConversions.updatebitmap_unsafe(img, output_bmp);
if (output_filename.ToLower().EndsWith("jpg"))
output_bmp.Save(output_filename, System.Drawing.Imaging.ImageFormat.Jpeg);
if (output_filename.ToLower().EndsWith("bmp"))
output_bmp.Save(output_filename, System.Drawing.Imaging.ImageFormat.Bmp);
}
/// <summary>
/// calibrate stereo camera from the given images
/// </summary>
/// <param name="directory">directory containing the calibration images</param>
/// <param name="baseline_mm">baseline of the stereo camera</param>
/// <param name="dotdist_mm">horizontal distance to the centre of the calibration pattern in millimetres</param>
/// <param name="height_mm">vertical height of the cameras above the calibration pattern</param>
/// <param name="fov_degrees">field of view of the cameras in degrees</param>
/// <param name="dot_spacing_mm">spacing between dots on the calibration pattern in millimetres</param>
/// <param name="focal_length_pixels">focal length of the cameras in pixels</param>
/// <param name="file_extension">file extension of the calibration images (typically "jpg" or "bmp")</param>
public static void Calibrate(string directory,
int baseline_mm,
int dotdist_mm,
int height_mm,
float fov_degrees,
float dot_spacing_mm,
float focal_length_pixels,
string file_extension)
{
string calibration_xml_filename = "calibration.xml";
if (File.Exists(calibration_xml_filename))
File.Delete(calibration_xml_filename);
if (directory.Contains("\\"))
{
if (!directory.EndsWith("\\")) directory += "\\";
}
else
{
if (directory.Contains("/"))
if (!directory.EndsWith("/")) directory += "/";
}
string[] filename = Directory.GetFiles(directory, "*." + file_extension);
if (filename != null)
{
// two lists to store filenames for camera 0 and camera 1
List<string>[] image_filename = new List<string>[2];
image_filename[0] = new List<string>();
image_filename[1] = new List<string>();
// populate the lists
for (int i = 0; i < filename.Length; i++)
{
if (filename[i].Contains("raw0"))
image_filename[0].Add(filename[i]);
if (filename[i].Contains("raw1"))
image_filename[1].Add(filename[i]);
}
if (image_filename[0].Count == 0)
{
Console.WriteLine("Did not find any calibration files. Do they begin with raw0 or raw1 ?");
}
else
{
if (image_filename[0].Count != image_filename[1].Count)
{
Console.WriteLine("There must be the same number of images from camera 0 and camera 1");
}
else
{
string lens_distortion_image_filename = "temp_lens_distortion.jpg";
string curve_fit_image_filename = "temp_curve_fit.jpg";
string rectified_image_filename = "temp_rectified.jpg";
string[] lens_distortion_filename = { "lens_distortion0.jpg", "lens_distortion1.jpg" };
string[] curve_fit_filename = { "curve_fit0.jpg", "curve_fit1.jpg" };
string[] rectified_filename = { "rectified0.jpg", "rectified1.jpg" };
int img_width = 0, img_height = 0;
// distance to the centre dot
float dist_to_centre_dot_mm = (float)Math.Sqrt(dotdist_mm * dotdist_mm + height_mm * height_mm);
// find dots within the images
polynomial[] distortion_curve = new polynomial[2];
double[] centre_x = new double[2];
double[] centre_y = new double[2];
double[] scale_factor = new double[2];
double[] rotation = new double[2];
// unrectified position of the calibration dot (in image coordinates) for each image
List<List<double>> centre_dot_position = new List<List<double>>();
// pan/tilt mechanism servo positions
List<List<double>> pan_servo_position = new List<List<double>>();
List<List<double>> tilt_servo_position = new List<List<double>>();
List<int[]> calibration_map = new List<int[]>();
int[] winner_index = new int[2];
for (int cam = 0; cam < image_filename.GetLength(0); cam++)
{
// unrectified centre dot positions in image coordinates
centre_dot_position.Add(new List<double>());
centre_dot_position.Add(new List<double>());
// pan/tilt mechanism servo positions
pan_servo_position.Add(new List<double>());
tilt_servo_position.Add(new List<double>());
double minimum_error = double.MaxValue;
for (int i = 0; i < image_filename[cam].Count; i++)
{
// extract the pan and tilt servo positions of from the filename
double pan = 9999, tilt = 9999;
GetPanTilt(image_filename[cam][i], ref pan, ref tilt);
pan_servo_position[cam].Add(pan);
tilt_servo_position[cam].Add(tilt);
int random_seed = 0;
// detect the dots and calculate a best fit curve
double centre_of_distortion_x = 0;
double centre_of_distortion_y = 0;
polynomial lens_distortion_curve = null;
double camera_rotation = 0;
double scale = 0;
float dot_x = -1, dot_y = -1;
double min_err = double.MaxValue;
int[] calib_map = null;
hypergraph dots =
Detect(image_filename[cam][i],
ref img_width, ref img_height,
fov_degrees, dist_to_centre_dot_mm, dot_spacing_mm,
ref centre_of_distortion_x, ref centre_of_distortion_y,
ref lens_distortion_curve,
ref camera_rotation, ref scale,
lens_distortion_image_filename,
curve_fit_image_filename,
rectified_image_filename,
ref dot_x, ref dot_y,
ref min_err, ref calib_map,
random_seed);
centre_dot_position[cam].Add(dot_x);
centre_dot_position[cam].Add(dot_y);
if (lens_distortion_curve != null)
{
bool update = false;
if (distortion_curve[cam] == null)
{
update = true;
}
else
{
if (min_err < minimum_error)
update = true;
}
if (update)
{
minimum_error = min_err;
// record the result with the smallest RMS error
distortion_curve[cam] = lens_distortion_curve;
centre_x[cam] = centre_of_distortion_x;
centre_y[cam] = centre_of_distortion_y;
rotation[cam] = camera_rotation;
scale_factor[cam] = scale;
if (calibration_map.Count <= cam)
calibration_map.Add(calib_map);
else
calibration_map[cam] = calib_map;
winner_index[cam] = i;
if (File.Exists(lens_distortion_filename[cam])) File.Delete(lens_distortion_filename[cam]);
File.Copy(lens_distortion_image_filename, lens_distortion_filename[cam]);
if (File.Exists(curve_fit_filename[cam])) File.Delete(curve_fit_filename[cam]);
File.Copy(curve_fit_image_filename, curve_fit_filename[cam]);
if (File.Exists(rectified_filename[cam])) File.Delete(rectified_filename[cam]);
File.Copy(rectified_image_filename, rectified_filename[cam]);
}
}
}
}
// positions of the centre dots
List<List<double>> rectified_dots = new List<List<double>>();
for (int cam = 0; cam < image_filename.GetLength(0); cam++)
{
if (distortion_curve[cam] != null)
{
// position in the centre dots in the raw image
List<double> pts = new List<double>();
for (int i = 0; i < centre_dot_position[cam].Count; i += 2)
{
pts.Add(centre_dot_position[cam][i]);
pts.Add(centre_dot_position[cam][i + 1]);
}
// rectified positions for the centre dots
List<double> rectified_pts =
RectifyDots(pts, img_width, img_height,
distortion_curve[cam],
centre_x[cam], centre_y[cam],
rotation[cam], scale_factor[cam]);
rectified_dots.Add(rectified_pts);
}
}
ShowCentreDots(filename[winner_index[0]], rectified_dots, "centre_dots.jpg");
ShowCentreDots(filename[winner_index[0]], centre_dot_position, "centre_dots2.jpg");
if (calibration_map.Count == 2)
{
if ((calibration_map[0] != null) &&
(calibration_map[1] != null))
{
Rectify(image_filename[0][winner_index[0]], image_filename[1][winner_index[0]],
calibration_map[0], calibration_map[1], "Rectification2.jpg");
}
}
if (rectified_dots.Count > 1)
{
if (distortion_curve[0] != null)
{
// find the relative offset in the left and right images
double offset_x = 0;
double offset_y = 0;
bool is_valid = false;
if (distortion_curve[1] != null)
{
int index0 = winner_index[0] * 2;
int index1 = winner_index[1] * 2;
if ((rectified_dots[0].Count < index0 + 1) &&
(rectified_dots[1].Count < index1 + 1))
{
double x0 = rectified_dots[0][index0];
double y0 = rectified_dots[0][index0 + 1];
double x1 = rectified_dots[1][index1];
double y1 = rectified_dots[1][index1 + 1];
offset_x = x1 - x0;
offset_y = y1 - y0;
// calculate the focal length
if (focal_length_pixels < 1)
{
focal_length_pixels = GetFocalLengthFromDisparity((float)dist_to_centre_dot_mm, (float)baseline_mm, (float)offset_x);
Console.WriteLine("Calculated focal length (pixels): " + focal_length_pixels.ToString());
}
// subtract the expected disparity for the centre dot
float expected_centre_dot_disparity = GetDisparityFromDistance(focal_length_pixels, baseline_mm, dist_to_centre_dot_mm);
float check_dist_mm = GetDistanceFromDisparity(focal_length_pixels, baseline_mm, expected_centre_dot_disparity);
//Console.WriteLine("expected_centre_dot_disparity: " + expected_centre_dot_disparity.ToString());
//Console.WriteLine("observed disparity: " + offset_x.ToString());
offset_x -= expected_centre_dot_disparity;
is_valid = true;
}
}
}
}
}
}
}
else
{
Console.WriteLine("No calibration " + file_extension + " images were found");
}
}
/// <summary>
/// returns stereo camera parameters as an xml document
/// </summary>
/// <param name="doc"></param>
/// <param name="parent"></param>
/// <param name="device_name">camera device name</param>
/// <param name="part_number">camera device part number</param>
/// <param name="serial_number">camera device serial number</param>
/// <param name="focal_length_mm">camera focal length in millimetres</param>
/// <param name="pixels_per_mm">number of pixels per millimetre, from the image sensor size/resolution</param>
/// <param name="baseline_mm">stereo baseline in millimetres</param>
/// <param name="fov_degrees">camera field of view in degrees</param>
/// <param name="image_width">camera image width in pixels</param>
/// <param name="image_height">camera image height in pixels</param>
/// <param name="lens_distortion_curve">polynomial describing the lens distortion curve</param>
/// <param name="centre_of_distortion_x">x coordinate of the centre of distortion within the image in pixels</param>
/// <param name="centre_of_distortion_y">y coordinate of the centre of distortion within the image in pixels</param>
/// <param name="minimum_rms_error">polynomial curve fitting</param>
/// <param name="rotation">rotation of the right image relative to the left in degrees</param>
/// <param name="scale"></param>
/// <param name="offset_x">offset from parallel alignment in pixels in the horizontal axis</param>
/// <param name="offset_y">offset from parallel alignment in pixels in the vertical axis</param>
/// <param name="flip_left_image">whether the left image should be flipped (camera mounted inverted)</param>
/// <param name="flip_right_image">whether the right image should be flipped (camera mounted inverted)</param>
/// <returns></returns>
protected static XmlElement getXml(
XmlDocument doc, XmlElement parent,
string device_name,
string part_number,
string serial_number,
float focal_length_mm,
float pixels_per_mm,
float baseline_mm,
float fov_degrees,
int image_width, int image_height,
polynomial[] lens_distortion_curve,
float[] centre_of_distortion_x, float[] centre_of_distortion_y,
float[] minimum_rms_error,
float rotation,
float scale,
float offset_x, float offset_y,
bool disable_rectification,
bool disable_radial_correction,
bool flip_left_image,
bool flip_right_image)
{
//usage.Update("Get xml, BaseVisionStereo, GetXml");
// make sure that floating points are saved in a standard format
IFormatProvider format = new System.Globalization.CultureInfo("en-GB");
// camera parameters
XmlElement nodeStereoCamera = doc.CreateElement("StereoCamera");
parent.AppendChild(nodeStereoCamera);
if ((device_name != null) && (device_name != ""))
{
xml.AddComment(doc, nodeStereoCamera, "Name of the camera device");
xml.AddTextElement(doc, nodeStereoCamera, "DeviceName", device_name);
}
xml.AddComment(doc, nodeStereoCamera, "Supplier of the camera device");
xml.AddTextElement(doc, nodeStereoCamera, "SupplierName", "Surveyor Corporation");
xml.AddComment(doc, nodeStereoCamera, "Part number of the camera device");
xml.AddTextElement(doc, nodeStereoCamera, "PartNumber", part_number);
xml.AddComment(doc, nodeStereoCamera, "Serial number of the camera device");
xml.AddTextElement(doc, nodeStereoCamera, "SerialNumber", serial_number);
xml.AddComment(doc, nodeStereoCamera, "Dimensions of the images in pixels");
xml.AddTextElement(doc, nodeStereoCamera, "ImageDimensions", Convert.ToString(image_width) + "x" + Convert.ToString(image_height));
xml.AddComment(doc, nodeStereoCamera, "Focal length in millimetres");
xml.AddTextElement(doc, nodeStereoCamera, "FocalLengthMillimetres", Convert.ToString(focal_length_mm, format));
xml.AddComment(doc, nodeStereoCamera, "Sensor density in pixels per millimetre");
xml.AddTextElement(doc, nodeStereoCamera, "SensorDensityPixelsPerMillimetre", Convert.ToString(pixels_per_mm, format));
xml.AddComment(doc, nodeStereoCamera, "Camera baseline distance in millimetres");
xml.AddTextElement(doc, nodeStereoCamera, "BaselineMillimetres", Convert.ToString(baseline_mm, format));
xml.AddComment(doc, nodeStereoCamera, "Calibration Data");
XmlElement nodeCalibration = doc.CreateElement("Calibration");
nodeStereoCamera.AppendChild(nodeCalibration);
XmlElement nodeCalibrationPanTilt = doc.CreateElement("PanTiltMechanism");
nodeCalibration.AppendChild(nodeCalibrationPanTilt);
xml.AddComment(doc, nodeCalibrationPanTilt, "Calibration parameters for panning (servo0 servo1 angle0 angle1 standard deviation)");
xml.AddTextElement(doc, nodeCalibrationPanTilt, "PanParameters", "");
xml.AddComment(doc, nodeCalibrationPanTilt, "Calibration parameters for tilting (servo0 servo1 angle0 angle1 standard deviation)");
xml.AddTextElement(doc, nodeCalibrationPanTilt, "TiltParameters", "");
string offsets = Convert.ToString(offset_x, format) + " " +
Convert.ToString(offset_y, format);
xml.AddComment(doc, nodeCalibration, "Image offsets in pixels due to small missalignment from parallel");
xml.AddTextElement(doc, nodeCalibration, "Offsets", offsets);
xml.AddComment(doc, nodeCalibration, "Rotation of the right image relative to the left in degrees");
xml.AddTextElement(doc, nodeCalibration, "RelativeRotationDegrees", Convert.ToString(rotation / (float)Math.PI * 180.0f, format));
xml.AddComment(doc, nodeCalibration, "Scale of the right image relative to the left");
xml.AddTextElement(doc, nodeCalibration, "RelativeImageScale", Convert.ToString(scale, format));
xml.AddComment(doc, nodeCalibration, "Disable all rectification");
xml.AddTextElement(doc, nodeCalibration, "DisableRectification", Convert.ToString(disable_rectification));
xml.AddComment(doc, nodeCalibration, "Disable radial correction, and use offsets and scale/rotation only");
xml.AddTextElement(doc, nodeCalibration, "DisableRadialCorrection", Convert.ToString(disable_radial_correction));
for (int cam = 0; cam < lens_distortion_curve.Length; cam++)
{
bool flip = false;
if (cam == 0)
{
if (flip_left_image) flip = true;
}
else
{
if (flip_right_image) flip = true;
}
XmlElement elem = getCameraXml(
doc, fov_degrees,
lens_distortion_curve[cam],
centre_of_distortion_x[cam], centre_of_distortion_y[cam],
minimum_rms_error[cam],
1.0f, flip);
nodeCalibration.AppendChild(elem);
}
return (nodeStereoCamera);
}
/// <summary>
/// saves camera calibration parameters to a binary file suitable
/// for uploading to the Surveyor SVS
/// </summary>
/// <param name="filename_left">filename for left camera params</param>
/// <param name="filename_right">filename for right camera params</param>
/// <param name="lens_distortion_curve">polynomials</param>
/// <param name="centre_of_distortion_x">centre of distortion</param>
/// <param name="centre_of_distortion_y">centre of distortion</param>
/// <param name="rotation">rotation in radians</param>
/// <param name="scale">scale</param>
/// <param name="offset_x">offset in pixels</param>
/// <param name="offset_y">offset in pixels</param>
public void SaveCameraParameters(
string filename_left,
string filename_right,
polynomial[] lens_distortion_curve,
float[] centre_of_distortion_x,
float[] centre_of_distortion_y,
float rotation,
float scale,
float offset_x, float offset_y)
{
usage.Update("Save camera params, BaseVisionStereo, SaveCameraParameters");
FileStream fs;
BinaryWriter bw;
for (int cam = 0; cam < 2; cam++)
{
if (cam == 0)
fs = File.Open(filename_left, FileMode.Create);
else
fs = File.Open(filename_right, FileMode.Create);
bw = new BinaryWriter(fs);
if (cam == 0)
{
bw.Write((int)0);
bw.Write((int)0);
}
else
{
bw.Write(Convert.ToInt32(-offset_x));
bw.Write(Convert.ToInt32(offset_y));
}
bw.Write(Convert.ToInt32(centre_of_distortion_x[cam]));
bw.Write(Convert.ToInt32(centre_of_distortion_y[cam]));
if ((cam == 0) || (scale == 1))
{
bw.Write((int)1);
bw.Write((int)1);
}
else
{
bw.Write(Convert.ToInt32(scale*6000));
bw.Write((int)6000);
}
int degree=0;
if (lens_distortion_curve[cam] != null)
degree = lens_distortion_curve[cam].GetDegree();
bw.Write(degree);
for (int i = 1; i <= degree; i++)
{
if (lens_distortion_curve[cam] != null)
bw.Write(Convert.ToInt32(Math.Round(lens_distortion_curve[cam].Coeff(i)*10000000)));
else
bw.Write(1);
}
bw.Write((int)image_width);
bw.Write((int)image_height);
bw.Close();
fs.Close();
}
}
/// <summary>
/// return an Xml document containing camera calibration parameters
/// </summary>
/// <param name="device_name"></param>
/// <param name="part_number"></param>
/// <param name="serial_number"></param>
/// <param name="focal_length_pixels"></param>
/// <param name="focal_length_mm"></param>
/// <param name="baseline_mm"></param>
/// <param name="fov_degrees"></param>
/// <param name="image_width"></param>
/// <param name="image_height"></param>
/// <param name="lens_distortion_curve"></param>
/// <param name="centre_of_distortion_x"></param>
/// <param name="centre_of_distortion_y"></param>
/// <param name="minimum_rms_error"></param>
/// <param name="rotation"></param>
/// <param name="scale"></param>
/// <param name="offset_x"></param>
/// <param name="offset_y"></param>
/// <returns></returns>
protected static XmlDocument getXmlDocument(
string device_name,
string part_number,
string serial_number,
float focal_length_pixels,
float focal_length_mm,
float baseline_mm,
float fov_degrees,
int image_width, int image_height,
polynomial[] lens_distortion_curve,
float[] centre_of_distortion_x, float[] centre_of_distortion_y,
float[] minimum_rms_error,
float rotation, float scale,
float offset_x, float offset_y,
bool disable_rectification,
bool disable_radial_correction,
bool flip_left_image,
bool flip_right_image)
{
//usage.Update("Get xml document, BaseVisionStereo, GetXmlDocument");
// Create the document.
XmlDocument doc = new XmlDocument();
// Insert the xml processing instruction and the root node
XmlDeclaration dec = doc.CreateXmlDeclaration("1.0", "ISO-8859-1", null);
doc.PrependChild(dec);
XmlNode commentnode = doc.CreateComment("Sentience 3D Perception System");
doc.AppendChild(commentnode);
XmlElement nodeCalibration = doc.CreateElement("Sentience");
doc.AppendChild(nodeCalibration);
XmlElement elem = getXml(doc, nodeCalibration,
device_name,
part_number,
serial_number,
focal_length_pixels,
focal_length_mm,
baseline_mm,
fov_degrees,
image_width, image_height,
lens_distortion_curve,
centre_of_distortion_x, centre_of_distortion_y,
minimum_rms_error,
rotation, scale,
offset_x, offset_y,
disable_rectification,
disable_radial_correction,
flip_left_image,
flip_right_image);
doc.DocumentElement.AppendChild(elem);
return (doc);
}
/// <summary>
/// save calibration parameters as an xml file
/// </summary>
/// <param name="filename">filename to save as</param>
public void Save(string filename)
{
usage.Update("Save, BaseVisionStereo, Save");
int img_width = image_width;
int img_height = image_height;
if ((rectified[0] != null) &&
(rectified[1] != null))
{
img_width = rectified[0].Width;
img_height = rectified[0].Height;
}
polynomial[] lens_distortion_curve = new polynomial[2];
float[] centre_of_distortion_x = new float[2];
float[] centre_of_distortion_y = new float[2];
float[] minimum_rms_error = new float[2];
for (int cam = 0; cam < 2; cam++)
{
lens_distortion_curve[cam] = calibration_survey[cam].best_fit_curve;
centre_of_distortion_x[cam] = calibration_survey[cam].centre_of_distortion_x;
centre_of_distortion_y[cam] = calibration_survey[cam].centre_of_distortion_y;
minimum_rms_error[cam] = (float)calibration_survey[cam].minimum_rms_error;
}
XmlDocument doc =
getXmlDocument(
device_name,
part_number,
serial_number,
focal_length_mm,
pixels_per_mm,
baseline_mm,
fov_degrees,
img_width, img_height,
lens_distortion_curve,
centre_of_distortion_x, centre_of_distortion_y,
minimum_rms_error,
rotation, scale,
offset_x, offset_y,
disable_rectification,
disable_radial_correction,
flip_left_image,
flip_right_image);
doc.Save(filename);
SaveCameraParameters(
"svs_left",
"svs_right",
lens_distortion_curve,
centre_of_distortion_x,
centre_of_distortion_y,
rotation,
scale,
offset_x,
offset_y);
}
/// <summary>
/// fits a line to the greatest number of aligned spots
/// </summary>
/// <param name="spots">list of detected spot features</param>
/// <param name="ignore_selected_spots">whether to ignore spots which have already been selected in previous line fits</param>
/// <param name="spots_aligned">list of aligned spots</param>
/// <param name="preferred_orientation">preferred direction of the line, in the range 0-2PI, or if set to -1 direction is ignored</param>
/// <param name="orientation_tollerance">max deviation from the preferred tollerance</param>
/// <param name="max_distance">the maximum perpendicular distance below which the spot is considered to touch the line, in the range, typically in the range 0.0-1.0 as a fraction of the spot radius</param>
/// <param name="line_centre_x">x centre point of the line</param>
/// <param name="line_centre_y">y centre point of the line</param>
/// <returns>polynomial line fit</returns>
private polynomial fitLineToSpots(ArrayList spots,
bool ignore_selected_spots,
ref ArrayList spots_aligned,
float preferred_orientation,
float orientation_tollerance,
float max_distance,
ref float line_centre_x,
ref float line_centre_y)
{
polynomial best_fit_line = null;
spots_aligned = null;
line_centre_x = 0;
line_centre_y = 0;
// find the maximum number of aligned spots
ArrayList max_spots_aligned =
MaxAlignedSpots(ignore_selected_spots,
preferred_orientation,
orientation_tollerance,
max_distance);
if (max_spots_aligned != null)
{
spots_aligned = max_spots_aligned;
if (max_spots_aligned.Count > 0)
{
// get the position of the centre of the line
for (int i = 0; i < max_spots_aligned.Count; i++)
{
blob spot = (blob)max_spots_aligned[i];
line_centre_x += spot.interpolated_x;
line_centre_y += spot.interpolated_y;
}
line_centre_x /= max_spots_aligned.Count;
line_centre_y /= max_spots_aligned.Count;
// fit a line to the points
best_fit_line = new polynomial();
best_fit_line.SetDegree(1);
for (int i = 0; i < max_spots_aligned.Count; i++)
{
blob spot = (blob)max_spots_aligned[i];
float dx = spot.interpolated_x - line_centre_x;
float dy = spot.interpolated_y - line_centre_y;
best_fit_line.AddPoint(dx, dy);
spot.selected = true;
}
// solve the line equation
best_fit_line.Solve();
}
}
return (best_fit_line);
}
private static void ShowRectifiedImage(Bitmap bmp,
float centre_of_distortion_x,
float centre_of_distortion_y,
polynomial distortion_curve,
ref Bitmap rectified)
{
byte[] img = new byte[bmp.Width * bmp.Height * 3];
BitmapArrayConversions.updatebitmap(bmp, img);
byte[] img_rectified = (byte[])img.Clone();
if (distortion_curve != null)
{
int[] calibration_map = null;
int[,,] calibration_map_inverse = null;
updateCalibrationMap(
bmp.Width, bmp.Height, distortion_curve,
1, 0,
centre_of_distortion_x,centre_of_distortion_y,
0, 0,
ref calibration_map, ref calibration_map_inverse);
int n = 0;
for (int i = 0; i < img.Length; i+=3, n++)
{
int index = calibration_map[n]*3;
for (int col = 0; col < 3; col++)
img_rectified[i+col] = img[index+col];
}
}
if (rectified == null)
rectified = new Bitmap(bmp.Width, bmp.Height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
BitmapArrayConversions.updatebitmap_unsafe(img_rectified, rectified);
}
public void Update(
int image_width,
int image_height,
CalibrationDot[,] grid)
{
if (survey_updates < test_interval)
{
int cx = image_width / 2;
int cy = image_height / 2;
int radius_pixels = image_width * radius_percent / 100;
int radius_pixels_sqr = radius_pixels*radius_pixels;
int diameter_pixels = radius_pixels * 2;
int tx = cx - radius_pixels;
int bx = tx + diameter_pixels;
int ty = cy - radius_pixels;
int by = ty + diameter_pixels;
if (survey == null) survey = new polynomial[(diameter_pixels*2)+1, (diameter_pixels*2)+1];
if (survey.GetLength(0) != diameter_pixels) survey = new polynomial[(diameter_pixels*2)+1, (diameter_pixels*2)+1];
for (float centre_x = tx; centre_x <= bx; centre_x += 0.5f)
{
float dcx = centre_x - cx;
dcx*= dcx;
for (float centre_y = ty; centre_y <= by; centre_y += 0.5f)
{
float dcy = centre_y - cy;
dcy *= dcy;
float r = dcx*dcx + dcy*dcy;
if (r < radius_pixels_sqr)
{
int xx = (int)((centre_x -tx)*2);
int yy = (int)((centre_y -ty)*2);
// get the curve associated with this possible centre of distortion
if (survey[xx, yy] == null)
{
polynomial p = new polynomial();
p.SetDegree(degree);
survey[xx, yy] = p;
}
polynomial curve = survey[xx, yy];
for (int grid_x = 0; grid_x < grid.GetLength(0); grid_x++)
{
for (int grid_y = 0; grid_y < grid.GetLength(1); grid_y++)
{
CalibrationDot dot = grid[grid_x, grid_y];
if (dot != null)
{
if (dot.rectified_x > 0)
{
double dx = dot.x - centre_x;
double dy = dot.y - centre_y;
double actual_radial_dist = Math.Sqrt(dx*dx + dy*dy);
dx = dot.rectified_x - centre_x;
dy = dot.rectified_y - centre_y;
double rectified_radial_dist = Math.Sqrt(dx*dx + dy*dy);
curve.AddPoint(rectified_radial_dist, actual_radial_dist);
}
}
}
}
}
}
}
survey_updates++;
if (survey_updates >= test_interval)
{
FindBestCurve(tx, ty);
survey = null;
survey_updates = 0;
}
}
}
private static void DetectLensDistortion(int image_width, int image_height,
CalibrationDot[,] grid,
grid2D overlay_grid,
List<List<double>> lines,
ref polynomial curve,
ref CalibrationDot centre_of_distortion,
ref List<List<double>> best_rectified_lines,
int grid_offset_x, int grid_offset_y,
double scale,
ref double minimum_error,
int random_seed)
{
double centre_of_distortion_search_radius = image_width / 100f; //80.0f;
centre_of_distortion = new CalibrationDot();
curve = FitCurve(image_width, image_height,
grid, overlay_grid,
centre_of_distortion,
centre_of_distortion_search_radius,
grid_offset_x, grid_offset_y, lines,
ref minimum_error,
random_seed);
if (curve != null)
{
double rotation = 0;
best_rectified_lines =
RectifyLines(lines,
image_width, image_height,
curve,
centre_of_distortion,
rotation, scale);
}
}
/// <summary>
/// return an xml element containing camera calibration parameters
/// </summary>
/// <param name="doc"></param>
/// <param name="fov_degrees">field of view in degrees</param>
/// <param name="lens_distortion_curve">polynomial describing the lens distortion</param>
/// <param name="centre_of_distortion_x">x coordinate of the centre of distortion within the image in pixels</param>
/// <param name="centre_of_distortion_y">y coordinate of the centre of distortion within the image in pixels</param>
/// <param name="minimum_rms_error">minimum curve fitting error in pixels</param>
/// <param name="scale"></param>
/// <param name="flip_image">whether to flip the image (camera mounted inverted)</param>
/// <returns></returns>
protected static XmlElement getCameraXml(
XmlDocument doc,
float fov_degrees,
polynomial lens_distortion_curve,
float centre_of_distortion_x, float centre_of_distortion_y,
float minimum_rms_error,
float scale,
bool flip_image)
{
//usage.Update("Get camera Xml, BaseVisionStereo, GetCameraXml");
// make sure that floating points are saved in a standard format
IFormatProvider format = new System.Globalization.CultureInfo("en-GB");
string coefficients = "";
if (lens_distortion_curve != null)
{
int degree = lens_distortion_curve.GetDegree();
for (int i = 0; i <= degree; i++)
{
coefficients += Convert.ToString(lens_distortion_curve.Coeff(i), format);
if (i < degree) coefficients += " ";
}
}
else coefficients = "0,0,0";
XmlElement elem = doc.CreateElement("Camera");
doc.DocumentElement.AppendChild(elem);
xml.AddComment(doc, elem, "Horizontal field of view of the camera in degrees");
xml.AddTextElement(doc, elem, "FieldOfViewDegrees", Convert.ToString(fov_degrees, format));
xml.AddComment(doc, elem, "The centre of distortion in pixels");
xml.AddTextElement(doc, elem, "CentreOfDistortion", Convert.ToString(centre_of_distortion_x, format) + " " + Convert.ToString(centre_of_distortion_y, format));
xml.AddComment(doc, elem, "Polynomial coefficients used to describe the camera lens distortion");
xml.AddTextElement(doc, elem, "DistortionCoefficients", coefficients);
xml.AddComment(doc, elem, "The minimum RMS error between the distortion curve and plotted points");
xml.AddTextElement(doc, elem, "RMSerror", Convert.ToString(minimum_rms_error, format));
xml.AddComment(doc, elem, "Scaling factor");
xml.AddTextElement(doc, elem, "Scale", Convert.ToString(scale, format));
if (flip_image)
{
xml.AddComment(doc, elem, "Whether to flip the image (camera mounted inverted)");
xml.AddTextElement(doc, elem, "Flip", Convert.ToString(flip_image));
}
return (elem);
}
public static void Test()
{
string filename = "/home/motters/calibrationdata/forward2/raw1_5000_2000.jpg";
//string filename = "c:\\develop\\sentience\\calibrationimages\\raw0_5000_2000.jpg";
//string filename = "c:\\develop\\sentience\\calibrationimages\\raw1_5250_2000.jpg";
float dotdist_mm = 525;
float height_mm = 550;
float dist_to_centre_dot_mm = (float)Math.Sqrt(dotdist_mm * dotdist_mm + height_mm * height_mm);
float dot_spacing_mm = 50;
double[] centre_of_distortion_x = new double[1];
double[] centre_of_distortion_y = new double[1];
polynomial[] lens_distortion_curve = new polynomial[1];
double[] camera_rotation = new double[1];
double[] scale = new double[1];
float dot_x = -1, dot_y = -1;
float focal_length_pixels = 300;
float baseline_mm = 100;
float fov_degrees = 78;
string[] lens_distortion_filename = { "lens_distortion.jpg" };
string[] curve_fit_filename = { "curve_fit.jpg" };
string[] rectified_filename = { "rectified.jpg" };
int image_width = 640;
int image_height = 480;
double minimum_error = 0;
//double pan_angle = 0;
//double tilt_angle = 0;
//GetPanTilt(filename, ref pan_angle, ref tilt_angle);
float expected_centre_dot_disparity = GetDisparityFromDistance(focal_length_pixels, baseline_mm, dist_to_centre_dot_mm);
float check_dist_mm = GetDistanceFromDisparity(focal_length_pixels, baseline_mm, expected_centre_dot_disparity);
//Console.WriteLine("dist_to_centre_dot_mm: " + dist_to_centre_dot_mm.ToString());
//Console.WriteLine("check_dist_mm: " + check_dist_mm.ToString());
//Console.WriteLine("expected_centre_dot_disparity: " + expected_centre_dot_disparity.ToString());
/*
Detect(filename,
ref image_width, ref image_height,
fov_degrees, dist_to_centre_dot_mm, dot_spacing_mm,
ref centre_of_distortion_x[0], ref centre_of_distortion_y[0],
ref lens_distortion_curve[0],
ref camera_rotation[0], ref scale[0],
lens_distortion_filename[0],
curve_fit_filename[0],
rectified_filename[0],
ref dot_x, ref dot_y,
ref minimum_error);
Save("calibration.xml", "Test", focal_length_mm, baseline_mm, fov_degrees,
image_width, image_height, lens_distortion_curve,
centre_of_distortion_x, centre_of_distortion_y, camera_rotation, scale,
lens_distortion_filename, curve_fit_filename,
0, 0);
*/
}
/// <summary>
/// applies the given curve to the lines to produce rectified coordinates
/// </summary>
/// <param name="lines"></param>
/// <param name="distortion_curve"></param>
/// <param name="centre_of_distortion"></param>
/// <returns></returns>
private static List<List<double>> RectifyLines(List<List<double>> lines,
int image_width, int image_height,
polynomial curve,
CalibrationDot centre_of_distortion,
double rotation,
double scale)
{
List<List<double>> rectified_lines = new List<List<double>>();
float half_width = image_width / 2;
float half_height = image_height / 2;
for (int i = 0; i < lines.Count; i++)
{
List<double> line = lines[i];
List<double> rectified_line = new List<double>();
for (int j = 0; j < line.Count; j += 2)
{
double x = line[j];
double y = line[j + 1];
double dx = x - centre_of_distortion.x;
double dy = y - centre_of_distortion.y;
double radial_dist_rectified = Math.Sqrt((dx * dx) + (dy * dy));
if (radial_dist_rectified >= 0.01f)
{
double radial_dist_original = curve.RegVal(radial_dist_rectified);
if (radial_dist_original > 0)
{
double ratio = radial_dist_rectified / radial_dist_original;
double x2 = Math.Round(centre_of_distortion.x + (dx * ratio));
x2 = (x2 - (image_width / 2)) * scale;
double y2 = Math.Round(centre_of_distortion.y + (dy * ratio));
y2 = (y2 - (image_height / 2)) * scale;
// apply rotation
double rectified_x = x2, rectified_y = y2;
rotatePoint(x2, y2, -rotation, ref rectified_x, ref rectified_y);
rectified_x += half_width;
rectified_y += half_height;
rectified_line.Add(rectified_x);
rectified_line.Add(rectified_y);
}
}
}
if (rectified_line.Count > 6)
rectified_lines.Add(rectified_line);
}
return (rectified_lines);
}
/// <summary>
/// shows the lens distortion model
/// </summary>
/// <param name="img_width">width of the image</param>
/// <param name="img_height">height of the image</param>
/// <param name="centre_of_distortion">coordinates for the centre of distortion</param>
/// <param name="curve">distortion curve</param>
/// <param name="FOV_degrees">horizontal field of view in degrees</param>
/// <param name="increment_degrees">increment for concentric circles in degrees</param>
/// <param name="output_filename">filename to save as</param>
private static void ShowLensDistortion(int img_width, int img_height,
CalibrationDot centre_of_distortion,
polynomial curve,
float FOV_degrees, float increment_degrees,
string output_filename)
{
byte[] img = new byte[img_width * img_height * 3];
for (int i = 0; i < img.Length; i++) img[i] = 255;
float max_radius = img_width / 2;
float total_difference = 0;
for (float r = 0; r < max_radius * 1.0f; r += 0.2f)
{
float diff = (float)Math.Abs(r - curve.RegVal(r));
total_difference += diff;
}
if (total_difference > 0)
{
int rr, gg, bb;
float diff_sum = 0;
for (float r = 0; r < max_radius * 1.8f; r += 0.2f)
{
float original_r = (float)curve.RegVal(r);
float d1 = r - original_r;
diff_sum += Math.Abs(d1);
float diff = diff_sum / total_difference;
if (diff > 1.0f) diff = 1.0f;
byte difference = (byte)(50 + (diff * 205));
if (d1 >= 0)
{
rr = difference;
gg = difference;
bb = difference;
}
else
{
rr = difference;
gg = difference;
bb = difference;
}
drawing.drawCircle(img, img_width, img_height,
(float)centre_of_distortion.x,
(float)centre_of_distortion.y,
r, rr, gg, bb, 1, 300);
}
}
float increment = max_radius * increment_degrees / FOV_degrees;
float angle_degrees = increment_degrees;
for (float r = increment; r <= max_radius * 150 / 100; r += increment)
{
float radius = (float)curve.RegVal(r);
drawing.drawCircle(img, img_width, img_height,
(float)centre_of_distortion.x,
(float)centre_of_distortion.y,
radius, 0, 0, 0, 0, 360);
drawing.AddText(img, img_width, img_height, angle_degrees.ToString(),
"Courier New", 10, 0, 0, 0,
(int)centre_of_distortion.x + (int)radius + 10, (int)centre_of_distortion.y);
angle_degrees += increment_degrees;
}
int incr = img_width / 20;
for (int x = 0; x < img_width; x += incr)
{
for (int y = 0; y < img_height; y += incr)
{
float dx = x - (float)centre_of_distortion.x;
float dy = y - (float)centre_of_distortion.y;
float radius = (float)Math.Sqrt(dx * dx + dy * dy);
float tot = 0;
for (float r = 0; r < radius; r += 0.2f)
{
float diff = (float)Math.Abs(r - curve.RegVal(r));
tot += diff;
}
float r1 = (float)Math.Abs((radius - 2f) - curve.RegVal(radius - 2f));
float r2 = (float)Math.Abs(radius - curve.RegVal(radius));
float fraction = 1.0f + (Math.Abs(r2 - r1) * img_width * 1.5f / total_difference);
int x2 = (int)(centre_of_distortion.x + (dx * fraction));
int y2 = (int)(centre_of_distortion.y + (dy * fraction));
drawing.drawLine(img, img_width, img_height, x, y, x2, y2, 255, 0, 0, 0, false);
}
}
drawing.drawLine(img, img_width, img_height, 0, (int)centre_of_distortion.y, img_width - 1, (int)centre_of_distortion.y, 0, 0, 0, 0, false);
drawing.drawLine(img, img_width, img_height, (int)centre_of_distortion.x, 0, (int)centre_of_distortion.x, img_height - 1, 0, 0, 0, 0, false);
Bitmap output_bmp = new Bitmap(img_width, img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
BitmapArrayConversions.updatebitmap_unsafe(img, output_bmp);
if (output_filename.ToLower().EndsWith("jpg"))
output_bmp.Save(output_filename, System.Drawing.Imaging.ImageFormat.Jpeg);
if (output_filename.ToLower().EndsWith("bmp"))
output_bmp.Save(output_filename, System.Drawing.Imaging.ImageFormat.Bmp);
}
/// <summary>
/// applies the given curve to the lines to produce rectified coordinates
/// </summary>
/// <param name="lines"></param>
/// <param name="distortion_curve"></param>
/// <param name="centre_of_distortion"></param>
/// <returns></returns>
private static List<double> RectifyDots(List<double> dots,
int image_width, int image_height,
polynomial curve,
double centre_of_distortion_x,
double centre_of_distortion_y,
double rotation,
double scale)
{
List<double> rectified_dots = new List<double>();
float half_width = image_width / 2;
float half_height = image_height / 2;
List<double> rectified_line = new List<double>();
for (int j = 0; j < dots.Count; j += 2)
{
double x = dots[j];
double y = dots[j + 1];
double dx = x - centre_of_distortion_x;
double dy = y - centre_of_distortion_y;
double radial_dist_rectified = Math.Sqrt((dx * dx) + (dy * dy));
if (radial_dist_rectified >= 0.01f)
{
double radial_dist_original = curve.RegVal(radial_dist_rectified);
if (radial_dist_original > 0)
{
double ratio = radial_dist_rectified / radial_dist_original;
double x2 = Math.Round(centre_of_distortion_x + (dx * ratio));
x2 = (x2 - (image_width / 2)) * scale;
double y2 = Math.Round(centre_of_distortion_y + (dy * ratio));
y2 = (y2 - (image_height / 2)) * scale;
// apply rotation
double rectified_x = x2, rectified_y = y2;
rotatePoint(x2, y2, -rotation, ref rectified_x, ref rectified_y);
rectified_x += half_width;
rectified_y += half_height;
rectified_dots.Add(rectified_x);
rectified_dots.Add(rectified_y);
}
}
}
return (rectified_dots);
}
/// <summary>
/// return an Xml document containing camera calibration parameters
/// </summary>
/// <param name="device_name"></param>
/// <param name="focal_length_pixels"></param>
/// <param name="baseline_mm"></param>
/// <param name="fov_degrees"></param>
/// <param name="image_width"></param>
/// <param name="image_height"></param>
/// <param name="lens_distortion_curve"></param>
/// <param name="centre_of_distortion_x"></param>
/// <param name="centre_of_distortion_y"></param>
/// <param name="rotation"></param>
/// <param name="scale"></param>
/// <param name="lens_distortion_image_filename"></param>
/// <param name="curve_fit_image_filename"></param>
/// <param name="offset_x"></param>
/// <param name="offset_y"></param>
/// <param name="pan_curve"></param>
/// <param name="pan_offset_x"></param>
/// <param name="pan_offset_y"></param>
/// <param name="tilt_curve"></param>
/// <param name="tilt_offset_x"></param>
/// <param name="tilt_offset_y"></param>
/// <returns></returns>
private static XmlDocument getXmlDocument(
string device_name,
float focal_length_pixels,
float baseline_mm,
float fov_degrees,
int image_width, int image_height,
polynomial[] lens_distortion_curve,
double[] centre_of_distortion_x, double[] centre_of_distortion_y,
double[] rotation, double[] scale,
string[] lens_distortion_image_filename,
string[] curve_fit_image_filename,
float offset_x, float offset_y,
polynomial pan_curve, float pan_offset_x, float pan_offset_y,
polynomial tilt_curve, float tilt_offset_x, float tilt_offset_y)
{
// Create the document.
XmlDocument doc = new XmlDocument();
// Insert the xml processing instruction and the root node
XmlDeclaration dec = doc.CreateXmlDeclaration("1.0", "ISO-8859-1", null);
doc.PrependChild(dec);
XmlNode commentnode = doc.CreateComment("Sentience 3D Perception System");
doc.AppendChild(commentnode);
XmlElement nodeCalibration = doc.CreateElement("Sentience");
doc.AppendChild(nodeCalibration);
xml.AddComment(doc, nodeCalibration, "Sentience");
XmlElement elem = getXml(doc, nodeCalibration,
device_name,
focal_length_pixels,
baseline_mm,
fov_degrees,
image_width, image_height,
lens_distortion_curve,
centre_of_distortion_x, centre_of_distortion_y,
rotation, scale,
lens_distortion_image_filename,
curve_fit_image_filename,
offset_x, offset_y,
pan_curve, pan_offset_x, pan_offset_y,
tilt_curve, tilt_offset_x, tilt_offset_y);
doc.DocumentElement.AppendChild(elem);
return (doc);
}
private static double DetectCameraRotation(int image_width, int image_height,
CalibrationDot[,] grid,
polynomial curve,
CalibrationDot centre_of_distortion,
ref List<List<double>> rectified_centre_line,
double scale)
{
double rotation = 0;
List<List<double>> centre_line = new List<List<double>>();
List<double> line = new List<double>();
// get the vertical centre line within the image
for (int grid_y = 0; grid_y < grid.GetLength(1); grid_y++)
{
int grid_x = 0;
bool found = false;
while ((grid_x < grid.GetLength(0)) && (!found))
{
if (grid[grid_x, grid_y] != null)
{
if (grid[grid_x, grid_y].grid_x == 0)
{
line.Add(grid[grid_x, grid_y].x);
line.Add(grid[grid_x, grid_y].y);
found = true;
}
}
grid_x++;
}
}
centre_line.Add(line);
// rectify the centre line
rectified_centre_line =
RectifyLines(centre_line, image_width, image_height,
curve, centre_of_distortion, 0, scale);
if (rectified_centre_line != null)
{
if (rectified_centre_line.Count > 0)
{
double[] px = new double[2];
double[] py = new double[2];
int[] hits = new int[2];
line = rectified_centre_line[0];
for (int i = 0; i < line.Count; i += 2)
{
double x = line[i];
double y = line[i + 1];
if (i < line.Count / 2)
{
px[0] += x;
py[0] += y;
hits[0]++;
}
else
{
px[1] += x;
py[1] += y;
hits[1]++;
}
}
if ((hits[0] > 0) && (hits[1] > 0))
{
px[0] /= hits[0];
py[0] /= hits[0];
px[1] /= hits[1];
py[1] /= hits[1];
double dx = px[1] - px[0];
double dy = py[1] - py[0];
double length = Math.Sqrt(dx * dx + dy * dy);
if (length > 0)
rotation = Math.Asin(dx / length);
}
}
}
return (rotation);
}
/// <summary>
/// returns settings for either pan or tilt axis in Xml format
/// </summary>
/// <param name="doc"></param>
/// <param name="parent"></param>
/// <param name="axis_name"></param>
/// <param name="curve"></param>
/// <param name="offset_x"></param>
/// <param name="offset_y"></param>
/// <returns></returns>
private static XmlElement getPanTiltAxis(
XmlDocument doc, XmlElement parent,
string axis_name,
polynomial curve, float offset_x, float offset_y)
{
// make sure that floating points are saved in a standard format
IFormatProvider format = new System.Globalization.CultureInfo("en-GB");
// pan/tilt mechanism parameters
XmlElement nodeAxis = doc.CreateElement("Axis");
xml.AddTextElement(doc, nodeAxis, "AxisName", axis_name);
string coefficients = "";
for (int i = 0; i <= curve.GetDegree(); i++)
{
coefficients += Convert.ToSingle(curve.Coeff(i), format);
if (i < curve.GetDegree()) coefficients += ",";
}
xml.AddTextElement(doc, nodeAxis, "Coefficients", coefficients);
xml.AddTextElement(doc, nodeAxis, "Offsets", Convert.ToString(offset_x, format) + "," + Convert.ToString(offset_y, format));
parent.AppendChild(nodeAxis);
return (nodeAxis);
}
/// <summary>
/// fits a curve to the given grid using the given centre of distortion
/// </summary>
/// <param name="image_width">width of the image in pixels</param>
/// <param name="image_height">height of the image in pixels</param>
/// <param name="grid">detected grid dots</param>
/// <param name="overlay_grid">overlayed ideal rectified grid</param>
/// <param name="centre_of_distortion">centre of lens distortion</param>
/// <param name="centre_of_distortion_search_radius">search radius for the centre of distortion</param>
/// <returns>fitted curve</returns>
private static polynomial FitCurve(int image_width, int image_height,
CalibrationDot[,] grid,
grid2D overlay_grid,
CalibrationDot centre_of_distortion,
double centre_of_distortion_search_radius,
int grid_offset_x, int grid_offset_y,
List<List<double>> lines,
ref double minimum_error,
int random_seed)
{
double overall_minimum_error = double.MaxValue;
double centre_of_distortion_x=0, centre_of_distortion_y=0;
polynomial overall_best_curve = null;
polynomial best_curve = null;
for (int rand_pass = random_seed; rand_pass < random_seed + 3; rand_pass++)
{
minimum_error = double.MaxValue;
double search_min_error = minimum_error;
int degrees = 3;
int best_degrees = degrees;
List<double> prev_minimum_error = new List<double>();
prev_minimum_error.Add(minimum_error);
double increment = 3.0f;
double noise = increment / 2;
best_curve = null;
double search_radius = (float)centre_of_distortion_search_radius;
double half_width = image_width / 2;
double half_height = image_height / 2;
double half_noise = noise / 2;
double max_radius_sqr = centre_of_distortion_search_radius * centre_of_distortion_search_radius;
int scaled_up = 0;
List<double> result = new List<double>();
double best_cx = half_width, best_cy = half_height;
float maxerr = (image_width / 2) * (image_width / 2);
Random rnd = new Random(rand_pass);
int max_passes = 1000;
for (int pass = 0; pass < max_passes; pass++)
{
double centre_x = 0;
double centre_y = 0;
double mass = 0;
for (double cx = half_width - search_radius; cx < half_width + search_radius; cx += increment)
{
double dx = cx - half_width;
for (double cy = half_height - search_radius; cy < half_height + search_radius; cy += increment)
{
double dy = cy - half_height;
double dist = dx * dx + dy * dy;
if (dist < max_radius_sqr)
{
polynomial curve = new polynomial();
curve.SetDegree(degrees);
centre_of_distortion.x = cx + (rnd.NextDouble() * noise) - half_noise;
centre_of_distortion.y = cy + (rnd.NextDouble() * noise) - half_noise;
FitCurve(grid, overlay_grid, centre_of_distortion, curve, noise, rnd, grid_offset_x, grid_offset_y);
// do a sanity check on the curve
if (ValidCurve(curve, image_width))
{
double error = curve.GetMeanError();
error = error * error;
if (error > 0.001)
{
error = maxerr - error; // inverse
if (error > 0)
{
centre_x += centre_of_distortion.x * error;
centre_y += centre_of_distortion.y * error;
mass += error;
}
}
}
}
}
}
if (mass > 0)
{
centre_x /= mass;
centre_y /= mass;
centre_of_distortion.x = centre_x;
centre_of_distortion.y = centre_y;
polynomial curve2 = new polynomial();
curve2.SetDegree(degrees);
FitCurve(grid, overlay_grid, centre_of_distortion, curve2, noise, rnd, grid_offset_x, grid_offset_y);
double mean_error = curve2.GetMeanError();
double scaledown = 0.99999999999999999;
if (mean_error < search_min_error)
{
search_min_error = mean_error;
// cool down
prev_minimum_error.Add(search_min_error);
search_radius *= scaledown;
increment *= scaledown;
noise = increment / 2;
half_noise = noise / 2;
half_width = centre_x;
half_height = centre_y;
if (mean_error < minimum_error)
{
best_cx = half_width;
best_cy = half_height;
minimum_error = mean_error;
Console.WriteLine("Cool " + pass.ToString() + ": " + mean_error.ToString());
if (max_passes - pass < 500) max_passes += 500;
best_degrees = degrees;
best_curve = curve2;
}
scaled_up = 0;
}
else
{
// heat up
double scaleup = 1.0 / scaledown;
search_radius /= scaledown;
increment /= scaledown;
noise = increment / 2;
half_noise = noise / 2;
scaled_up++;
half_width = best_cx + (rnd.NextDouble() * noise) - half_noise;
half_height = best_cy + (rnd.NextDouble() * noise) - half_noise;
if (prev_minimum_error.Count > 0)
{
minimum_error = prev_minimum_error[prev_minimum_error.Count - 1];
prev_minimum_error.RemoveAt(prev_minimum_error.Count - 1);
}
}
result.Add(mean_error);
}
}
minimum_error = Math.Sqrt(minimum_error);
centre_of_distortion.x = best_cx;
centre_of_distortion.y = best_cy;
if (best_curve != null)
minimum_error = best_curve.GetMeanError();
if (minimum_error < overall_minimum_error)
{
overall_minimum_error = minimum_error;
centre_of_distortion_x = best_cx;
centre_of_distortion_y = best_cy;
overall_best_curve = best_curve;
}
}
overall_minimum_error = minimum_error;
centre_of_distortion.x = centre_of_distortion_x;
centre_of_distortion.y = centre_of_distortion_y;
best_curve = overall_best_curve;
return (best_curve);
}
/// <summary>
/// return an xml element containing camera calibration parameters
/// </summary>
/// <param name="doc"></param>
/// <param name="fov_degrees"></param>
/// <param name="image_width"></param>
/// <param name="image_height"></param>
/// <param name="lens_distortion_curve"></param>
/// <param name="centre_of_distortion_x"></param>
/// <param name="centre_of_distortion_y"></param>
/// <param name="rotation"></param>
/// <param name="scale"></param>
/// <param name="lens_distortion_image_filename"></param>
/// <param name="curve_fit_image_filename"></param>
/// <returns></returns>
private static XmlElement getCameraXml(
XmlDocument doc,
float fov_degrees,
int image_width, int image_height,
polynomial lens_distortion_curve,
double centre_of_distortion_x, double centre_of_distortion_y,
double rotation, double scale,
string lens_distortion_image_filename,
string curve_fit_image_filename)
{
// make sure that floating points are saved in a standard format
IFormatProvider format = new System.Globalization.CultureInfo("en-GB");
string coefficients = "";
if (lens_distortion_curve != null)
{
int degree = lens_distortion_curve.GetDegree();
for (int i = 0; i <= degree; i++)
{
coefficients += Convert.ToString(lens_distortion_curve.Coeff(i), format);
if (i < degree) coefficients += ",";
}
}
else coefficients = "0,0,0";
XmlElement elem = doc.CreateElement("Camera");
doc.DocumentElement.AppendChild(elem);
xml.AddComment(doc, elem, "Horizontal field of view of the camera in degrees");
xml.AddTextElement(doc, elem, "FieldOfViewDegrees", Convert.ToString(fov_degrees, format));
xml.AddComment(doc, elem, "Image dimensions in pixels");
xml.AddTextElement(doc, elem, "ImageDimensions", Convert.ToString(image_width, format) + "," + Convert.ToString(image_height, format));
xml.AddComment(doc, elem, "The centre of distortion in pixels");
xml.AddTextElement(doc, elem, "CentreOfDistortion", Convert.ToString(centre_of_distortion_x, format) + "," + Convert.ToString(centre_of_distortion_y, format));
xml.AddComment(doc, elem, "Polynomial coefficients used to describe the camera lens distortion");
xml.AddTextElement(doc, elem, "DistortionCoefficients", coefficients);
xml.AddComment(doc, elem, "Scaling factor");
xml.AddTextElement(doc, elem, "Scale", Convert.ToString(scale));
xml.AddComment(doc, elem, "Rotation of the image in degrees");
xml.AddTextElement(doc, elem, "RotationDegrees", Convert.ToString(rotation / (float)Math.PI * 180.0f));
xml.AddComment(doc, elem, "The minimum RMS error between the distortion curve and plotted points");
xml.AddTextElement(doc, elem, "RMSerror", Convert.ToString(lens_distortion_curve.GetRMSerror(), format));
xml.AddComment(doc, elem, "Image showing the lens distortion");
xml.AddTextElement(doc, elem, "DistortionImageFilename", lens_distortion_image_filename);
xml.AddComment(doc, elem, "Image showing the best fit curve");
xml.AddTextElement(doc, elem, "CurveFitImageFilename", curve_fit_image_filename);
return (elem);
}
private static bool ValidCurve(polynomial curve, int image_width)
{
int max_radius = image_width / 2;
int half_radius = max_radius / 2;
double diff1 = curve.RegVal(half_radius) - half_radius;
double diff2 = curve.RegVal(max_radius) - max_radius;
if (diff2 > diff1)
return (false);
else
return (true);
}
/// <summary>
/// update the calibration lookup table, which maps pixels
/// in the rectified image into the original image
/// </summary>
/// <param name="image_width"></param>
/// <param name="image_height"></param>
/// <param name="curve">polynomial curve describing the lens distortion</param>
/// <param name="scale"></param>
/// <param name="rotation"></param>
/// <param name="centre_of_distortion_x"></param>
/// <param name="centre_of_distortion_y"></param>
/// <param name="calibration_map"></param>
/// <param name="calibration_map_inverse"></param>
private static void updateCalibrationMap(int image_width,
int image_height,
polynomial curve,
float scale,
float rotation,
float centre_of_distortion_x,
float centre_of_distortion_y,
ref int[] calibration_map,
ref int[, ,] calibration_map_inverse)
{
int half_width = image_width / 2;
int half_height = image_height / 2;
calibration_map = new int[image_width * image_height];
calibration_map_inverse = new int[image_width, image_height, 2];
for (int x = 0; x < image_width; x++)
{
float dx = x - centre_of_distortion_x;
for (int y = 0; y < image_height; y++)
{
float dy = y - centre_of_distortion_y;
float radial_dist_rectified = (float)Math.Sqrt((dx * dx) + (dy * dy));
if (radial_dist_rectified >= 0.01f)
{
double radial_dist_original = curve.RegVal(radial_dist_rectified);
if (radial_dist_original > 0)
{
double ratio = radial_dist_original / radial_dist_rectified;
float x2 = (float)Math.Round(centre_of_distortion_x + (dx * ratio));
x2 = (x2 - (image_width / 2)) * scale;
float y2 = (float)Math.Round(centre_of_distortion_y + (dy * ratio));
y2 = (y2 - (image_height / 2)) * scale;
// apply rotation
double x3 = x2, y3 = y2;
rotatePoint(x2, y2, -rotation, ref x3, ref y3);
x3 += half_width;
y3 += half_height;
if (((int)x3 > -1) && ((int)x3 < image_width) &&
((int)y3 > -1) && ((int)y3 < image_height))
{
int n = (y * image_width) + x;
int n2 = ((int)y3 * image_width) + (int)x3;
calibration_map[n] = n2;
calibration_map_inverse[(int)x3, (int)y3, 0] = x;
calibration_map_inverse[(int)x3, (int)y3, 1] = y;
}
}
}
}
}
}
private void FindBestCurve(int tx, int ty)
{
for (int x = 0; x < survey.GetLength(0); x++)
{
for (int y = 0; y < survey.GetLength(1); y++)
{
polynomial p = survey[x,y];
if (p != null)
{
p.Solve();
double rms_error = p.GetRMSerror();
if (rms_error < minimum_rms_error)
{
best_fit_curve = p;
minimum_rms_error = rms_error;
centre_of_distortion_x = tx + (x/2.0f);
centre_of_distortion_y = ty + (y/2.0f);
}
}
}
}
Console.WriteLine("Minimum RMS error: " + minimum_rms_error.ToString());
}