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()); }