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