CFeatureLines.cpp

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          https://www.mrpt.org/                            |
   |                                                                           |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
   | See: https://www.mrpt.org/Authors - All rights reserved.                  |
   | Released under BSD License. See details in https://www.mrpt.org/License   |
   +---------------------------------------------------------------------------+
 */

#include "vision-precomp.h"  // Precompiled headers

#include <mrpt/core/exceptions.h>
#include <mrpt/vision/CFeatureLines.h>
#include <mrpt/vision/utils.h>

// Universal include for all versions of OpenCV
#include <mrpt/3rdparty/do_opencv_includes.h>

using namespace mrpt::vision;
using namespace std;

#if MRPT_HAS_OPENCV

static void extractLines_CannyHough(
    const cv::Mat& canny_image, const std::vector<cv::Vec2f> lines,
    std::vector<cv::Vec4i>& segments, size_t threshold)
{
    // Some variables to change the coordinate system from polar to cartesian
    double rho, theta;
    double cosTheta, sinTheta;
    double m, c, cMax;

    // Variables to define the two extreme points on the line, clipped on the
    // window The constraint is x <= xF
    int x, y, xF, yF;

    segments.clear();

    // For each line
    for (size_t n(0); n < lines.size(); ++n)
    {
        // OpenCV implements the Hough accumulator with theta from 0 to PI, thus
        // rho could be negative We want to always have rho >= 0
        if (lines[n][0] < 0)
        {
            rho = -lines[n][0];
            theta = lines[n][1] - CV_PI;
        }
        else
        {
            rho = lines[n][0];
            theta = lines[n][1];
        }

        if (rho == 0 && theta == 0)
        {  // That case appeared at least once, so a test is needed
            continue;
        }

        // Since the step for theta in cv::HoughLines should not be too small,
        // theta should exactly be 0 (and the line vertical) when this condition
        // is true
        if (fabs(theta) < 0.00001)
        {
            x = xF = lround(rho);
            y = 0;
            yF = canny_image.rows - 1;
        }
        else
        {
            // Get the (m, c) slope and y-intercept from (rho, theta), so that y
            // = mx + c <=> (x, y) is on the line
            cosTheta = cos(theta);
            sinTheta = sin(theta);
            m = -cosTheta / sinTheta;  // We are certain that sinTheta != 0
            c = rho * (sinTheta - m * cosTheta);

            // Get the two extreme points (x, y) and (xF, xF) for the line
            // inside the window, using (m, c)
            if (c >= 0)
            {
                if (c < canny_image.rows)
                {
                    // (x, y) is at the left of the window
                    x = 0;
                    y = static_cast<int>(c);
                }
                else
                {
                    // (x, y) is at the bottom of the window
                    y = canny_image.rows - 1;
                    x = static_cast<int>((y - c) / m);
                }
            }
            else
            {
                // (x, y) is at the top of the window
                x = static_cast<int>(-c / m);
                y = 0;
            }
            // Define the intersection with the right side of the window
            cMax = m * (canny_image.cols - 1) + c;
            if (cMax >= 0)
            {
                if (cMax < canny_image.rows)
                {
                    // (xF, yF) is at the right of the window
                    xF = canny_image.cols - 1;
                    yF = static_cast<int>(cMax);
                }
                else
                {
                    // (xF, yF) is at the bottom of the window
                    yF = canny_image.rows - 1;
                    xF = static_cast<int>((yF - c) / m);
                }
            }
            else
            {
                // (xF, yF) is at the top of the window
                xF = static_cast<int>(-c / m);
                yF = 0;
            }
        }

        // Going through the line using the Bresenham algorithm
        // dx1, dx2, dy1 and dy2 are increments that allow to be successful for
        // each of the 8 octants (possible directions while iterating)
        bool onSegment = false;
        int memory;
        int memoryX = 0, memoryY = 0;
        int xPrev = 0, yPrev = 0;
        size_t nbPixels = 0;

        int w = xF - x;
        int h = yF - y;
        int dx1, dy1, dx2, dy2 = 0;

        int longest, shortest;
        int numerator;

        if (w < 0)
        {
            longest = -w;
            dx1 = -1;
            dx2 = -1;
        }
        else
        {
            longest = w;
            dx1 = 1;
            dx2 = 1;
        }

        if (h < 0)
        {
            shortest = -h;
            dy1 = -1;
        }
        else
        {
            shortest = h;
            dy1 = 1;
        }

        // We want to know whether the direction is more horizontal or vertical,
        // and set the increments accordingly
        if (longest <= shortest)
        {
            memory = longest;
            longest = shortest;
            shortest = memory;
            dx2 = 0;
            if (h < 0)
            {
                dy2 = -1;
            }
            else
            {
                dy2 = 1;
            }
        }

        numerator = longest / 2;

        // cout << n << " numerator " << numerator << endl;
        for (int i(0); i <= longest; ++i)
        {
            // For each pixel, we don't want to use a classic "plot", but to
            // look into canny_image for a black or white pixel
            if (onSegment)
            {
                if (canny_image.at<char>(y, x) == 0 || i == longest)
                {
                    // We are leaving a segment
                    onSegment = false;
                    if (nbPixels >= threshold)
                    {
                        segments.emplace_back(memoryX, memoryY, xPrev, yPrev);
                        // cout << "new segment " << segments.back() << endl;
                    }
                }
                else
                {
                    // We are still on a segment
                    ++nbPixels;
                }
            }
            else if (canny_image.at<char>(y, x) != 0)
            {
                // We are entering a segment, and keep this first position in
                // (memoryX, memoryY)
                onSegment = true;
                nbPixels = 0;
                memoryX = x;
                memoryY = y;
            }

            // xPrev and yPrev are used when leaving a segment, to keep in
            // memory the last pixel on it
            xPrev = x;
            yPrev = y;

            // Next pixel using the condition of the Bresenham algorithm
            numerator += shortest;
            if (numerator >= longest)
            {
                numerator -= longest;
                x += dx1;
                y += dy1;
            }
            else
            {
                x += dx2;
                y += dy2;
            }
        }
    }
}
#endif

void CFeatureLines::extractLines(
    const mrpt::img::CImage& img_in,
    std::vector<std::array<int, 4>>& segments_out, size_t threshold,
    const bool display)
{
#if MRPT_HAS_OPENCV

    using namespace cv;
    using namespace std;
    const Mat image = img_in.asCvMat<Mat>(mrpt::img::SHALLOW_COPY);

    // Canny edge detector
    cv::Mat canny_img;
    int lowThreshold = 150;
    // int const max_lowThreshold = 100;
    int ratio = 3;
    int kernel_size = 3;
    cv::Canny(
        image, canny_img, lowThreshold, lowThreshold * ratio,
        kernel_size);  // 250, 600 // CAUTION: Both thresholds depend on the
    // input image, they might be a bit hard to set because
    // they depend on the strength of the gradients
    //            cv::namedWindow("Canny detector", cv::WINDOW_AUTOSIZE);
    //            cv::imshow("Canny detector", canny_img);
    //            cv::waitKey(0);
    //            cv::destroyWindow("Canny detector");

    // Get lines through the Hough transform

    // CvMat matCannyStub, *matCannyImg = (CvMat*)canny_img.getAs<IplImage>();
    // CV_CALL(matCannyImg = cvGetMat(canny_img.getAs<IplImage>(),
    // &matCannyStub));

    std::vector<cv::Vec2f> lines;
    cv::HoughLines(
        canny_img, lines, 1, CV_PI / 180.0,
        threshold);  // CAUTION: The last parameter depends on the input image,
    // it's the smallest number of pixels to consider a line in
    // the accumulator
    //    double minLineLength=50, maxLineGap=5;
    //    cv::HoughLinesP(canny_img, lines, 1, CV_PI / 180.0, threshold,
    //    minLineLength, maxLineGap); // CAUTION: The last parameter depends on
    //    the input image, it's the smallest number of pixels to consider a line
    //    in the accumulator
    // std::cout << lines.size() << " lines detected" << std::endl;

    // Possible dilatation of the canny detector
    // Useful when the lines are thin and not perfectly straight
    cv::Mat filteredCanny;
    // Choose whether filtering the Canny or not, for thin and non-perfect edges
    /*filteredCanny = canny_img.clone();*/
    cv::dilate(canny_img, filteredCanny, cv::Mat());
    //    cv::namedWindow("Filtered Canny detector");
    //    cv::imshow("Filtered Canny detector", filteredCanny);
    //    cv::waitKey(0);
    //    cv::destroyWindow("Filtered Canny detector");

    // Extracting segments (pairs of points) from the filtered Canny detector
    // And using the line parameters from lines
    std::vector<cv::Vec4i> segments;
    extractLines_CannyHough(filteredCanny, lines, segments, threshold);

    // Force the segments to have a predefined order (e1y <= e2y)
    for (auto line = begin(segments); line != end(segments); ++line)
        if ((*line)[1] == (*line)[3])
        {
            if ((*line)[0] > (*line)[1])
                *line =
                    cv::Vec4i((*line)[2], (*line)[3], (*line)[0], (*line)[1]);
        }
        else if ((*line)[1] < (*line)[3])
            *line = cv::Vec4i((*line)[2], (*line)[3], (*line)[0], (*line)[1]);

    // convert cv->STL:
    segments_out.resize(segments.size());
    for (size_t i = 0; i < segments.size(); i++)
        for (size_t k = 0; k < segments_out[i].size(); k++)
            segments_out[i][k] = segments[i][k];

    // Display 2D segments
    if (display)
    {
        cv::Mat image_lines;
        image.convertTo(image_lines, CV_8UC1, 1.0 / 2);
        for (auto line = begin(segments); line != end(segments); ++line)
        {
            cv::line(
                image_lines, cv::Point((*line)[0], (*line)[1]),
                cv::Point((*line)[2], (*line)[3]), cv::Scalar(255, 0, 255), 1);
            cv::circle(
                image_lines, cv::Point((*line)[0], (*line)[1]), 3,
                cv::Scalar(255, 0, 255), 3);
            cv::putText(
                image_lines, string(to_string(distance(begin(segments), line))),
                cv::Point(
                    ((*line)[0] + (*line)[2]) / 2,
                    ((*line)[1] + (*line)[3]) / 2),
                0, 1.2, cv::Scalar(200, 0, 0), 3);
        }
        cv::imshow("lines", image_lines);
        cv::moveWindow("lines", 20, 100 + 700);
        cv::waitKey(0);
    }
#else
    THROW_EXCEPTION("Requires building MRPT with OpenCV support");
#endif
}