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