/// <summary>
/// use canny algorithm to find edges
/// </summary>
/// <param name="bmp"></param>
/// <param name="edge_detector"></param>
/// <param name="dots"></param>
/// <param name="edges_bmp"></param>
private static void DetectEdges(
Bitmap bmp,
ref EdgeDetectorCanny edge_detector,
ref hypergraph dots,
ref Bitmap edges_bmp)
{
byte[] image_data = new byte[bmp.Width * bmp.Height * 3];
BitmapArrayConversions.updatebitmap(bmp, image_data);
if (edge_detector == null) edge_detector = new EdgeDetectorCanny();
edge_detector.automatic_thresholds = true;
edge_detector.connected_sets_only = true;
byte[] edges_data = edge_detector.Update(image_data, bmp.Width, bmp.Height);
bool connected_sets_ok = false;
try
{
edges_data = edge_detector.GetConnectedSetsImage(image_data, 10, bmp.Width / SurveyorCalibration.dots_across * 3, true, ref dots);
connected_sets_ok = true;
}
catch
{
}
if (connected_sets_ok)
{
// locate the red centre dot
int max_redness = 0;
CalibrationDot centre_dot = null;
for (int i = 0; i < dots.Nodes.Count; i++)
{
CalibrationDot dot = (CalibrationDot)dots.Nodes[i];
int n = (((int)dot.y * bmp.Width) + (int)dot.x) * 3;
if ((n > 3) && (n < image_data.Length - 4))
{
int r = image_data[n + 2] + image_data[n + 2 + 3] + image_data[n + 2 - 3];
int g = image_data[n + 1] + image_data[n + 1 + 3] + image_data[n + 1 - 3];
int b = image_data[n] + image_data[n + 3] + image_data[n - 3];
int redness = (r * 2) - g - b;
if (redness > max_redness)
{
max_redness = redness;
centre_dot = dot;
}
}
}
if (centre_dot != null) centre_dot.centre = true;
if (edges_bmp == null)
edges_bmp = new Bitmap(bmp.Width, bmp.Height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
BitmapArrayConversions.updatebitmap_unsafe(edges_data, edges_bmp);
}
}
/// <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);
}
/// <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 the horizontal and vertical distance of the centre dot from the centre of the image
/// </summary>
/// <param name="image_width">
/// width of the image <see cref="System.Int32"/>
/// </param>
/// <param name="image_height">
/// height of the image <see cref="System.Int32"/>
/// </param>
/// <param name="dots">
/// detected dots <see cref="hypergraph"/>
/// </param>
/// <param name="horizontal_distance">
/// horizontal distance from the image centre in pixels <see cref="System.Single"/>
/// </param>
/// <param name="vertical_distance">
/// vertical distance from the image centre in pixels <see cref="System.Single"/>
/// </param>
private static void GetCentreDotDisplacement(int image_width, int image_height,
hypergraph dots,
ref float horizontal_distance,
ref float vertical_distance)
{
CalibrationDot centre_dot = null;
int i = 0;
while ((i < dots.Nodes.Count) && (centre_dot == null))
{
CalibrationDot dot = (CalibrationDot)dots.Nodes[i];
if (dot.centre)
centre_dot = dot;
else
i++;
}
if (centre_dot != null)
{
horizontal_distance = (float)centre_dot.x - (image_width / 2.0f);
vertical_distance = (float)centre_dot.y - (image_height / 2.0f);
}
}
/// <summary>
/// detects a square around the centre dot on the calibration pattern
/// </summary>
/// <param name="dots">detected dots</param>
/// <param name="centredots">returned dots belonging to the centre square</param>
/// <returns>centre square</returns>
private static polygon2D GetCentreSquare(hypergraph dots,
ref List<CalibrationDot> centredots)
{
centredots = new List<CalibrationDot>();
// find the centre dot
CalibrationDot centre = null;
int i = 0;
while ((i < dots.Nodes.Count) && (centre == null))
{
CalibrationDot dot = (CalibrationDot)dots.Nodes[i];
if (dot.centre) centre = dot;
i++;
}
// look for the four surrounding dots
List<CalibrationDot> centre_dots = new List<CalibrationDot>();
List<double> distances = new List<double>();
i = 0;
for (i = 0; i < dots.Nodes.Count; i++)
{
CalibrationDot dot = (CalibrationDot)dots.Nodes[i];
if (!dot.centre)
{
double dx = dot.x - centre.x;
double dy = dot.y - centre.y;
double dist = Math.Sqrt(dx * dx + dy * dy);
if (distances.Count == 4)
{
int index = -1;
double max_dist = 0;
for (int j = 0; j < 4; j++)
{
if (distances[j] > max_dist)
{
index = j;
max_dist = distances[j];
}
}
if (dist < max_dist)
{
distances[index] = dist;
centre_dots[index] = dot;
}
}
else
{
distances.Add(dist);
centre_dots.Add(dot);
}
}
}
polygon2D centre_square = null;
if (centre_dots.Count == 4)
{
centre_square = new polygon2D();
for (i = 0; i < 4; i++)
centre_square.Add(0, 0);
double xx = centre.x;
double yy = centre.y;
int index = 0;
for (i = 0; i < 4; i++)
{
if ((centre_dots[i].x < xx) &&
(centre_dots[i].y < yy))
{
xx = centre_dots[i].x;
yy = centre_dots[i].y;
centre_square.x_points[0] = (float)xx;
centre_square.y_points[0] = (float)yy;
index = i;
}
}
centredots.Add(centre_dots[index]);
xx = centre.x;
yy = centre.y;
for (i = 0; i < 4; i++)
{
if ((centre_dots[i].x > xx) &&
(centre_dots[i].y < yy))
{
xx = centre_dots[i].x;
yy = centre_dots[i].y;
centre_square.x_points[1] = (float)xx;
centre_square.y_points[1] = (float)yy;
index = i;
}
}
centredots.Add(centre_dots[index]);
xx = centre.x;
yy = centre.y;
for (i = 0; i < 4; i++)
{
if ((centre_dots[i].x > xx) &&
(centre_dots[i].y > yy))
{
xx = centre_dots[i].x;
yy = centre_dots[i].y;
centre_square.x_points[2] = (float)xx;
centre_square.y_points[2] = (float)yy;
index = i;
}
}
centredots.Add(centre_dots[index]);
xx = centre.x;
yy = centre.y;
for (i = 0; i < 4; i++)
{
if ((centre_dots[i].x < xx) &&
(centre_dots[i].y > yy))
{
xx = centre_dots[i].x;
yy = centre_dots[i].y;
centre_square.x_points[3] = (float)xx;
centre_square.y_points[3] = (float)yy;
index = i;
}
}
centredots.Add(centre_dots[index]);
}
return (centre_square);
}
/// <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 ApplyGrid(hypergraph dots,
List<CalibrationDot> centre_dots)
{
const int UNASSIGNED = 9999;
// mark all dots as unassigned
for (int i = 0; i < dots.Nodes.Count; i++)
{
CalibrationDot dot = (CalibrationDot)dots.Nodes[i];
dot.grid_x = UNASSIGNED;
dot.grid_y = UNASSIGNED;
}
// assign grid positions to the four dots
// surrounding the centre dot
centre_dots[0].grid_x = -1;
centre_dots[0].grid_y = 1;
centre_dots[1].grid_x = 0;
centre_dots[1].grid_y = 1;
centre_dots[2].grid_x = 0;
centre_dots[2].grid_y = 0;
centre_dots[3].grid_x = -1;
centre_dots[3].grid_y = 0;
int dots_assigned = 4;
// recursively assign grid positions to dots
for (int i = 0; i < centre_dots.Count; i++)
ApplyGrid(centre_dots[i], UNASSIGNED, ref dots_assigned);
Console.WriteLine(dots_assigned.ToString() + " dots assigned grid coordinates");
}
/// <summary>
/// shows the grid coordinates for each detected dot
/// </summary>
/// <param name="filename">raw image filename</param>
/// <param name="dots">detected dots</param>
/// <param name="output_filename">filename to save as</param>
private static void ShowGridCoordinates(string filename,
hypergraph dots,
string output_filename)
{
Bitmap bmp = (Bitmap)Bitmap.FromFile(filename);
byte[] img = new byte[bmp.Width * bmp.Height * 3];
BitmapArrayConversions.updatebitmap(bmp, img);
for (int i = 0; i < dots.Nodes.Count; i++)
{
CalibrationDot dot = (CalibrationDot)dots.Nodes[i];
drawing.drawCircle(img, bmp.Width, bmp.Height, (float)dot.x, (float)dot.y, dot.radius, 0, 255, 0, 0);
if (dot.grid_x != 9999)
{
string coord = dot.grid_x.ToString() + "," + dot.grid_y.ToString();
drawing.AddText(img, bmp.Width, bmp.Height, coord, "Courier New", 8, 0, 0, 0, (int)dot.x - 0, (int)dot.y + 5);
}
}
Bitmap output_bmp = new Bitmap(bmp.Width, bmp.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);
}
/// <summary>
/// shows the grid fitted to the calibration pattern
/// </summary>
/// <param name="filename">raw image filename</param>
/// <param name="dots">detected dots on the grid</param>
/// <param name="search_regions">search regions around the dots</param>
/// <param name="output_filename">filename to save as</param>
private static void ShowGrid(string filename,
hypergraph dots,
List<CalibrationDot> search_regions,
string output_filename)
{
Bitmap bmp = (Bitmap)Bitmap.FromFile(filename);
byte[] img = new byte[bmp.Width * bmp.Height * 3];
BitmapArrayConversions.updatebitmap(bmp, img);
if (search_regions != null)
{
for (int i = 0; i < search_regions.Count; i++)
{
CalibrationDot dot = (CalibrationDot)search_regions[i];
drawing.drawCircle(img, bmp.Width, bmp.Height, (float)dot.x, (float)dot.y, (float)dot.radius, 255, 255, 0, 0);
}
}
for (int i = 0; i < dots.Nodes.Count; i++)
{
CalibrationDot dot = (CalibrationDot)dots.Nodes[i];
drawing.drawCircle(img, bmp.Width, bmp.Height, (float)dot.x, (float)dot.y, dot.radius, 0, 255, 0, 0);
}
for (int i = 0; i < dots.Links.Count; i++)
{
CalibrationDot from_dot = (CalibrationDot)dots.Links[i].From;
CalibrationDot to_dot = (CalibrationDot)dots.Links[i].To;
drawing.drawLine(img, bmp.Width, bmp.Height, (int)from_dot.x, (int)from_dot.y, (int)to_dot.x, (int)to_dot.y, 255, 0, 0, 0, false);
}
Bitmap output_bmp = new Bitmap(bmp.Width, bmp.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);
}
protected Bitmap DetectEdges(Bitmap bmp, EdgeDetectorCanny edge_detector,
ref hypergraph dots)
{
usage.Update("Detect edges, BaseVisionStereo, DetectEdges");
byte[] image_data = new byte[bmp.Width * bmp.Height * 3];
BitmapArrayConversions.updatebitmap(bmp, image_data);
if (edge_detector == null) edge_detector = new EdgeDetectorCanny();
edge_detector.automatic_thresholds = true;
edge_detector.connected_sets_only = true;
byte[] edges_data = edge_detector.Update(image_data, bmp.Width, bmp.Height);
edges_data = edge_detector.GetConnectedSetsImage(image_data, 10, bmp.Width / SurveyorCalibration.dots_across * 3, true, ref dots);
Bitmap edges_bmp = new Bitmap(bmp.Width, bmp.Height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
BitmapArrayConversions.updatebitmap_unsafe(edges_data, edges_bmp);
return(edges_bmp);
}
/// <summary>
/// shows the grid fitted to the calibration dots
/// </summary>
/// <param name="bmp">raw image</param>
/// <param name="dots">detected dots on the grid</param>
/// <param name="search_regions">search regions around the dots</param>
private static void ShowLinkedDots(Bitmap bmp,
hypergraph dots,
List<CalibrationDot> search_regions,
ref Bitmap output_bmp)
{
byte[] img = new byte[bmp.Width * bmp.Height * 3];
BitmapArrayConversions.updatebitmap(bmp, img);
if (search_regions != null)
{
for (int i = 0; i < search_regions.Count; i++)
{
CalibrationDot dot = (CalibrationDot)search_regions[i];
drawing.drawCircle(img, bmp.Width, bmp.Height, (float)dot.x, (float)dot.y, (float)dot.radius, 255, 255, 0, 0);
}
}
for (int i = 0; i < dots.Nodes.Count; i++)
{
CalibrationDot dot = (CalibrationDot)dots.Nodes[i];
drawing.drawCircle(img, bmp.Width, bmp.Height, (float)dot.x, (float)dot.y, dot.radius, 0, 255, 0, 0);
}
for (int i = 0; i < dots.Links.Count; i++)
{
CalibrationDot from_dot = (CalibrationDot)dots.Links[i].From;
CalibrationDot to_dot = (CalibrationDot)dots.Links[i].To;
drawing.drawLine(img, bmp.Width, bmp.Height, (int)from_dot.x, (int)from_dot.y, (int)to_dot.x, (int)to_dot.y, 255, 0, 0, 0, false);
}
if (output_bmp == null)
output_bmp = new Bitmap(bmp.Width, bmp.Height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
BitmapArrayConversions.updatebitmap_unsafe(img, output_bmp);
}
/// <summary>
/// is the given set of connected dots valid ?
/// </summary>
/// <param name="graph"></param>
/// <param name="image_width"></param>
/// <param name="image_height"></param>
/// <param name="tollerance_degrees"></param>
/// <returns>
/// </returns>
private static bool ValidGrid(hypergraph graph,
int image_width, int image_height,
int tollerance_degrees)
{
bool is_valid = true;
int tx = image_width / 3;
int bx = image_width - tx;
int ty = image_height / 3;
int by = image_height - tx;
int i = 0;
while ((i < graph.Nodes.Count) && (is_valid))
{
CalibrationDot n = (CalibrationDot)graph.Nodes[i];
if ((n.x > tx) && (n.x < bx) &&
(n.y > ty) && (n.y < by))
{
int j = 0;
while ((j < n.Links.Count) && (is_valid))
{
CalibrationLink link = (CalibrationLink)n.Links[j];
double dx = ((CalibrationDot)link.From).x - n.x;
double dy = ((CalibrationDot)link.From).y - n.y;
double dist = Math.Sqrt(dx*dx + dy*dy);
if (dist > 0)
{
double angle = Math.Asin(dx / dist);
if (dy < 0) angle = (Math.PI * 2) - angle;
angle = angle / Math.PI * 180;
if (angle < 0) angle += 360;
bool within_tollerance = false;
if (angle > 360 - tollerance_degrees) within_tollerance = true;
if (angle < tollerance_degrees) within_tollerance = true;
if ((angle > 180 - tollerance_degrees) &&
(angle < 180 + tollerance_degrees)) within_tollerance = true;
if ((angle > 90 - tollerance_degrees) &&
(angle < 90 + tollerance_degrees)) within_tollerance = true;
if ((angle > 270 - tollerance_degrees) &&
(angle < 270 + tollerance_degrees)) within_tollerance = true;
is_valid = within_tollerance;
}
j++;
}
}
i++;
}
return(is_valid);
}
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);
}
