public static float horizon_detection(Byte[] colour_image, int img_width, int img_height,
FASTline[] lines, int horizon_threshold,
ref int vertical_position)
{
float gradient = 0;
float gravity_angle = 0;
int hits = 0;
int av_y = 0;
vertical_position = 0;
if (lines != null)
{
for (int i = 0; i < lines.Length; i++)
{
FASTline line = lines[i];
if (line.Visible)
{
if (line.isOnHorizon(colour_image, img_width, img_height, horizon_threshold))
{
line.onHorizon = true;
float dx = line.point2.x - line.point1.x;
float dy = line.point2.y - line.point1.y;
float grad = 0;
if (dy != 0) grad = dx / dy;
int y = (int)((grad * (img_width / 2)) + line.point1.y);
if ((y > 0) && (y < img_height))
{
av_y += y;
hits++;
gradient += grad;
}
}
}
}
}
if (hits > 0)
{
av_y /= hits;
gradient /= (float)hits;
vertical_position = av_y;
gravity_angle = gradient;
}
return (gravity_angle);
}
public void Update(Byte[] raw_image, int width, int height)
{
mono_image = image.monoImage(raw_image, width, height);
if (line_threshold == 0)
{
line_threshold = 200;
}
if (corner_threshold == 0)
{
corner_threshold = 50;
}
FASTcorner[] corners_all = FAST.fast_corner_detect_10(mono_image, width, height, corner_threshold);
FASTcorner[] corners1 = FAST.fast_nonmax(mono_image, width, height, corners_all, corner_threshold * 2, 0, 0);
corners = local_nonmax(corners1, width / 15, height / 15);
if (corners != null)
{
int no_of_feats = corners1.Length;
if (no_of_feats < required_features / 2)
{
corner_threshold -= 4;
}
if (no_of_feats > required_features)
{
corner_threshold += 4;
}
if ((no_of_feats > 1) && (no_of_feats < required_features * 2))
{
int min_line_length = width / 10;
lines = FAST.fast_lines(mono_image, width, height, corners, line_threshold, min_line_length);
if (lines != null)
{
int no_of_lines = 0;
for (int i = 0; i < lines.Length; i++)
{
FASTline line = lines[i];
if (!(((line.point1.y < height / 10) && (line.point2.y < height / 10)) ||
((line.point1.y > height - (height / 20)) && (line.point2.y > height - (height / 20))) ||
((line.point1.x > width - (width / 20)) && (line.point2.x > width - (width / 20))) ||
((line.point1.x < width / 20) && (line.point2.x < width / 20))
))
{
line.Visible = true;
no_of_lines++;
}
}
// detect the horizon
if (showHorizon)
{
int vertical_position = 0;
float gradient = FAST.horizon_detection(raw_image, width, height, lines, horizon_threshold, ref vertical_position);
if (vertical_position > 0)
{
int tx = (width / 2) + (int)((width / 4) * 1);
int ty = vertical_position - (int)((width / 4) * gradient);
//ty = ty * height / width;
drawing.drawLine(raw_image, width, height, width / 2, vertical_position, tx, ty, 0, 255, 0, 1, false);
}
}
// draw lines
if (drawLines)
{
for (int i = 0; i < lines.Length; i++)
{
FASTline line = lines[i];
if (line.Visible)
{
if (!line.onHorizon)
{
drawing.drawLine(raw_image, width, height,
line.point1.x, line.point1.y, line.point2.x, line.point2.y,
255, 0, 0, 0, false);
}
else
{
drawing.drawLine(raw_image, width, height,
line.point1.x, line.point1.y, line.point2.x, line.point2.y,
0, 255, 0, 0, false);
}
}
}
}
if (no_of_lines < required_lines)
{
line_threshold -= 4;
}
if (no_of_lines > required_lines)
{
line_threshold += 8;
}
if (line_threshold < 100)
{
line_threshold = 100;
}
// detect the gravity angle
if (showGravity)
{
float gravity_angle = FAST.gravity_direction(lines);
if (drawLines)
{
int pendulum_length = width / 8;
int px = (width / 2) + (int)(pendulum_length * Math.Sin(gravity_angle));
int py = (height / 2) + (int)(pendulum_length * Math.Cos(gravity_angle) * height / width);
drawing.drawLine(raw_image, width, height,
width / 2, height / 2, px, py,
0, 255, 0, 1, false);
int arrow_length = pendulum_length / 5;
float angle2 = gravity_angle + (float)(Math.PI * 0.8f);
int px2 = px + (int)(arrow_length * Math.Sin(angle2));
int py2 = py + (int)(arrow_length * Math.Cos(angle2) * height / width);
drawing.drawLine(raw_image, width, height,
px2, py2, px, py,
0, 255, 0, 1, false);
angle2 = gravity_angle - (float)(Math.PI * 0.8f);
px2 = px + (int)(arrow_length * Math.Sin(angle2));
py2 = py + (int)(arrow_length * Math.Cos(angle2) * height / width);
drawing.drawLine(raw_image, width, height,
px2, py2, px, py,
0, 255, 0, 1, false);
}
}
}
}
}
//odometry.update(disp_bmp_data, width, height);
output_image = raw_image;
}
public static float gravity_direction(FASTline[] lines)
{
float gravity_angle = 0;
if (lines != null)
{
int[] orientation_histogram = new int[30];
for (int i = 0; i < lines.Length; i++)
{
FASTline line = lines[i];
if (line.Visible)
{
// get the orientation of the line
int dx = line.point2.x - line.point1.x;
int dy = line.point2.y - line.point1.y;
int length = (int)Math.Sqrt((dx * dx) + (dy * dy));
if (length > 0)
{
float angle = (float)Math.Asin(dx / (float)length);
if (dy < 0) angle = (float)(Math.PI * 2) - angle;
if (angle < 0) angle = (float)(Math.PI) + angle;
int bin_number = (int)(angle / (float)(Math.PI * 2) * (orientation_histogram.Length - 1));
orientation_histogram[bin_number] += 1;
}
}
}
int max = 0;
for (int i = 0; i < orientation_histogram.Length / 4; i++)
{
int curr_i = i;
int prev_i = curr_i - 1;
if (prev_i < 0) prev_i = orientation_histogram.Length + prev_i;
int next_i = curr_i + 1;
if (next_i >= orientation_histogram.Length) next_i -= orientation_histogram.Length;
int value = orientation_histogram[curr_i] + ((orientation_histogram[prev_i] + orientation_histogram[next_i]) / 4);
curr_i = i + (orientation_histogram.Length / 4);
prev_i = curr_i - 1;
if (prev_i < 0) prev_i = orientation_histogram.Length + prev_i;
next_i = curr_i + 1;
if (next_i >= orientation_histogram.Length) next_i -= orientation_histogram.Length;
value += orientation_histogram[curr_i] + ((orientation_histogram[prev_i] + orientation_histogram[next_i]) / 4);
curr_i = i + (orientation_histogram.Length / 2);
prev_i = curr_i - 1;
if (prev_i < 0) prev_i = orientation_histogram.Length + prev_i;
next_i = curr_i + 1;
if (next_i >= orientation_histogram.Length) next_i -= orientation_histogram.Length;
value += orientation_histogram[curr_i] + ((orientation_histogram[prev_i] + orientation_histogram[next_i]) / 4);
if (value > max)
{
max = value;
gravity_angle = i * (float)Math.PI * 2 / orientation_histogram.Length;
}
}
}
if (gravity_angle > (float)(Math.PI / 4))
gravity_angle -= (float)(Math.PI / 2);
return (gravity_angle);
}
/// <summary>
/// show the detected line features in the given image
/// </summary>
/// <param name="lines">array containing detected lines</param>
/// <param name="background">background image to be used</param>
/// <param name="bmp">image to be returned</param>
/// <param name="image_width">width of the image</param>
/// <param name="image_height">height of the image</param>
/// <param name="bytes_per_pixel">number of bytes per pixel</param>
public static void Show(FASTline[] lines,
byte[] background,
byte[] bmp, int image_width, int image_height,
int bytes_per_pixel)
{
// copy the background
for (int i = 0; i < image_width * image_height * bytes_per_pixel; i++)
bmp[i] = background[i];
for (int i = 0; i < lines.Length; i++)
{
FASTline line = lines[i];
if (bytes_per_pixel == 3)
{
sluggish.utilities.drawing.drawLine(bmp, image_width, image_height,
line.point1.x, line.point1.y, line.point2.x, line.point2.y,
0, 255, 0, 0, false);
}
}
}
/// <summary>
/// Detect lines in the given mono image using the given corner features
/// </summary>
public static FASTline[] fast_lines(Byte[] img, int xsize, int ysize,
FASTcorner[] corners, int line_threshold, int min_length)
{
int min_length_y = min_length * ysize / xsize;
int no_of_lines = 0;
FASTline[] lines = new FASTline[corners.Length * corners.Length];
for (int i = 0; i < corners.Length - 1; i++)
{
FASTcorner corner1 = corners[i];
for (int j = i + 1; j < corners.Length; j++)
{
FASTcorner corner2 = corners[j];
int dx = corner2.x - corner1.x;
if (dx < 0) dx = -dx;
int dy = corner2.y - corner1.y;
if (dy < 0) dy = -dy;
if ((dx > min_length) || (dy > min_length_y))
{
if (FASTline.isLine(img, xsize, ysize, 1, corner1, corner2, line_threshold))
{
lines[no_of_lines] = new FASTline(corner1, corner2);
no_of_lines++;
}
}
}
}
if (no_of_lines > 0)
#if DOTNET_V1_1
lines = (FASTline[])ArrayResize(lines, no_of_lines);
#else
Array.Resize(ref lines, no_of_lines);
#endif
else
lines = null;
return (lines);
}
