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