/// <summary> /// extracts lines from the given detected dots /// </summary> /// <param name="dots"></param> /// <returns></returns> private static List<List<double>> CreateLines(hypergraph dots, calibrationDot[,] grid) { List<List<double>> lines = new List<List<double>>(); if (grid != null) { for (int grid_x = 0; grid_x < grid.GetLength(0); grid_x++) { List<double> line = new List<double>(); for (int grid_y = 0; grid_y < grid.GetLength(1); grid_y++) { if (grid[grid_x, grid_y] != null) { line.Add(grid[grid_x, grid_y].x); line.Add(grid[grid_x, grid_y].y); } } if (line.Count > 6) { lines.Add(line); } } for (int grid_y = 0; grid_y < grid.GetLength(1); grid_y++) { List<double> line = new List<double>(); for (int grid_x = 0; grid_x < grid.GetLength(0); grid_x++) { if (grid[grid_x, grid_y] != null) { line.Add(grid[grid_x, grid_y].x); line.Add(grid[grid_x, grid_y].y); } } if (line.Count > 6) { lines.Add(line); } } } return (lines); }
/// <summary> /// puts the given dots into a grid for easy lookup /// </summary> /// <param name="dots">detected calibration dots</param> /// <returns>grid object</returns> private static calibrationDot[,] CreateGrid(hypergraph dots) { int grid_tx = 9999, grid_ty = 9999; int grid_bx = -9999, grid_by = -9999; for (int i = 0; i < dots.Nodes.Count; i++) { calibrationDot dot = (calibrationDot)dots.Nodes[i]; if (Math.Abs(dot.grid_x) < 50) { if (dot.grid_x < grid_tx) grid_tx = dot.grid_x; if (dot.grid_y < grid_ty) grid_ty = dot.grid_y; if (dot.grid_x > grid_bx) grid_bx = dot.grid_x; if (dot.grid_y > grid_by) grid_by = dot.grid_y; } } calibrationDot[,] grid = null; if (grid_bx > grid_tx + 1) { grid = new calibrationDot[grid_bx - grid_tx + 1, grid_by - grid_ty + 1]; for (int i = 0; i < dots.Nodes.Count; i++) { calibrationDot dot = (calibrationDot)dots.Nodes[i]; if ((!dot.centre) && (Math.Abs(dot.grid_x) < 50)) { grid[dot.grid_x - grid_tx, dot.grid_y - grid_ty] = dot; } } } return (grid); }
/// <summary> /// applies grid coordinates to the given connected dots /// </summary> /// <param name="current_dot">the current dot of interest</param> /// <param name="unassigned_value"></param> /// <param name="dots_assigned"></param> private static void ApplyGrid(calibrationDot current_dot, int unassigned_value, ref int dots_assigned) { for (int i = 0; i < current_dot.Links.Count; i++) { calibrationLink link = (calibrationLink)current_dot.Links[i]; calibrationDot dot = (calibrationDot)link.From; if (dot.grid_x == unassigned_value) { if (link.horizontal) { if (dot.x < current_dot.x) dot.grid_x = current_dot.grid_x - 1; else dot.grid_x = current_dot.grid_x + 1; dot.grid_y = current_dot.grid_y; } else { dot.grid_x = current_dot.grid_x; if (dot.y > current_dot.y) dot.grid_y = current_dot.grid_y - 1; else dot.grid_y = current_dot.grid_y + 1; } dots_assigned++; ApplyGrid(dot, unassigned_value, ref dots_assigned); } } }
/// <summary> /// links detected dots together /// </summary> /// <param name="dots">detected dots</param> /// <param name="current_dot">the current dot of interest</param> /// <param name="horizontal_dx">current expected horizontal displacement x coordinate</param> /// <param name="horizontal_dy">current expected horizontal displacement y coordinate</param> /// <param name="vertical_dx">current expected vertical displacement x coordinate</param> /// <param name="vertical_dy">current expected vertical displacement x coordinate</param> /// <param name="start_index">index number indicating which direction we will search in first</param> /// <param name="search_regions">returned list of search regions</param> private static void LinkDots(hypergraph dots, calibrationDot current_dot, double horizontal_dx, double horizontal_dy, double vertical_dx, double vertical_dy, int start_index, List<calibrationDot> search_regions) { if (!current_dot.centre) { int start_index2 = 0; double tollerance_divisor = 0.3f; double horizontal_tollerance = Math.Sqrt((horizontal_dx * horizontal_dx) + (horizontal_dy * horizontal_dy)) * tollerance_divisor; double vertical_tollerance = Math.Sqrt((vertical_dx * vertical_dx) + (vertical_dy * vertical_dy)) * tollerance_divisor; double x = 0, y = 0; List<int> indexes_found = new List<int>(); List<bool> found_vertical = new List<bool>(); // check each direction for (int i = 0; i < 4; i++) { // starting direction offset int ii = i + start_index; if (ii >= 4) ii -= 4; if (current_dot.Flags[ii] == false) { current_dot.Flags[ii] = true; int opposite_flag = ii + 2; if (opposite_flag >= 4) opposite_flag -= 4; switch (ii) { case 0: { // look above x = current_dot.x - vertical_dx; y = current_dot.y - vertical_dy; break; } case 1: { // look right x = current_dot.x + horizontal_dx; y = current_dot.y + horizontal_dy; break; } case 2: { // look below x = current_dot.x + vertical_dx; y = current_dot.y + vertical_dy; break; } case 3: { // look left x = current_dot.x - horizontal_dx; y = current_dot.y - horizontal_dy; break; } } calibrationDot search_region = new calibrationDot(); search_region.x = x; search_region.y = y; search_region.radius = (float)horizontal_tollerance; search_regions.Add(search_region); for (int j = 0; j < dots.Nodes.Count; j++) { if ((!((calibrationDot)dots.Nodes[j]).centre) && (dots.Nodes[j] != current_dot)) { double dx = ((calibrationDot)dots.Nodes[j]).x - x; double dy = ((calibrationDot)dots.Nodes[j]).y - y; double dist_from_expected_position = Math.Sqrt(dx * dx + dy * dy); bool dot_found = false; if ((ii == 0) || (ii == 2)) { // vertical search if (dist_from_expected_position < vertical_tollerance) { dot_found = true; found_vertical.Add(true); } } else { // horizontal search if (dist_from_expected_position < horizontal_tollerance) { dot_found = true; found_vertical.Add(false); } } if (dot_found) { indexes_found.Add(j); j = dots.Nodes.Count; } } } } } for (int i = 0; i < indexes_found.Count; i++) { start_index2 = start_index + 1; if (start_index2 >= 4) start_index2 -= 4; double found_dx = ((calibrationDot)dots.Nodes[indexes_found[i]]).x - current_dot.x; double found_dy = ((calibrationDot)dots.Nodes[indexes_found[i]]).y - current_dot.y; calibrationLink link = new calibrationLink(); if (found_vertical[i]) { link.horizontal = false; if (((vertical_dy > 0) && (found_dy < 0)) || ((vertical_dy < 0) && (found_dy > 0))) { found_dx = -found_dx; found_dy = -found_dy; } LinkDots(dots, (calibrationDot)dots.Nodes[indexes_found[i]], horizontal_dx, horizontal_dy, found_dx, found_dy, start_index2, search_regions); } else { link.horizontal = true; if (((horizontal_dx > 0) && (found_dx < 0)) || ((horizontal_dx < 0) && (found_dx > 0))) { found_dx = -found_dx; found_dy = -found_dy; } LinkDots(dots, (calibrationDot)dots.Nodes[indexes_found[i]], found_dx, found_dy, vertical_dx, vertical_dy, start_index2, search_regions); } dots.LinkByReference((calibrationDot)dots.Nodes[indexes_found[i]], current_dot, link); } } }
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> /// 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); }
private static void BestFit(int grid_tx, int grid_ty, calibrationDot[,] grid, grid2D overlay_grid, ref double min_dist, ref double min_dx, ref double min_dy, ref int min_hits, ref int grid_offset_x, ref int grid_offset_y) { for (int off_x = -1; off_x <= 1; off_x++) { for (int off_y = -1; off_y <= 1; off_y++) { int grid_x_offset = -grid_tx + off_x; int grid_y_offset = -grid_ty + off_y; int grid_x_offset_start = 0; int grid_x_offset_end = 0; if (grid_x_offset < 0) { grid_x_offset_start = -grid_x_offset; grid_x_offset_end = 0; } else { grid_x_offset_start = 0; grid_x_offset_end = grid_x_offset; } int grid_y_offset_start = 0; int grid_y_offset_end = 0; if (grid_y_offset < 0) { grid_y_offset_start = -grid_y_offset; grid_y_offset_end = 0; } else { grid_y_offset_start = 0; grid_y_offset_end = grid_y_offset; } double dx = 0; double dy = 0; double dist = 0; int hits = 0; for (int grid_x = grid_x_offset_start; grid_x < grid.GetLength(0) - grid_x_offset_end; grid_x++) { for (int grid_y = grid_y_offset_start; grid_y < grid.GetLength(1) - grid_y_offset_end; grid_y++) { if (grid[grid_x, grid_y] != null) { if ((grid_x + grid_x_offset < overlay_grid.line_intercepts.GetLength(0)) && (grid_y + grid_y_offset < overlay_grid.line_intercepts.GetLength(1))) { double intercept_x = overlay_grid.line_intercepts[grid_x + grid_x_offset, grid_y + grid_y_offset, 0]; double intercept_y = overlay_grid.line_intercepts[grid_x + grid_x_offset, grid_y + grid_y_offset, 1]; double dxx = grid[grid_x, grid_y].x - intercept_x; double dyy = grid[grid_x, grid_y].y - intercept_y; dx += dxx; dy += dyy; dist += Math.Abs(dxx) + Math.Abs(dyy); hits++; } } } } if (hits > 0) { dx /= hits; dy /= hits; //double dist = Math.Sqrt(dx * dx + dy * dy); if (dist < min_dist) { min_dist = dist; min_dx = dx; min_dy = dy; min_hits = hits; grid_offset_x = grid_x_offset; grid_offset_y = grid_y_offset; } } } } }
/// <summary> /// returns an ideally spaced grid over the actual detected spots /// </summary> /// <param name="grid"></param> /// <returns></returns> private static grid2D OverlayIdealGrid(calibrationDot[,] grid, List<calibrationDot> corners, ref int grid_offset_x, ref int grid_offset_y, int random_seed) { grid2D overlay_grid = null; int grid_tx = -1; int grid_ty = -1; int grid_bx = -1; int grid_by = -1; int offset_x = 0; int offset_y = 0; bool found = false; int max_region_area = 0; // try searching horizontally and vertically // then pick the result with the greatest area for (int test_orientation = 0; test_orientation < 2; test_orientation++) { bool temp_found = false; int temp_grid_tx = -1; int temp_grid_ty = -1; int temp_grid_bx = -1; int temp_grid_by = -1; int temp_offset_x = 0; int temp_offset_y = 0; switch (test_orientation) { case 0: { while ((temp_offset_y < 5) && (!temp_found)) { temp_offset_x = 0; while ((temp_offset_x < 3) && (!temp_found)) { temp_grid_tx = temp_offset_x; temp_grid_ty = temp_offset_y; temp_grid_bx = grid.GetLength(0) - 1 - temp_offset_x; temp_grid_by = grid.GetLength(1) - 1 - temp_offset_y; if ((temp_grid_bx < grid.GetLength(0)) && (temp_grid_tx < grid.GetLength(0)) && (temp_grid_by < grid.GetLength(1)) && (temp_grid_ty < grid.GetLength(1)) && (temp_grid_ty >= 0) && (temp_grid_by >= 0) && (temp_grid_tx >= 0) && (temp_grid_bx >= 0)) { if ((grid[temp_grid_tx, temp_grid_ty] != null) && (grid[temp_grid_bx, temp_grid_ty] != null) && (grid[temp_grid_bx, temp_grid_by] != null) && (grid[temp_grid_tx, temp_grid_by] != null)) { temp_found = true; } } temp_offset_x++; } temp_offset_y++; } break; } case 1: { while ((temp_offset_x < 3) && (!temp_found)) { temp_offset_y = 0; while ((temp_offset_y < 5) && (!temp_found)) { temp_grid_tx = temp_offset_x; temp_grid_ty = temp_offset_y; temp_grid_bx = grid.GetLength(0) - 1 - temp_offset_x; temp_grid_by = grid.GetLength(1) - 1 - temp_offset_y; if ((temp_grid_bx < grid.GetLength(0)) && (temp_grid_tx < grid.GetLength(0)) && (temp_grid_by < grid.GetLength(1)) && (temp_grid_ty < grid.GetLength(1)) && (temp_grid_ty >= 0) && (temp_grid_by >= 0) && (temp_grid_tx >= 0) && (temp_grid_bx >= 0)) { if ((grid[temp_grid_tx, temp_grid_ty] != null) && (grid[temp_grid_bx, temp_grid_ty] != null) && (grid[temp_grid_bx, temp_grid_by] != null) && (grid[temp_grid_tx, temp_grid_by] != null)) { temp_found = true; } } temp_offset_y++; } temp_offset_x++; } break; } } temp_offset_y = temp_grid_ty - 1; while (temp_offset_y >= 0) { if ((temp_offset_y < grid.GetLength(1)) && (temp_offset_y >= 0)) { if ((grid[temp_grid_tx, temp_offset_y] != null) && (grid[temp_grid_bx, temp_offset_y] != null)) { temp_grid_ty = temp_offset_y; temp_offset_y--; } else break; } else break; } temp_offset_y = temp_grid_by + 1; while (temp_offset_y < grid.GetLength(1)) { if ((temp_offset_y < grid.GetLength(1)) && (temp_offset_y >= 0)) { if ((grid[temp_grid_tx, temp_offset_y] != null) && (grid[temp_grid_bx, temp_offset_y] != null)) { temp_grid_by = temp_offset_y; temp_offset_y++; } else break; } else break; } if (temp_found) { int region_area = (temp_grid_bx - temp_grid_tx) * (temp_grid_by - temp_grid_ty); if (region_area > max_region_area) { max_region_area = region_area; found = true; grid_tx = temp_grid_tx; grid_ty = temp_grid_ty; grid_bx = temp_grid_bx; grid_by = temp_grid_by; offset_x = temp_offset_x; offset_y = temp_offset_y; } } } if (found) { // record the positions of the corners corners.Add(grid[grid_tx, grid_ty]); corners.Add(grid[grid_bx, grid_ty]); corners.Add(grid[grid_bx, grid_by]); corners.Add(grid[grid_tx, grid_by]); double dx, dy; double x0 = grid[grid_tx, grid_ty].x; double y0 = grid[grid_tx, grid_ty].y; double x1 = grid[grid_bx, grid_ty].x; double y1 = grid[grid_bx, grid_ty].y; double x2 = grid[grid_tx, grid_by].x; double y2 = grid[grid_tx, grid_by].y; double x3 = grid[grid_bx, grid_by].x; double y3 = grid[grid_bx, grid_by].y; polygon2D perimeter = new polygon2D(); perimeter.Add((float)x0, (float)y0); perimeter.Add((float)x1, (float)y1); perimeter.Add((float)x3, (float)y3); perimeter.Add((float)x2, (float)y2); int grid_width = grid_bx - grid_tx; int grid_height = grid_by - grid_ty; int min_hits = 0; double min_dx = 0, min_dy = 0; // try various perimeter sizes double min_dist = double.MaxValue; int max_perim_size_tries = 100; polygon2D best_perimeter = perimeter; MersenneTwister rnd = new MersenneTwister(random_seed); for (int perim_size = 0; perim_size < max_perim_size_tries; perim_size++) { // try a small range of translations for (int nudge_x = -10; nudge_x <= 10; nudge_x++) { for (int nudge_y = -5; nudge_y <= 5; nudge_y++) { // create a perimeter at this scale and translation polygon2D temp_perimeter = perimeter.Scale(1.0f + (perim_size * 0.1f / max_perim_size_tries)); temp_perimeter = temp_perimeter.ScaleSideLength(0, 0.95f + ((float)rnd.NextDouble() * 0.1f)); temp_perimeter = temp_perimeter.ScaleSideLength(2, 0.95f + ((float)rnd.NextDouble() * 0.1f)); for (int i = 0; i < temp_perimeter.x_points.Count; i++) { temp_perimeter.x_points[i] += nudge_x; temp_perimeter.y_points[i] += nudge_y; } // create a grid based upon the perimeter grid2D temp_overlay_grid = new grid2D(grid_width, grid_height, temp_perimeter, 0, false); // how closely does the grid fit the actual observations ? double temp_min_dist = min_dist; BestFit(grid_tx, grid_ty, grid, temp_overlay_grid, ref min_dist, ref min_dx, ref min_dy, ref min_hits, ref grid_offset_x, ref grid_offset_y); // record the closest fit if (temp_min_dist < min_dist) { best_perimeter = temp_perimeter; overlay_grid = temp_overlay_grid; } } } } if (min_hits > 0) { dx = min_dx; dy = min_dy; Console.WriteLine("dx: " + dx.ToString()); Console.WriteLine("dy: " + dy.ToString()); x0 += dx; y0 += dy; x1 += dx; y1 += dy; x2 += dx; y2 += dy; x3 += dx; y3 += dy; perimeter = new polygon2D(); perimeter.Add((float)x0, (float)y0); perimeter.Add((float)x1, (float)y1); perimeter.Add((float)x3, (float)y3); perimeter.Add((float)x2, (float)y2); overlay_grid = new grid2D(grid_width, grid_height, perimeter, 0, false); } } return (overlay_grid); }
/// <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, MersenneTwister 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> /// 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); MersenneTwister rnd = new MersenneTwister(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); }
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> /// 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); }
public static List<calibrationDot> GetDots(List<List<int>> groups, byte[] img_colour, int img_width, int img_height, float minimum_aspect, float maximum_aspect, int minimum_width, int maximum_width) { List<calibrationDot> dots = new List<calibrationDot>(); float ideal_aspect = minimum_aspect + ((maximum_aspect - minimum_aspect)/2.0f); float minimum_diff_from_ideal = 9999; float max_redness = 0; int winner = -1; for (int i = 0; i < groups.Count; i++) { int tx = img_width-1; int ty = img_height-1; int bx = 0; int by = 0; float cx = 0; float cy = 0; List<int> edges = groups[i]; for (int j = 0; j < edges.Count; j += 2) { int x = edges[j]; int y = edges[j + 1]; cx += x; cy += y; if (x < tx) tx = x; if (y < ty) ty = y; if (x > bx) bx = x; if (y > by) by = y; } int dx = bx - tx; int dy = by - ty; if ((dx > minimum_width) && (dx < maximum_width) && (dy > minimum_width) && (dy < maximum_width)) { float aspect_ratio = 0; if (dy > dx) aspect_ratio = dx / (float)dy; else aspect_ratio = dy / (float)dx; if ((aspect_ratio > minimum_aspect) && (aspect_ratio < maximum_aspect)) { float radius = (dx + dy) / 4.0f; cx /= (edges.Count/2); cy /= (edges.Count/2); calibrationDot cdot = new calibrationDot(); cdot.x = cx; cdot.y = cy; cdot.radius = radius; // get the colour of the dot int[] dot_colour = new int[3]; SampleDotColour(img_colour, img_width, img_height, (int)cx, (int)cy, dot_colour); cdot.r = dot_colour[0]; cdot.g = dot_colour[1]; cdot.b = dot_colour[2]; float redness = (cdot.r*2) - cdot.g - cdot.b; if (redness > max_redness) { max_redness = redness; winner = dots.Count; } dots.Add(cdot); } } } if (winner > -1) dots[winner].centre = true; return(dots); }