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