public MorphologyCraterCallback(VertexShaderCrater vShaderCrater) { p1 = new polynomial(); p2 = new polynomial(); g = new gaussian(); mVShaderCrater = vShaderCrater; }
// this should only be a casting, but c# sucks! public static void convertToFloatArray(float x,float y, polynomial mP1,polynomial mP2, gaussian mG,ref float[] o) { // float[] if(o==null) o = new float[21]; o[0] = 0; o[1] = x; o[2] = y; o[3] = mP1.min; o[4] = mP1.max; o[5] = mP1.w0; o[6] = mP1.w1; o[7] = mP1.w2; o[8] = mP1.w3; // second poly o[9] = mP2.min; o[10] = mP2.max; o[11] = mP2.w0; o[12] = mP2.w1; o[13] = mP2.w2; o[14] = mP2.w3; // first gaussian o[15] = mG.min ; o[16] = mG.max ; o[17] = mG.mean; o[18] = mG.sigma; o[19] = mG.h ; o[20] = mG.hs ; //return o; }
public void Reset() { survey = null; survey_updates = 0; minimum_rms_error = double.MaxValue; best_fit_curve = null; }
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()); }
/// <summary> /// parse an xml node to extract camera calibration parameters /// </summary> /// <param name="xnod"></param> /// <param name="level"></param> public void LoadFromXml(XmlNode xnod, int level) { XmlNode xnodWorking; if (xnod.Name == "FieldOfViewDegrees") camera_FOV_degrees = Convert.ToInt32(xnod.InnerText); if (xnod.Name == "ImageDimensions") { IFormatProvider format = new System.Globalization.CultureInfo("en-GB"); String[] dimStr = xnod.InnerText.Split(','); image_width = Convert.ToInt32(dimStr[0], format); image_height = Convert.ToInt32(dimStr[1], format); } if (xnod.Name == "CentreOfDistortion") { IFormatProvider format = new System.Globalization.CultureInfo("en-GB"); String[] centreStr = xnod.InnerText.Split(','); centre_of_distortion = new calibration_point( Convert.ToSingle(centreStr[0], format), Convert.ToSingle(centreStr[1], format)); } if (xnod.Name == "DistortionCoefficients") { if (xnod.InnerText != "") { IFormatProvider format = new System.Globalization.CultureInfo("en-GB"); String[] coeffStr = xnod.InnerText.Split(','); fitter = new polynomial(); fitter.SetDegree(coeffStr.Length - 1); for (int i = 0; i < coeffStr.Length; i++) fitter.SetCoeff(i, Convert.ToSingle(coeffStr[i], format)); } } if (xnod.Name == "Scale") { scale = Convert.ToSingle(xnod.InnerText); } if (xnod.Name == "RotationDegrees") { rotation = Convert.ToSingle(xnod.InnerText) / 180.0f * (float)Math.PI; } if (xnod.Name == "RMSerror") { min_RMS_error = Convert.ToSingle(xnod.InnerText); // update the calibration lookup table updateCalibrationMap(); } // if this is an element, extract any attributes /* if (xnod.NodeType == XmlNodeType.Element) { XmlNamedNodeMap mapAttributes = xnod.Attributes; for (int i = 0; i < mapAttributes.Count; i += 1) { //Console.WriteLine(pad + " " + mapAttributes.Item(i).Name // + " = " + mapAttributes.Item(i).Value); } } */ // call recursively on all children of the current node if (xnod.HasChildNodes) { xnodWorking = xnod.FirstChild; while (xnodWorking != null) { LoadFromXml(xnodWorking, level + 1); xnodWorking = xnodWorking.NextSibling; } } }
/// <summary> /// update the calibration lookup table, which maps pixels /// in the rectified image into the original image /// </summary> /// <param name="width"></param> /// <param name="height"></param> private void updateCalibrationMap(int width, int height, polynomial curve, float scale, float rotation) { temp_calibration_map = new int[width * height]; temp_calibration_map_inverse = new int[width, height, 2]; for (int x = 0; x < width; x++) { float dx = x - centre_of_distortion.x; for (int y = 0; y < 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) { float radial_dist_original = curve.RegVal(radial_dist_rectified); if (radial_dist_original > 0) { float ratio = radial_dist_original / radial_dist_rectified; float x2 = (float)Math.Round(centre_of_distortion.x + (dx * ratio)); x2 = (x2 - (width / 2)) * scale; float y2 = (float)Math.Round(centre_of_distortion.y + (dy * ratio)); y2 = (y2 - (height / 2)) * scale; // apply rotation float x3=x2, y3=y2; rotatePoint(x2, y2, -rotation, ref x3, ref y3); x3 += (width / 2); y3 += (height / 2); if (((int)x3 > -1) && ((int)x3 < width) && ((int)y3 > -1) && ((int)y3 < height)) { int n = (y * width) + x; int n2 = ((int)y3 * width) + (int)x3; temp_calibration_map[n] = n2; temp_calibration_map_inverse[(int)x3, (int)y3, 0] = x; temp_calibration_map_inverse[(int)x3, (int)y3, 1] = y; } } } } } }
private void detectLensDistortion(int width, int height, int grid_x, int grid_y) { if (grid != null) { if (grid[grid_x, grid_y] != null) { // field of vision in radians float FOV_horizontal = camera_FOV_degrees * (float)Math.PI / 180.0f; float FOV_vertical = FOV_horizontal *height / (float)width; // center point of the grid within the image pattern_centre_x = (int)grid[grid_x, grid_y].x; pattern_centre_y = (int)grid[grid_x, grid_y].y; // calculate the distance to the centre grid point on the ground plane float ground_dist_to_point = camera_dist_to_pattern_centre_mm; // line of sight distance between the camera lens and the centre point float camera_to_point_dist = (float)Math.Sqrt((ground_dist_to_point * ground_dist_to_point) + (camera_height_mm * camera_height_mm)); distance_to_pattern_centre = camera_to_point_dist; // tilt angle at the centre point float centre_tilt = (float)Math.Asin(camera_height_mm / camera_to_point_dist); // angle subtended by one grid spacing at the centre float point_pan = (float)Math.Asin(calibration_pattern_spacing_mm / camera_to_point_dist); // grid width at the centre point float x1 = pattern_centre_x + (point_pan * width / FOV_horizontal); // calculate the distance to the observed grid point on the ground plane ground_dist_to_point = camera_dist_to_pattern_centre_mm + ((grid_y + 2) * calibration_pattern_spacing_mm); // line of sight distance between the camera lens and the observed point camera_to_point_dist = (float)Math.Sqrt((ground_dist_to_point * ground_dist_to_point) + (camera_height_mm * camera_height_mm)); // tilt angle float point_tilt = (float)Math.Asin(camera_height_mm / camera_to_point_dist); // angle subtended by one grid spacing point_pan = (float)Math.Asin(calibration_pattern_spacing_mm / camera_to_point_dist); // calc the position of the grid point within the image after rectification float x2 = pattern_centre_x + (point_pan * width / FOV_horizontal); float y2 = pattern_centre_y + ((point_tilt - centre_tilt) * height / FOV_vertical); // calc the gradient float grad = (x2 - x1) / (float)(y2 - pattern_centre_y); float baseline_fraction = baseline_offset / (float)(calibration_pattern_spacing_mm); centre_of_distortion = new calibration_point(0, 0); int hits = 0; float cx = 0, cy = 0; for (int x = 0; x < grid.GetLength(0); x++) { for (int y = 0; y < grid.GetLength(1); y++) { if (grid[x, y] != null) { // calculate the distance to the observed grid point on the ground plane ground_dist_to_point = camera_dist_to_pattern_centre_mm + ((grid_y - y) * calibration_pattern_spacing_mm); // line of sight distance between the camera lens and the observed point camera_to_point_dist = (float)Math.Sqrt((ground_dist_to_point * ground_dist_to_point) + (camera_height_mm * camera_height_mm)); // tilt angle point_tilt = (float)Math.Asin(camera_height_mm / camera_to_point_dist); // distance to the point on the ground plave along the x (horizontal axis) float ground_dist_to_point_x = ((x - grid_x) * calibration_pattern_spacing_mm) + baseline_offset; // pan angle point_pan = (float)Math.Asin(ground_dist_to_point_x / camera_to_point_dist); // calc the position of the grid point within the image after rectification float w = ((x1 - pattern_centre_x) + ((grid[x, y].y - pattern_centre_y) * grad)); float wbaseline = baseline_fraction * (grid[x, y].y - pattern_centre_y) * grad; float rectified_x = pattern_centre_x + (w * (x - grid_x)) - wbaseline; float rectified_y = pattern_centre_y + ((point_tilt - centre_tilt) * height / FOV_vertical); grid[x, y].rectified_x = rectified_x; grid[x, y].rectified_y = rectified_y; cx += (grid[x, y].x - rectified_x); cy += (grid[x, y].y - rectified_y); hits++; } } } if (hits > 0) { // a ballpack figure for the centre of distortion centre_of_distortion.x = (width / 2) + (cx / (float)hits); centre_of_distortion.y = (height / 2) + (cy / (float)hits); float winner_x = centre_of_distortion.x; float winner_y = centre_of_distortion.y; float min_rms_err = 999999; int radius = 5; for (int search_x = (int)centre_of_distortion.x - radius; search_x <= (int)centre_of_distortion.x + radius; search_x++) { for (int search_y = (int)centre_of_distortion.y - radius; search_y <= (int)centre_of_distortion.y + radius; search_y++) { polynomial curvefit = new polynomial(); curvefit.SetDegree(2); for (int x = 0; x < grid.GetLength(0); x++) { for (int y = 0; y < grid.GetLength(1); y++) { if (grid[x, y] != null) { addDataPoint(grid[x, y].x, grid[x, y].y, grid[x, y].rectified_x, grid[x, y].rectified_y, curvefit); // intermediary points if (y > 0) { if (grid[x, y - 1] != null) { float xx = grid[x, y - 1].x + ((grid[x, y].x - grid[x, y - 1].x) / 2); float yy = grid[x, y - 1].y + ((grid[x, y].y - grid[x, y - 1].y) / 2); float rectified_xx = grid[x, y - 1].rectified_x + ((grid[x, y].rectified_x - grid[x, y - 1].rectified_x) / 2); float rectified_yy = grid[x, y - 1].rectified_y + ((grid[x, y].rectified_y - grid[x, y - 1].rectified_y) / 2); addDataPoint(xx, yy, rectified_xx, rectified_yy, curvefit); } } if (x > 0) { if (grid[x-1, y] != null) { float xx = grid[x-1, y].x + ((grid[x, y].x - grid[x-1, y].x) / 2); float yy = grid[x-1, y].y + ((grid[x, y].y - grid[x-1, y].y) / 2); float rectified_xx = grid[x-1, y].rectified_x + ((grid[x, y].rectified_x - grid[x-1, y].rectified_x) / 2); float rectified_yy = grid[x-1, y].rectified_y + ((grid[x, y].rectified_y - grid[x-1, y].rectified_y) / 2); addDataPoint(xx, yy, rectified_xx, rectified_yy, curvefit); } } } } } curvefit.Solve(); float rms_err = curvefit.GetRMSerror(); if (rms_err < min_rms_err) { min_rms_err = rms_err; winner_x = search_x; winner_y = search_y; fitter = curvefit; } } } centre_of_distortion.x = winner_x; centre_of_distortion.y = winner_y; } } } }
/// <summary> /// add a data point to the given curve fitter /// </summary> /// <param name="x"></param> /// <param name="y"></param> /// <param name="rectified_x"></param> /// <param name="rectified_y"></param> /// <param name="polycurve"></param> private void addDataPoint(float x, float y, float rectified_x, float rectified_y, polynomial polycurve) { float dx = rectified_x - centre_of_distortion.x; float dy = rectified_y - centre_of_distortion.y; float radial_dist_rectified = (float)Math.Sqrt((dx * dx) + (dy * dy)); dx = x - centre_of_distortion.x; dy = y - centre_of_distortion.y; float radial_dist_original = (float)Math.Sqrt((dx * dx) + (dy * dy)); polycurve.AddPoint(radial_dist_rectified, radial_dist_original); }
public void updatePolinomial(polynomial p1,polynomial p2,gaussian g) { mP1 = p1; mP2 = p2; mG = g; // SetFloat ("radious", p2.max); }
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; } } }