Example #1
0
        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);
        }
Example #2
0
        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;
        }
Example #3
0
        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);
        }
Example #4
0
 /// <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);
         }
     }
 }
Example #5
0
        /// <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);
        }