/// <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;
Random rnd = new Random(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="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 byte[] GetConnectedSetsImage(byte[] raw_image,
int minimum_length,
int maximum_length,
bool square_aspect,
ref hypergraph dots)
{
dots = new hypergraph();
int total_bytes = width * height * 3;
byte[] result = new byte[total_bytes];
for (int i = result.Length-1; i >= 0; i--) result[i] = raw_image[i];
List<float> centres = null;
List<List<int>> connected_sets = GetConnectedSets(minimum_length, maximum_length, square_aspect, ref centres);
for (int i = 0; i < centres.Count; i += 3)
{
float centre_x = centres[i];
float centre_y = centres[i + 1];
float radius = centres[i + 2];
CalibrationDot dot = new CalibrationDot();
dot.x = centre_x;
dot.y = centre_y;
dot.radius = radius;
dots.Add(dot);
for (int j = 0; j < 360; j += 5)
{
float angle = j * (float)Math.PI * 2 / 360.0f;
int x = (int)(centre_x + (Math.Sin(angle) * radius));
int y = (int)(centre_y + (Math.Cos(angle) * radius));
int n = ((y * width) + x) * 3;
if ((n > 3) && (n < total_bytes - 4))
{
result[n] = 0;
result[n+1] = 255;
result[n+2] = 0;
}
}
}
return(result);
}
/// <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);
}
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>
/// returns an ideal grid with perfectly regular spacing
/// </summary>
/// <param name="grid"></param>
/// <param name="image_width"></param>
/// <param name="image_height"></param>
/// <returns>
/// </returns>
private static grid2D GetIdealGrid(CalibrationDot[,] grid,
int image_width, int image_height)
{
grid2D ideal_grid = null;
float ideal_spacing = 0;
double centre_x = image_width / 2;
double centre_y = image_height / 2;
double min_dist = double.MaxValue;
int grid_cx=0, grid_cy=0;
for (int x = 0; x < grid.GetLength(0); x++)
{
for (int y = 0; y < grid.GetLength(1); y++)
{
if (grid[x, y] != null)
{
double dx = grid[x, y].x - centre_x;
double dy = grid[x, y].y - centre_y;
double dist = dx*dx + dy*dy;
if (dist < min_dist)
{
min_dist = dist;
grid_cx = x;
grid_cy = y;
}
}
}
}
if (grid_cx > 0)
{
int[] orientation_histogram = new int[361];
List<double>[] orientations = new List<double>[361];
double average_dist = 0;
int hits = 0;
int local_search = 2;
for (int x = grid_cx - local_search; x <= grid_cx + local_search; x++)
{
if ((x >= 0) && (x < grid.GetLength(0)-1))
{
for (int y = grid_cy - local_search; y <= grid_cy + local_search; y++)
{
if ((y >= 1) && (y < grid.GetLength(1)-1))
{
if (grid[x, y] != null)
{
for (int i = 0; i < grid[x, y].Links.Count; i++)
{
CalibrationLink link = (CalibrationLink)grid[x, y].Links[i];
CalibrationDot from_dot = (CalibrationDot)link.From;
double dx = grid[x, y].x - from_dot.x;
double dy = grid[x, y].y - from_dot.y;
double dist = Math.Sqrt(dx*dx + dy*dy);
if (dist > 0)
{
double orientation = Math.Asin(dx / dist);
if (orientation < 0) orientation += Math.PI;
if (dy < 0) orientation = (Math.PI * 2) - orientation;
orientation = orientation / Math.PI * 180;
int bucket = (int)orientation;
orientation_histogram[bucket]++;
if (orientations[bucket] == null) orientations[bucket] = new List<double>();
orientations[bucket].Add(orientation);
average_dist += Math.Sqrt(dx*dx + dy*dy);
hits++;
}
}
}
}
}
}
}
if (hits > 0) average_dist = average_dist / hits;
ideal_spacing = (float)average_dist;
int max_orientation_response = 0;
int best_ang = 0;
for (int ang = 0; ang < 90; ang++)
{
int response = orientation_histogram[ang] +
orientation_histogram[ang + 90] +
orientation_histogram[ang + 180];
if (response > max_orientation_response)
{
max_orientation_response = response;
best_ang = ang;
}
}
double average_orientation = 0;
hits = 0;
for (int offset = 0; offset < 3; offset++)
{
if (orientations[best_ang + offset] != null)
{
for (int ang = 0; ang < orientations[best_ang + offset].Count; ang++)
{
average_orientation += orientations[best_ang + offset][ang] - (Math.PI*offset/2);
hits++;
}
}
}
if (hits > 0)
{
average_orientation /= hits;
float pattern_orientation = (float)average_orientation;
float offset_x = (float)grid[grid_cx, grid_cy].x;
float offset_y = (float)grid[grid_cx, grid_cy].y;
float r = ideal_spacing * dots_across;
polygon2D perimeter = new polygon2D();
perimeter.Add(-r + offset_x, -r + offset_y);
perimeter.Add(r + offset_x, -r + offset_y);
perimeter.Add(r + offset_x, r + offset_y);
perimeter.Add(-r + offset_x, r + offset_y);
perimeter.rotate((float)average_orientation / 180 * (float)Math.PI, offset_x, offset_y);
ideal_grid = new grid2D(dots_across*2, dots_across*2, perimeter, 0, false);
int grid_width = grid.GetLength(0);
int grid_height = grid.GetLength(1);
int idx = 0;
int xx=0, yy=0;
float ideal_x, ideal_y;
for (int orient = 0; orient < 8; orient++)
{
for (int x = 0; x < grid_width; x++)
{
for (int y = 0; y < grid_height; y++)
{
if (grid[x, y] != null)
{
switch(orient)
{
case 0:
{
xx = (x - grid_cx) + dots_across;
yy = (y - grid_cy) + dots_across;
break;
}
case 1:
{
xx = (x - grid_cx) + dots_across;
yy = (grid_cy - y) + dots_across;
break;
}
case 2:
{
xx = (grid_cx - x) + dots_across;
yy = (y - grid_cy) + dots_across;
break;
}
case 3:
{
xx = (grid_cx - x) + dots_across;
yy = (grid_cy - y) + dots_across;
break;
}
case 4:
{
xx = (y - grid_cy) + dots_across;
yy = (x - grid_cx) + dots_across;
break;
}
case 5:
{
xx = (y - grid_cy) + dots_across;
yy = (grid_cx - x) + dots_across;
break;
}
case 6:
{
xx = (grid_cy - y) + dots_across;
yy = (x - grid_cx) + dots_across;
break;
}
case 7:
{
xx = (grid_cy - y) + dots_across;
yy = (grid_cx - x) + dots_across;
break;
}
}
if ((xx >= 0) && (xx < ideal_grid.cell.Length) &&
(yy >= 0) && (yy < ideal_grid.cell[0].Length))
{
ideal_x = ideal_grid.cell[xx][yy].perimeter.x_points[idx];
ideal_y = ideal_grid.cell[xx][yy].perimeter.y_points[idx];
if (orient == 0)
{
grid[x, y].rectified_x = ideal_x;
grid[x, y].rectified_y = ideal_y;
}
else
{
double diff_x1 = grid[x, y].rectified_x - grid[x, y].x;
double diff_y1 = grid[x, y].rectified_y - grid[x, y].y;
double dist1 = diff_x1*diff_x1 + diff_y1*diff_y1;
double diff_x2 = ideal_x - grid[x, y].x;
double diff_y2 = ideal_y - grid[x, y].y;
double dist2 = diff_x2*diff_x2 + diff_y2*diff_y2;
if (dist2 < dist1)
{
grid[x, y].rectified_x = ideal_x;
grid[x, y].rectified_y = ideal_y;
}
}
}
}
}
}
}
}
}
return(ideal_grid);
}
private static void ShowIdealGrid(Bitmap bmp,
grid2D ideal_grid,
CalibrationDot[,] grid,
ref Bitmap output_bmp)
{
byte[] img = new byte[bmp.Width * bmp.Height * 3];
BitmapArrayConversions.updatebitmap(bmp, img);
if (grid != null)
{
for (int grid_x = 0; grid_x < grid.GetLength(0); grid_x++)
{
for (int grid_y = 0; grid_y < grid.GetLength(1); grid_y++)
{
if (grid[grid_x, grid_y] != null)
{
drawing.drawCross(img, bmp.Width, bmp.Height,
(int)grid[grid_x, grid_y].x, (int)grid[grid_x, grid_y].y,
1, 0,255,0,0);
}
}
}
}
if (ideal_grid != null) ideal_grid.ShowIntercepts(img, bmp.Width, bmp.Height, 255,0,0, 3, 0);
if (output_bmp == null)
output_bmp = new Bitmap(bmp.Width, bmp.Height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
BitmapArrayConversions.updatebitmap_unsafe(img, output_bmp);
}
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;
}
}
}
/// <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>
/// 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>
/// 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);
}
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;
}
}
}