/// <summary>
/// calculate the disparity values for observed lines
/// </summary>
private void updateLineDisparities()
{
if (line_detector != null)
{
if (line_detector.lines != null)
{
const int linepoints = 8;
for (int i = 0; i < line_detector.lines.Length; i++)
{
FASTline line = line_detector.lines[i];
if (line.Visible)
{
int dx = line.point2.x - line.point1.x;
int dy = line.point2.y - line.point1.y;
float tot_lower = 0;
int lower_hits = 0;
float tot_upper = 0;
int upper_hits = 0;
for (int j = 0; j < linepoints; j++)
{
int xx = line.point1.x + (dx * j / linepoints);
int yy = line.point1.y + (dy * j / linepoints);
float disp = stereovision_contours.getDisparityMapPoint(xx, yy);
if (disp > 0)
{
if (j < linepoints / 2)
{
tot_lower += disp;
lower_hits++;
}
else
{
tot_upper += disp;
upper_hits++;
}
}
}
if ((lower_hits > 0) && (upper_hits > 0))
{
float av_lower = tot_lower / lower_hits;
float av_upper = tot_upper / upper_hits;
line.point1.disparity = av_lower;
line.point2.disparity = av_upper;
}
}
}
}
}
}
public void Update(Byte[] raw_image, int width, int height)
{
mono_image = sluggish.utilities.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;
sluggish.utilities.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)
{
sluggish.utilities.drawing.drawLine(raw_image, width, height,
line.point1.x, line.point1.y, line.point2.x, line.point2.y,
255, 0, 0, 0, false);
}
else
{
sluggish.utilities.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);
sluggish.utilities.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);
sluggish.utilities.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);
sluggish.utilities.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;
}
