opencv C++霍夫曼Line实现

极坐标=》霍夫空间
HoughLines 与 HoughLinesP区别：
1.HoughLines 返回直线参数，r和theta,他对于连续的直线检测效果较好，比如边缘直线。
2.HoughLinesP返回线段起点和终点坐标，适合线段检测。

#include <cmath>
#include <opencv2/core/cvdef.h>
#include<stdio.h>
#include <vector>
#include <opencv2/core/mat.hpp>
#include "Hou.h"



struct LinePolar
{
    float rho;
    float angle;
};
/// <summary>
/// 两个角度之间的差值接近pi,
/// 可能导致一个线段被检测两次
/// </summary>
static inline int computeNumangle(double min_theta, double max_theta, double theta_step)
{
    int numangle = floor((max_theta - min_theta) / theta_step) + 1;
    if (numangle > 1 && fabs(CV_PI - (numangle - 1) * theta_step) < theta_step / 2)
        --numangle;
    return numangle;
}
 /// <summary>
 /// 极坐标系数值计算 sin cos
 /// </summary>
void createTrigTable(int numangle, double min_theta, 
    double theta_step,
    float irho, float* tabSin, 
    float* tabCos)
{
    float ang = static_cast<float>(min_theta);
    for (int n = 0; n < numangle; ang += (float)theta_step, n++)
    {
        tabSin[n] = (float)(sin((double)ang) * irho);
        tabCos[n] = (float)(cos((double)ang) * irho);
    }
}

 /// <summary>
 /// 阈值内的保留
 /// </summary>
void findLocalMaximums(int numrho, int numangle,
    int threshold,
    const int* accum,
    std::vector<int>& sort_buf)
{
    for (int r = 0; r < numrho; r++)
        for (int n = 0; n < numangle; n++)
        {
            int base = (n + 1) * (numrho + 2) + r + 1;
            if (accum[base] > threshold &&
                accum[base] > accum[base - 1] && accum[base] >= accum[base + 1] &&
                accum[base] > accum[base - numrho - 2] && accum[base] >= accum[base + numrho + 2])
                sort_buf.push_back(base);
        }
}

/// <summary>
/// 排序
/// </summary>
struct hough_cmp_gt
{
    hough_cmp_gt(const int* _aux) : aux(_aux) {}
    inline bool operator()(int l1, int l2) const
    {
        return aux[l1] > aux[l2] || (aux[l1] == aux[l2] && l1 < l2);
    }
    const int* aux;
};

void Hou::HoughLinesStandard(cv::InputArray src, cv::OutputArray lines, int type,
    float rho, float theta,
    int threshold, int linesMax,
    double min_theta, double max_theta)
{
    CV_CheckType(type, type == CV_32FC2 || type == CV_32FC3, "Internal error");

    cv::Mat img = src.getMat();

    int i, j;
    float irho = 1 / rho;

    CV_Assert(img.type() == CV_8UC1);
    CV_Assert(linesMax > 0);

    const uchar* image = img.ptr();
    int step = (int)img.step;
    int width = img.cols;
    int height = img.rows;

    int max_rho = width + height;
    int min_rho = -max_rho;

    CV_CheckGE(max_theta, min_theta, "max_theta must be greater than min_theta");

    int numangle = computeNumangle(min_theta, max_theta, theta);
    int numrho = cvRound(((max_rho - min_rho) + 1) / rho);


    cv::Mat _accum = cv::Mat::zeros((numangle + 2), (numrho + 2), CV_32SC1);
    std::vector<int> _sort_buf;
    cv::AutoBuffer<float> _tabSin(numangle);
    cv::AutoBuffer<float> _tabCos(numangle);
    int* accum = _accum.ptr<int>();
    float* tabSin = _tabSin.data(), * tabCos = _tabCos.data();

    //创建极坐标计算值
    createTrigTable(numangle, min_theta, theta,
        irho, tabSin, tabCos);

    //累加计数
    for (i = 0; i < height; i++)
        for (j = 0; j < width; j++)
        {
            if (image[i * step + j] != 0)
                for (int n = 0; n < numangle; n++)
                {
                    int r = cvRound(j * tabCos[n] + i * tabSin[n]);
                    r += (numrho - 1) / 2;
                    accum[(n + 1) * (numrho + 2) + r + 1]++;
                }
        }

    //阈值内保留
    findLocalMaximums(numrho, numangle, threshold, accum, _sort_buf);

    // 排序
    std::sort(_sort_buf.begin(), _sort_buf.end(), hough_cmp_gt(accum));

    // 输出值转化
    linesMax = std::min(linesMax, (int)_sort_buf.size());
    double scale = 1. / (numrho + 2);
    lines.create(linesMax, 1, type);
    cv::Mat _lines = lines.getMat();
    for (i = 0; i < linesMax; i++)
    {
        LinePolar line;
        int idx = _sort_buf[i];
        int n = cvFloor(idx * scale) - 1;
        int r = idx - (n + 1) * (numrho + 2) - 1;
        line.rho = (r - (numrho - 1) * 0.5f) * rho;
        line.angle = static_cast<float>(min_theta) + n * theta;
        if (type == CV_32FC2)
        {
            _lines.at<cv::Vec2f>(i) = cv::Vec2f(line.rho, line.angle);
        }
        else
        {
            CV_DbgAssert(type == CV_32FC3);
            _lines.at<cv::Vec3f>(i) = cv::Vec3f(line.rho, line.angle, (float)accum[idx]);
        }
    }
}
  
void Hou::HoughLinesProbabilistic(cv::Mat& image,
    float rho,
    float theta,
    int threshold,
    int lineLength,
    int lineGap,
    std::vector<cv::Vec4i>& lines,
    int linesMax)
{
    cv::Point pt;
    float irho = 1 / rho;
    cv::RNG rng((uint64)-1);

    CV_Assert(image.type() == CV_8UC1);

    int width = image.cols;
    int height = image.rows;

    int numangle = computeNumangle(0.0, CV_PI, theta);
    int numrho = cvRound(((width + height) * 2 + 1) / rho);

    cv::Mat accum = cv::Mat::zeros(numangle, numrho, CV_32SC1);
    cv::Mat mask(height, width, CV_8UC1);
    std::vector<float> trigtab(numangle * 2);

    for (int n = 0; n < numangle; n++)
    {
        trigtab[n * 2] = (float)(cos((double)n * theta) * irho);//0，2，4(cos值)
        trigtab[n * 2 + 1] = (float)(sin((double)n * theta) * irho);//1，3，5(sin值)
        std::cout << "cos:" << trigtab[n * 2] <<"theta："<<n << std::endl;
        std::cout << "sin:" << trigtab[n * 2+1] << "theta：" << n <<std::endl;
    
    }
    const float* ttab = &trigtab[0];
    uchar* mdata0 = mask.ptr();
    std::vector<cv::Point> nzloc;

    //保留非黑的像素点
    //mask0像素点为1，0为0
    //0-1二值图
    for (pt.y = 0; pt.y < height; pt.y++)
    {
        const uchar* data = image.ptr(pt.y);
        uchar* mdata = mask.ptr(pt.y);
        for (pt.x = 0; pt.x < width; pt.x++)
        {
            if (data[pt.x])
            {
                mdata[pt.x] = (uchar)1;
                nzloc.push_back(pt);
            }
            else
                mdata[pt.x] = 0;
        }
    }

    int count = (int)nzloc.size();

    // 随机处理，像素点
    for (; count > 0; count--)
    {
        // 随机抽取一个像素点
        int idx = rng.uniform(0, count);
        int max_val = threshold - 1, max_n = 0;
        cv::Point point = nzloc[idx];
        cv::Point line_end[2];
        float a, b;
        int* adata = accum.ptr<int>();
        int i = point.y, j = point.x, k, x0, y0, dx0, dy0, xflag;
        int good_line;
        const int shift = 16;

        // 随机像素点位置被覆盖
        nzloc[idx] = nzloc[count - 1];

        //随机的像素点是0
        if (!mdata0[i * width + j])
            continue;

        // 累加计数
        for (int n = 0; n < numangle; n++, adata += numrho)
        {
            //r=x*cos(theta)+y*sin(theta)
            int r = cvRound(j * ttab[n * 2] + i * ttab[n * 2 + 1]);
            r += (numrho - 1) / 2;
            //r值++
            int val = ++adata[r];
            //找到次数和对应的angle值
            if (max_val < val)
            {
                max_val = val;
                max_n = n;
            }
        }

        // 最大的次数值小于阈值
        if (max_val < threshold)
            continue;

        //找到空间内对应的次数最多的
        a = -ttab[max_n * 2 + 1];//sin(0-180 +),
        b = ttab[max_n * 2];//cos(0-90 +)

        x0 = j;
        y0 = i;

       
        if (fabs(a) > fabs(b))
        {
            xflag = 1;
            dx0 = a > 0 ? 1 : -1;
            dy0 = cvRound(b * (1 << shift) / fabs(a));
            y0 = (y0 << shift) + (1 << (shift - 1));
        }
        else
        {
            xflag = 0;
            dy0 = b > 0 ? 1 : -1;
            dx0 = cvRound(a * (1 << shift) / fabs(b));
            x0 = (x0 << shift) + (1 << (shift - 1));
        }
        //
        for (k = 0; k < 2; k++)
        {
            int gap = 0, x = x0, y = y0, dx = dx0, dy = dy0;

            if (k > 0)
                dx = -dx, dy = -dy;

            //像素走查
            for (;; x += dx, y += dy)
            {
                uchar* mdata;
                int i1, j1;

                if (xflag)
                {
                    j1 = x;
                    i1 = y >> shift;
                }
                else
                {
                    j1 = x >> shift;
                    i1 = y;
                }
                //像素点位置超过阈值
                if (j1 < 0 || j1 >= width || i1 < 0 || i1 >= height)
                    break;

                mdata = mdata0 + i1 * width + j1;

                //如果像素点不为0，记录像素点位置
                if (*mdata)
                {
                    gap = 0;
                    line_end[k].y = i1;
                    line_end[k].x = j1;
                }//查找到的像素点如果是黑点（background），那么记录该间隙，间隙超过阈值，跳出
                else if (++gap > lineGap)
                    break;
            }
        }

        //线段长度达到，
        good_line = std::abs(line_end[1].x - line_end[0].x) >= lineLength ||
            std::abs(line_end[1].y - line_end[0].y) >= lineLength;
        
        //像素回归
        for (k = 0; k < 2; k++)
        {
            int x = x0, y = y0, dx = dx0, dy = dy0;

            if (k > 0)
                dx = -dx, dy = -dy;

            for (;; x += dx, y += dy)
            {
                uchar* mdata;
                int i1, j1;

                if (xflag)
                {
                    j1 = x;
                    i1 = y >> shift;
                }
                else
                {
                    j1 = x >> shift;
                    i1 = y;
                }
                //取第i1行j1列的像素值
                mdata = mdata0 + i1 * width + j1;

                if (*mdata)
                {
                    if (good_line)
                    {
                        adata = accum.ptr<int>();
                        //计数回归
                        for (int n = 0; n < numangle; n++, adata += numrho)
                        {
                            int r = cvRound(j1 * ttab[n * 2] + i1 * ttab[n * 2 + 1]);
                            r += (numrho - 1) / 2;
                            adata[r]--;
                        }
                    }
                    *mdata = 0;
                }
                //位置回归
                if (i1 == line_end[k].y && j1 == line_end[k].x)
                    break;
            }
        }
        //添加线条
        if (good_line)
        {   //line_start-->line_end
            cv::Vec4i lr(line_end[0].x, line_end[0].y, line_end[1].x, line_end[1].y);
            lines.push_back(lr);
            if ((int)lines.size() >= linesMax)
                return;
        }
    }
}

 /// <summary>
 /// 霍夫直线检测（standard)
 /// </summary>
 /// <param name="_image">输入</param>
 /// <param name="lines">输出</param>
 /// <param name="rho">1</param>
 /// <param name="theta">pi/180</param>
 /// <param name="threshold">极坐标交点次数阈值</param>
 /// <param name="min_theta">角度扫描范围的起始角度</param>
 /// <param name="max_theta">pi 角度扫描范围的结束角度</param>
void Hou::HoughLines(cv::InputArray _image, cv::OutputArray lines,
    double rho,
    double theta,
    int threshold,
    double min_theta,
    double max_theta)
{

    int type = CV_32FC2;
    if (lines.fixedType())
    {
        type = lines.type();
        CV_CheckType(type, type == CV_32FC2 || type == CV_32FC3, "Wrong type of output lines");
    }
    HoughLinesStandard(_image, lines, type, (float)rho, (float)theta, threshold, INT_MAX, min_theta, max_theta);
}

 /// <summary>
 /// 线段检测
 /// </summary>
 /// <param name="_image">输入</param>
 /// <param name="_lines">输出</param>
 /// <param name="rho">r</param>
 /// <param name="theta">pi/180</param>
 /// <param name="threshold">极坐标交点次数阈值</param>
 /// <param name="minLineLength">所检测的最短直线长度</param>
 /// <param name="maxGap">连接的最大间隙</param>
void Hou::HoughLinesP(cv::InputArray _image, cv::OutputArray _lines,
    double rho,
    double theta,
    int threshold,
    double minLineLength,
    double maxGap)
{
    cv::Mat image = _image.getMat();
    std::vector<cv::Vec4i> lines;
    HoughLinesProbabilistic(image, (float)rho, (float)theta, threshold, cvRound(minLineLength), cvRound(maxGap), lines, INT_MAX);
    cv::Mat(lines).copyTo(_lines);
}