1/*M///////////////////////////////////////////////////////////////////////////////////////
2//
3//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4//
5//  By downloading, copying, installing or using the software you agree to this license.
6//  If you do not agree to this license, do not download, install,
7//  copy or use the software.
8//
9//
10//                           License Agreement
11//                For Open Source Computer Vision Library
12//
13// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
14// Third party copyrights are property of their respective owners.
15//
16// Redistribution and use in source and binary forms, with or without modification,
17// are permitted provided that the following conditions are met:
18//
19//   * Redistributions of source code must retain the above copyright notice,
20//     this list of conditions and the following disclaimer.
21//
22//   * Redistributions in binary form must reproduce the above copyright notice,
23//     this list of conditions and the following disclaimer in the documentation
24//     and/or other materials provided with the distribution.
25//
26//   * The name of the copyright holders may not be used to endorse or promote products
27//     derived from this software without specific prior written permission.
28//
29// This software is provided by the copyright holders and contributors "as is" and
30// any express or implied warranties, including, but not limited to, the implied
31// warranties of merchantability and fitness for a particular purpose are disclaimed.
32// In no event shall the Intel Corporation or contributors be liable for any direct,
33// indirect, incidental, special, exemplary, or consequential damages
34// (including, but not limited to, procurement of substitute goods or services;
35// loss of use, data, or profits; or business interruption) however caused
36// and on any theory of liability, whether in contract, strict liability,
37// or tort (including negligence or otherwise) arising in any way out of
38// the use of this software, even if advised of the possibility of such damage.
39//
40//M*/
41
42#include "precomp.hpp"
43#include <vector>
44
45/////////////////////////////////////////////////////////////////////////////////////////
46// Default LSD parameters
47// SIGMA_SCALE 0.6    - Sigma for Gaussian filter is computed as sigma = sigma_scale/scale.
48// QUANT       2.0    - Bound to the quantization error on the gradient norm.
49// ANG_TH      22.5   - Gradient angle tolerance in degrees.
50// LOG_EPS     0.0    - Detection threshold: -log10(NFA) > log_eps
51// DENSITY_TH  0.7    - Minimal density of region points in rectangle.
52// N_BINS      1024   - Number of bins in pseudo-ordering of gradient modulus.
53
54#define M_3_2_PI    (3 * CV_PI) / 2   // 3/2 pi
55#define M_2__PI     (2 * CV_PI)         // 2 pi
56
57#ifndef M_LN10
58#define M_LN10      2.30258509299404568402
59#endif
60
61#define NOTDEF      double(-1024.0) // Label for pixels with undefined gradient.
62
63#define NOTUSED     0   // Label for pixels not used in yet.
64#define USED        1   // Label for pixels already used in detection.
65
66#define RELATIVE_ERROR_FACTOR 100.0
67
68const double DEG_TO_RADS = CV_PI / 180;
69
70#define log_gamma(x) ((x)>15.0?log_gamma_windschitl(x):log_gamma_lanczos(x))
71
72struct edge
73{
74    cv::Point p;
75    bool taken;
76};
77
78/////////////////////////////////////////////////////////////////////////////////////////
79
80inline double distSq(const double x1, const double y1,
81                     const double x2, const double y2)
82{
83    return (x2 - x1)*(x2 - x1) + (y2 - y1)*(y2 - y1);
84}
85
86inline double dist(const double x1, const double y1,
87                   const double x2, const double y2)
88{
89    return sqrt(distSq(x1, y1, x2, y2));
90}
91
92// Signed angle difference
93inline double angle_diff_signed(const double& a, const double& b)
94{
95    double diff = a - b;
96    while(diff <= -CV_PI) diff += M_2__PI;
97    while(diff >   CV_PI) diff -= M_2__PI;
98    return diff;
99}
100
101// Absolute value angle difference
102inline double angle_diff(const double& a, const double& b)
103{
104    return std::fabs(angle_diff_signed(a, b));
105}
106
107// Compare doubles by relative error.
108inline bool double_equal(const double& a, const double& b)
109{
110    // trivial case
111    if(a == b) return true;
112
113    double abs_diff = fabs(a - b);
114    double aa = fabs(a);
115    double bb = fabs(b);
116    double abs_max = (aa > bb)? aa : bb;
117
118    if(abs_max < DBL_MIN) abs_max = DBL_MIN;
119
120    return (abs_diff / abs_max) <= (RELATIVE_ERROR_FACTOR * DBL_EPSILON);
121}
122
123inline bool AsmallerB_XoverY(const edge& a, const edge& b)
124{
125    if (a.p.x == b.p.x) return a.p.y < b.p.y;
126    else return a.p.x < b.p.x;
127}
128
129/**
130 *   Computes the natural logarithm of the absolute value of
131 *   the gamma function of x using Windschitl method.
132 *   See http://www.rskey.org/gamma.htm
133 */
134inline double log_gamma_windschitl(const double& x)
135{
136    return 0.918938533204673 + (x-0.5)*log(x) - x
137         + 0.5*x*log(x*sinh(1/x) + 1/(810.0*pow(x, 6.0)));
138}
139
140/**
141 *   Computes the natural logarithm of the absolute value of
142 *   the gamma function of x using the Lanczos approximation.
143 *   See http://www.rskey.org/gamma.htm
144 */
145inline double log_gamma_lanczos(const double& x)
146{
147    static double q[7] = { 75122.6331530, 80916.6278952, 36308.2951477,
148                         8687.24529705, 1168.92649479, 83.8676043424,
149                         2.50662827511 };
150    double a = (x + 0.5) * log(x + 5.5) - (x + 5.5);
151    double b = 0;
152    for(int n = 0; n < 7; ++n)
153    {
154        a -= log(x + double(n));
155        b += q[n] * pow(x, double(n));
156    }
157    return a + log(b);
158}
159///////////////////////////////////////////////////////////////////////////////////////////////////////////////
160
161namespace cv{
162
163class LineSegmentDetectorImpl : public LineSegmentDetector
164{
165public:
166
167/**
168 * Create a LineSegmentDetectorImpl object. Specifying scale, number of subdivisions for the image, should the lines be refined and other constants as follows:
169 *
170 * @param _refine       How should the lines found be refined?
171 *                      LSD_REFINE_NONE - No refinement applied.
172 *                      LSD_REFINE_STD  - Standard refinement is applied. E.g. breaking arches into smaller line approximations.
173 *                      LSD_REFINE_ADV  - Advanced refinement. Number of false alarms is calculated,
174 *                                    lines are refined through increase of precision, decrement in size, etc.
175 * @param _scale        The scale of the image that will be used to find the lines. Range (0..1].
176 * @param _sigma_scale  Sigma for Gaussian filter is computed as sigma = _sigma_scale/_scale.
177 * @param _quant        Bound to the quantization error on the gradient norm.
178 * @param _ang_th       Gradient angle tolerance in degrees.
179 * @param _log_eps      Detection threshold: -log10(NFA) > _log_eps
180 * @param _density_th   Minimal density of aligned region points in rectangle.
181 * @param _n_bins       Number of bins in pseudo-ordering of gradient modulus.
182 */
183    LineSegmentDetectorImpl(int _refine = LSD_REFINE_STD, double _scale = 0.8,
184        double _sigma_scale = 0.6, double _quant = 2.0, double _ang_th = 22.5,
185        double _log_eps = 0, double _density_th = 0.7, int _n_bins = 1024);
186
187/**
188 * Detect lines in the input image.
189 *
190 * @param _image    A grayscale(CV_8UC1) input image.
191 *                  If only a roi needs to be selected, use
192 *                  lsd_ptr->detect(image(roi), ..., lines);
193 *                  lines += Scalar(roi.x, roi.y, roi.x, roi.y);
194 * @param _lines    Return: A vector of Vec4i or Vec4f elements specifying the beginning and ending point of a line.
195 *                          Where Vec4i/Vec4f is (x1, y1, x2, y2), point 1 is the start, point 2 - end.
196 *                          Returned lines are strictly oriented depending on the gradient.
197 * @param width     Return: Vector of widths of the regions, where the lines are found. E.g. Width of line.
198 * @param prec      Return: Vector of precisions with which the lines are found.
199 * @param nfa       Return: Vector containing number of false alarms in the line region, with precision of 10%.
200 *                          The bigger the value, logarithmically better the detection.
201 *                              * -1 corresponds to 10 mean false alarms
202 *                              * 0 corresponds to 1 mean false alarm
203 *                              * 1 corresponds to 0.1 mean false alarms
204 *                          This vector will be calculated _only_ when the objects type is REFINE_ADV
205 */
206    void detect(InputArray _image, OutputArray _lines,
207                OutputArray width = noArray(), OutputArray prec = noArray(),
208                OutputArray nfa = noArray());
209
210/**
211 * Draw lines on the given canvas.
212 *
213 * @param image     The image, where lines will be drawn.
214 *                  Should have the size of the image, where the lines were found
215 * @param lines     The lines that need to be drawn
216 */
217    void drawSegments(InputOutputArray _image, InputArray lines);
218
219/**
220 * Draw both vectors on the image canvas. Uses blue for lines 1 and red for lines 2.
221 *
222 * @param size      The size of the image, where lines1 and lines2 were found.
223 * @param lines1    The first lines that need to be drawn. Color - Blue.
224 * @param lines2    The second lines that need to be drawn. Color - Red.
225 * @param image     An optional image, where lines will be drawn.
226 *                  Should have the size of the image, where the lines were found
227 * @return          The number of mismatching pixels between lines1 and lines2.
228 */
229    int compareSegments(const Size& size, InputArray lines1, InputArray lines2, InputOutputArray _image = noArray());
230
231private:
232    Mat image;
233    Mat_<double> scaled_image;
234    double *scaled_image_data;
235    Mat_<double> angles;     // in rads
236    double *angles_data;
237    Mat_<double> modgrad;
238    double *modgrad_data;
239    Mat_<uchar> used;
240
241    int img_width;
242    int img_height;
243    double LOG_NT;
244
245    bool w_needed;
246    bool p_needed;
247    bool n_needed;
248
249    const double SCALE;
250    const int doRefine;
251    const double SIGMA_SCALE;
252    const double QUANT;
253    const double ANG_TH;
254    const double LOG_EPS;
255    const double DENSITY_TH;
256    const int N_BINS;
257
258    struct RegionPoint {
259        int x;
260        int y;
261        uchar* used;
262        double angle;
263        double modgrad;
264    };
265
266
267    struct coorlist
268    {
269        Point2i p;
270        struct coorlist* next;
271    };
272
273    struct rect
274    {
275        double x1, y1, x2, y2;    // first and second point of the line segment
276        double width;             // rectangle width
277        double x, y;              // center of the rectangle
278        double theta;             // angle
279        double dx,dy;             // (dx,dy) is vector oriented as the line segment
280        double prec;              // tolerance angle
281        double p;                 // probability of a point with angle within 'prec'
282    };
283
284    LineSegmentDetectorImpl& operator= (const LineSegmentDetectorImpl&); // to quiet MSVC
285
286/**
287 * Detect lines in the whole input image.
288 *
289 * @param lines         Return: A vector of Vec4f elements specifying the beginning and ending point of a line.
290 *                              Where Vec4f is (x1, y1, x2, y2), point 1 is the start, point 2 - end.
291 *                              Returned lines are strictly oriented depending on the gradient.
292 * @param widths        Return: Vector of widths of the regions, where the lines are found. E.g. Width of line.
293 * @param precisions    Return: Vector of precisions with which the lines are found.
294 * @param nfas          Return: Vector containing number of false alarms in the line region, with precision of 10%.
295 *                              The bigger the value, logarithmically better the detection.
296 *                                  * -1 corresponds to 10 mean false alarms
297 *                                  * 0 corresponds to 1 mean false alarm
298 *                                  * 1 corresponds to 0.1 mean false alarms
299 */
300    void flsd(std::vector<Vec4f>& lines,
301              std::vector<double>& widths, std::vector<double>& precisions,
302              std::vector<double>& nfas);
303
304/**
305 * Finds the angles and the gradients of the image. Generates a list of pseudo ordered points.
306 *
307 * @param threshold The minimum value of the angle that is considered defined, otherwise NOTDEF
308 * @param n_bins    The number of bins with which gradients are ordered by, using bucket sort.
309 * @param list      Return: Vector of coordinate points that are pseudo ordered by magnitude.
310 *                  Pixels would be ordered by norm value, up to a precision given by max_grad/n_bins.
311 */
312    void ll_angle(const double& threshold, const unsigned int& n_bins, std::vector<coorlist>& list);
313
314/**
315 * Grow a region starting from point s with a defined precision,
316 * returning the containing points size and the angle of the gradients.
317 *
318 * @param s         Starting point for the region.
319 * @param reg       Return: Vector of points, that are part of the region
320 * @param reg_size  Return: The size of the region.
321 * @param reg_angle Return: The mean angle of the region.
322 * @param prec      The precision by which each region angle should be aligned to the mean.
323 */
324    void region_grow(const Point2i& s, std::vector<RegionPoint>& reg,
325                     int& reg_size, double& reg_angle, const double& prec);
326
327/**
328 * Finds the bounding rotated rectangle of a region.
329 *
330 * @param reg       The region of points, from which the rectangle to be constructed from.
331 * @param reg_size  The number of points in the region.
332 * @param reg_angle The mean angle of the region.
333 * @param prec      The precision by which points were found.
334 * @param p         Probability of a point with angle within 'prec'.
335 * @param rec       Return: The generated rectangle.
336 */
337    void region2rect(const std::vector<RegionPoint>& reg, const int reg_size, const double reg_angle,
338                     const double prec, const double p, rect& rec) const;
339
340/**
341 * Compute region's angle as the principal inertia axis of the region.
342 * @return          Regions angle.
343 */
344    double get_theta(const std::vector<RegionPoint>& reg, const int& reg_size, const double& x,
345                     const double& y, const double& reg_angle, const double& prec) const;
346
347/**
348 * An estimation of the angle tolerance is performed by the standard deviation of the angle at points
349 * near the region's starting point. Then, a new region is grown starting from the same point, but using the
350 * estimated angle tolerance. If this fails to produce a rectangle with the right density of region points,
351 * 'reduce_region_radius' is called to try to satisfy this condition.
352 */
353    bool refine(std::vector<RegionPoint>& reg, int& reg_size, double reg_angle,
354                const double prec, double p, rect& rec, const double& density_th);
355
356/**
357 * Reduce the region size, by elimination the points far from the starting point, until that leads to
358 * rectangle with the right density of region points or to discard the region if too small.
359 */
360    bool reduce_region_radius(std::vector<RegionPoint>& reg, int& reg_size, double reg_angle,
361                const double prec, double p, rect& rec, double density, const double& density_th);
362
363/**
364 * Try some rectangles variations to improve NFA value. Only if the rectangle is not meaningful (i.e., log_nfa <= log_eps).
365 * @return      The new NFA value.
366 */
367    double rect_improve(rect& rec) const;
368
369/**
370 * Calculates the number of correctly aligned points within the rectangle.
371 * @return      The new NFA value.
372 */
373    double rect_nfa(const rect& rec) const;
374
375/**
376 * Computes the NFA values based on the total number of points, points that agree.
377 * n, k, p are the binomial parameters.
378 * @return      The new NFA value.
379 */
380    double nfa(const int& n, const int& k, const double& p) const;
381
382/**
383 * Is the point at place 'address' aligned to angle theta, up to precision 'prec'?
384 * @return      Whether the point is aligned.
385 */
386    bool isAligned(const int& address, const double& theta, const double& prec) const;
387};
388
389/////////////////////////////////////////////////////////////////////////////////////////
390
391CV_EXPORTS Ptr<LineSegmentDetector> createLineSegmentDetector(
392        int _refine, double _scale, double _sigma_scale, double _quant, double _ang_th,
393        double _log_eps, double _density_th, int _n_bins)
394{
395    return makePtr<LineSegmentDetectorImpl>(
396            _refine, _scale, _sigma_scale, _quant, _ang_th,
397            _log_eps, _density_th, _n_bins);
398}
399
400/////////////////////////////////////////////////////////////////////////////////////////
401
402LineSegmentDetectorImpl::LineSegmentDetectorImpl(int _refine, double _scale, double _sigma_scale, double _quant,
403        double _ang_th, double _log_eps, double _density_th, int _n_bins)
404        :SCALE(_scale), doRefine(_refine), SIGMA_SCALE(_sigma_scale), QUANT(_quant),
405        ANG_TH(_ang_th), LOG_EPS(_log_eps), DENSITY_TH(_density_th), N_BINS(_n_bins)
406{
407    CV_Assert(_scale > 0 && _sigma_scale > 0 && _quant >= 0 &&
408              _ang_th > 0 && _ang_th < 180 && _density_th >= 0 && _density_th < 1 &&
409              _n_bins > 0);
410}
411
412void LineSegmentDetectorImpl::detect(InputArray _image, OutputArray _lines,
413                OutputArray _width, OutputArray _prec, OutputArray _nfa)
414{
415    Mat_<double> img = _image.getMat();
416    CV_Assert(!img.empty() && img.channels() == 1);
417
418    // Convert image to double
419    img.convertTo(image, CV_64FC1);
420
421    std::vector<Vec4f> lines;
422    std::vector<double> w, p, n;
423    w_needed = _width.needed();
424    p_needed = _prec.needed();
425    if (doRefine < LSD_REFINE_ADV)
426        n_needed = false;
427    else
428        n_needed = _nfa.needed();
429
430    flsd(lines, w, p, n);
431
432    Mat(lines).copyTo(_lines);
433    if(w_needed) Mat(w).copyTo(_width);
434    if(p_needed) Mat(p).copyTo(_prec);
435    if(n_needed) Mat(n).copyTo(_nfa);
436}
437
438void LineSegmentDetectorImpl::flsd(std::vector<Vec4f>& lines,
439    std::vector<double>& widths, std::vector<double>& precisions,
440    std::vector<double>& nfas)
441{
442    // Angle tolerance
443    const double prec = CV_PI * ANG_TH / 180;
444    const double p = ANG_TH / 180;
445    const double rho = QUANT / sin(prec);    // gradient magnitude threshold
446
447    std::vector<coorlist> list;
448    if(SCALE != 1)
449    {
450        Mat gaussian_img;
451        const double sigma = (SCALE < 1)?(SIGMA_SCALE / SCALE):(SIGMA_SCALE);
452        const double sprec = 3;
453        const unsigned int h =  (unsigned int)(ceil(sigma * sqrt(2 * sprec * log(10.0))));
454        Size ksize(1 + 2 * h, 1 + 2 * h); // kernel size
455        GaussianBlur(image, gaussian_img, ksize, sigma);
456        // Scale image to needed size
457        resize(gaussian_img, scaled_image, Size(), SCALE, SCALE);
458        ll_angle(rho, N_BINS, list);
459    }
460    else
461    {
462        scaled_image = image;
463        ll_angle(rho, N_BINS, list);
464    }
465
466    LOG_NT = 5 * (log10(double(img_width)) + log10(double(img_height))) / 2 + log10(11.0);
467    const int min_reg_size = int(-LOG_NT/log10(p)); // minimal number of points in region that can give a meaningful event
468
469    // // Initialize region only when needed
470    // Mat region = Mat::zeros(scaled_image.size(), CV_8UC1);
471    used = Mat_<uchar>::zeros(scaled_image.size()); // zeros = NOTUSED
472    std::vector<RegionPoint> reg(img_width * img_height);
473
474    // Search for line segments
475    unsigned int ls_count = 0;
476    for(size_t i = 0, list_size = list.size(); i < list_size; ++i)
477    {
478        unsigned int adx = list[i].p.x + list[i].p.y * img_width;
479        if((used.ptr()[adx] == NOTUSED) && (angles_data[adx] != NOTDEF))
480        {
481            int reg_size;
482            double reg_angle;
483            region_grow(list[i].p, reg, reg_size, reg_angle, prec);
484
485            // Ignore small regions
486            if(reg_size < min_reg_size) { continue; }
487
488            // Construct rectangular approximation for the region
489            rect rec;
490            region2rect(reg, reg_size, reg_angle, prec, p, rec);
491
492            double log_nfa = -1;
493            if(doRefine > LSD_REFINE_NONE)
494            {
495                // At least REFINE_STANDARD lvl.
496                if(!refine(reg, reg_size, reg_angle, prec, p, rec, DENSITY_TH)) { continue; }
497
498                if(doRefine >= LSD_REFINE_ADV)
499                {
500                    // Compute NFA
501                    log_nfa = rect_improve(rec);
502                    if(log_nfa <= LOG_EPS) { continue; }
503                }
504            }
505            // Found new line
506            ++ls_count;
507
508            // Add the offset
509            rec.x1 += 0.5; rec.y1 += 0.5;
510            rec.x2 += 0.5; rec.y2 += 0.5;
511
512            // scale the result values if a sub-sampling was performed
513            if(SCALE != 1)
514            {
515                rec.x1 /= SCALE; rec.y1 /= SCALE;
516                rec.x2 /= SCALE; rec.y2 /= SCALE;
517                rec.width /= SCALE;
518            }
519
520            //Store the relevant data
521            lines.push_back(Vec4f(float(rec.x1), float(rec.y1), float(rec.x2), float(rec.y2)));
522            if(w_needed) widths.push_back(rec.width);
523            if(p_needed) precisions.push_back(rec.p);
524            if(n_needed && doRefine >= LSD_REFINE_ADV) nfas.push_back(log_nfa);
525
526
527            // //Add the linesID to the region on the image
528            // for(unsigned int el = 0; el < reg_size; el++)
529            // {
530            //     region.data[reg[i].x + reg[i].y * width] = ls_count;
531            // }
532        }
533    }
534}
535
536void LineSegmentDetectorImpl::ll_angle(const double& threshold,
537                                   const unsigned int& n_bins,
538                                   std::vector<coorlist>& list)
539{
540    //Initialize data
541    angles = Mat_<double>(scaled_image.size());
542    modgrad = Mat_<double>(scaled_image.size());
543
544    angles_data = angles.ptr<double>(0);
545    modgrad_data = modgrad.ptr<double>(0);
546    scaled_image_data = scaled_image.ptr<double>(0);
547
548    img_width = scaled_image.cols;
549    img_height = scaled_image.rows;
550
551    // Undefined the down and right boundaries
552    angles.row(img_height - 1).setTo(NOTDEF);
553    angles.col(img_width - 1).setTo(NOTDEF);
554
555    // Computing gradient for remaining pixels
556    CV_Assert(scaled_image.isContinuous() &&
557              modgrad.isContinuous() &&
558              angles.isContinuous());   // Accessing image data linearly
559
560    double max_grad = -1;
561    for(int y = 0; y < img_height - 1; ++y)
562    {
563        for(int addr = y * img_width, addr_end = addr + img_width - 1; addr < addr_end; ++addr)
564        {
565            double DA = scaled_image_data[addr + img_width + 1] - scaled_image_data[addr];
566            double BC = scaled_image_data[addr + 1] - scaled_image_data[addr + img_width];
567            double gx = DA + BC;    // gradient x component
568            double gy = DA - BC;    // gradient y component
569            double norm = std::sqrt((gx * gx + gy * gy) / 4); // gradient norm
570
571            modgrad_data[addr] = norm;    // store gradient
572
573            if (norm <= threshold)  // norm too small, gradient no defined
574            {
575                angles_data[addr] = NOTDEF;
576            }
577            else
578            {
579                angles_data[addr] = fastAtan2(float(gx), float(-gy)) * DEG_TO_RADS;  // gradient angle computation
580                if (norm > max_grad) { max_grad = norm; }
581            }
582
583        }
584    }
585
586    // Compute histogram of gradient values
587    list = std::vector<coorlist>(img_width * img_height);
588    std::vector<coorlist*> range_s(n_bins);
589    std::vector<coorlist*> range_e(n_bins);
590    unsigned int count = 0;
591    double bin_coef = (max_grad > 0) ? double(n_bins - 1) / max_grad : 0; // If all image is smooth, max_grad <= 0
592
593    for(int y = 0; y < img_height - 1; ++y)
594    {
595        const double* norm = modgrad_data + y * img_width;
596        for(int x = 0; x < img_width - 1; ++x, ++norm)
597        {
598            // Store the point in the right bin according to its norm
599            int i = int((*norm) * bin_coef);
600            if(!range_e[i])
601            {
602                range_e[i] = range_s[i] = &list[count];
603                ++count;
604            }
605            else
606            {
607                range_e[i]->next = &list[count];
608                range_e[i] = &list[count];
609                ++count;
610            }
611            range_e[i]->p = Point(x, y);
612            range_e[i]->next = 0;
613        }
614    }
615
616    // Sort
617    int idx = n_bins - 1;
618    for(;idx > 0 && !range_s[idx]; --idx);
619    coorlist* start = range_s[idx];
620    coorlist* end = range_e[idx];
621    if(start)
622    {
623        while(idx > 0)
624        {
625            --idx;
626            if(range_s[idx])
627            {
628                end->next = range_s[idx];
629                end = range_e[idx];
630            }
631        }
632    }
633}
634
635void LineSegmentDetectorImpl::region_grow(const Point2i& s, std::vector<RegionPoint>& reg,
636                                      int& reg_size, double& reg_angle, const double& prec)
637{
638    // Point to this region
639    reg_size = 1;
640    reg[0].x = s.x;
641    reg[0].y = s.y;
642    int addr = s.x + s.y * img_width;
643    reg[0].used = used.ptr() + addr;
644    reg_angle = angles_data[addr];
645    reg[0].angle = reg_angle;
646    reg[0].modgrad = modgrad_data[addr];
647
648    float sumdx = float(std::cos(reg_angle));
649    float sumdy = float(std::sin(reg_angle));
650    *reg[0].used = USED;
651
652    //Try neighboring regions
653    for(int i = 0; i < reg_size; ++i)
654    {
655        const RegionPoint& rpoint = reg[i];
656        int xx_min = std::max(rpoint.x - 1, 0), xx_max = std::min(rpoint.x + 1, img_width - 1);
657        int yy_min = std::max(rpoint.y - 1, 0), yy_max = std::min(rpoint.y + 1, img_height - 1);
658        for(int yy = yy_min; yy <= yy_max; ++yy)
659        {
660            int c_addr = xx_min + yy * img_width;
661            for(int xx = xx_min; xx <= xx_max; ++xx, ++c_addr)
662            {
663                if((used.ptr()[c_addr] != USED) &&
664                   (isAligned(c_addr, reg_angle, prec)))
665                {
666                    // Add point
667                    used.ptr()[c_addr] = USED;
668                    RegionPoint& region_point = reg[reg_size];
669                    region_point.x = xx;
670                    region_point.y = yy;
671                    region_point.used = &(used.ptr()[c_addr]);
672                    region_point.modgrad = modgrad_data[c_addr];
673                    const double& angle = angles_data[c_addr];
674                    region_point.angle = angle;
675                    ++reg_size;
676
677                    // Update region's angle
678                    sumdx += cos(float(angle));
679                    sumdy += sin(float(angle));
680                    // reg_angle is used in the isAligned, so it needs to be updates?
681                    reg_angle = fastAtan2(sumdy, sumdx) * DEG_TO_RADS;
682                }
683            }
684        }
685    }
686}
687
688void LineSegmentDetectorImpl::region2rect(const std::vector<RegionPoint>& reg, const int reg_size,
689                                      const double reg_angle, const double prec, const double p, rect& rec) const
690{
691    double x = 0, y = 0, sum = 0;
692    for(int i = 0; i < reg_size; ++i)
693    {
694        const RegionPoint& pnt = reg[i];
695        const double& weight = pnt.modgrad;
696        x += double(pnt.x) * weight;
697        y += double(pnt.y) * weight;
698        sum += weight;
699    }
700
701    // Weighted sum must differ from 0
702    CV_Assert(sum > 0);
703
704    x /= sum;
705    y /= sum;
706
707    double theta = get_theta(reg, reg_size, x, y, reg_angle, prec);
708
709    // Find length and width
710    double dx = cos(theta);
711    double dy = sin(theta);
712    double l_min = 0, l_max = 0, w_min = 0, w_max = 0;
713
714    for(int i = 0; i < reg_size; ++i)
715    {
716        double regdx = double(reg[i].x) - x;
717        double regdy = double(reg[i].y) - y;
718
719        double l = regdx * dx + regdy * dy;
720        double w = -regdx * dy + regdy * dx;
721
722        if(l > l_max) l_max = l;
723        else if(l < l_min) l_min = l;
724        if(w > w_max) w_max = w;
725        else if(w < w_min) w_min = w;
726    }
727
728    // Store values
729    rec.x1 = x + l_min * dx;
730    rec.y1 = y + l_min * dy;
731    rec.x2 = x + l_max * dx;
732    rec.y2 = y + l_max * dy;
733    rec.width = w_max - w_min;
734    rec.x = x;
735    rec.y = y;
736    rec.theta = theta;
737    rec.dx = dx;
738    rec.dy = dy;
739    rec.prec = prec;
740    rec.p = p;
741
742    // Min width of 1 pixel
743    if(rec.width < 1.0) rec.width = 1.0;
744}
745
746double LineSegmentDetectorImpl::get_theta(const std::vector<RegionPoint>& reg, const int& reg_size, const double& x,
747                                      const double& y, const double& reg_angle, const double& prec) const
748{
749    double Ixx = 0.0;
750    double Iyy = 0.0;
751    double Ixy = 0.0;
752
753    // Compute inertia matrix
754    for(int i = 0; i < reg_size; ++i)
755    {
756        const double& regx = reg[i].x;
757        const double& regy = reg[i].y;
758        const double& weight = reg[i].modgrad;
759        double dx = regx - x;
760        double dy = regy - y;
761        Ixx += dy * dy * weight;
762        Iyy += dx * dx * weight;
763        Ixy -= dx * dy * weight;
764    }
765
766    // Check if inertia matrix is null
767    CV_Assert(!(double_equal(Ixx, 0) && double_equal(Iyy, 0) && double_equal(Ixy, 0)));
768
769    // Compute smallest eigenvalue
770    double lambda = 0.5 * (Ixx + Iyy - sqrt((Ixx - Iyy) * (Ixx - Iyy) + 4.0 * Ixy * Ixy));
771
772    // Compute angle
773    double theta = (fabs(Ixx)>fabs(Iyy))?
774                    double(fastAtan2(float(lambda - Ixx), float(Ixy))):
775                    double(fastAtan2(float(Ixy), float(lambda - Iyy))); // in degs
776    theta *= DEG_TO_RADS;
777
778    // Correct angle by 180 deg if necessary
779    if(angle_diff(theta, reg_angle) > prec) { theta += CV_PI; }
780
781    return theta;
782}
783
784bool LineSegmentDetectorImpl::refine(std::vector<RegionPoint>& reg, int& reg_size, double reg_angle,
785                                 const double prec, double p, rect& rec, const double& density_th)
786{
787    double density = double(reg_size) / (dist(rec.x1, rec.y1, rec.x2, rec.y2) * rec.width);
788
789    if (density >= density_th) { return true; }
790
791    // Try to reduce angle tolerance
792    double xc = double(reg[0].x);
793    double yc = double(reg[0].y);
794    const double& ang_c = reg[0].angle;
795    double sum = 0, s_sum = 0;
796    int n = 0;
797
798    for (int i = 0; i < reg_size; ++i)
799    {
800        *(reg[i].used) = NOTUSED;
801        if (dist(xc, yc, reg[i].x, reg[i].y) < rec.width)
802        {
803            const double& angle = reg[i].angle;
804            double ang_d = angle_diff_signed(angle, ang_c);
805            sum += ang_d;
806            s_sum += ang_d * ang_d;
807            ++n;
808        }
809    }
810    double mean_angle = sum / double(n);
811    // 2 * standard deviation
812    double tau = 2.0 * sqrt((s_sum - 2.0 * mean_angle * sum) / double(n) + mean_angle * mean_angle);
813
814    // Try new region
815    region_grow(Point(reg[0].x, reg[0].y), reg, reg_size, reg_angle, tau);
816
817    if (reg_size < 2) { return false; }
818
819    region2rect(reg, reg_size, reg_angle, prec, p, rec);
820    density = double(reg_size) / (dist(rec.x1, rec.y1, rec.x2, rec.y2) * rec.width);
821
822    if (density < density_th)
823    {
824        return reduce_region_radius(reg, reg_size, reg_angle, prec, p, rec, density, density_th);
825    }
826    else
827    {
828        return true;
829    }
830}
831
832bool LineSegmentDetectorImpl::reduce_region_radius(std::vector<RegionPoint>& reg, int& reg_size, double reg_angle,
833                const double prec, double p, rect& rec, double density, const double& density_th)
834{
835    // Compute region's radius
836    double xc = double(reg[0].x);
837    double yc = double(reg[0].y);
838    double radSq1 = distSq(xc, yc, rec.x1, rec.y1);
839    double radSq2 = distSq(xc, yc, rec.x2, rec.y2);
840    double radSq = radSq1 > radSq2 ? radSq1 : radSq2;
841
842    while(density < density_th)
843    {
844        radSq *= 0.75*0.75; // Reduce region's radius to 75% of its value
845        // Remove points from the region and update 'used' map
846        for(int i = 0; i < reg_size; ++i)
847        {
848            if(distSq(xc, yc, double(reg[i].x), double(reg[i].y)) > radSq)
849            {
850                // Remove point from the region
851                *(reg[i].used) = NOTUSED;
852                std::swap(reg[i], reg[reg_size - 1]);
853                --reg_size;
854                --i; // To avoid skipping one point
855            }
856        }
857
858        if(reg_size < 2) { return false; }
859
860        // Re-compute rectangle
861        region2rect(reg, reg_size ,reg_angle, prec, p, rec);
862
863        // Re-compute region points density
864        density = double(reg_size) /
865                  (dist(rec.x1, rec.y1, rec.x2, rec.y2) * rec.width);
866    }
867
868    return true;
869}
870
871double LineSegmentDetectorImpl::rect_improve(rect& rec) const
872{
873    double delta = 0.5;
874    double delta_2 = delta / 2.0;
875
876    double log_nfa = rect_nfa(rec);
877
878    if(log_nfa > LOG_EPS) return log_nfa; // Good rectangle
879
880    // Try to improve
881    // Finer precision
882    rect r = rect(rec); // Copy
883    for(int n = 0; n < 5; ++n)
884    {
885        r.p /= 2;
886        r.prec = r.p * CV_PI;
887        double log_nfa_new = rect_nfa(r);
888        if(log_nfa_new > log_nfa)
889        {
890            log_nfa = log_nfa_new;
891            rec = rect(r);
892        }
893    }
894    if(log_nfa > LOG_EPS) return log_nfa;
895
896    // Try to reduce width
897    r = rect(rec);
898    for(unsigned int n = 0; n < 5; ++n)
899    {
900        if((r.width - delta) >= 0.5)
901        {
902            r.width -= delta;
903            double log_nfa_new = rect_nfa(r);
904            if(log_nfa_new > log_nfa)
905            {
906                rec = rect(r);
907                log_nfa = log_nfa_new;
908            }
909        }
910    }
911    if(log_nfa > LOG_EPS) return log_nfa;
912
913    // Try to reduce one side of rectangle
914    r = rect(rec);
915    for(unsigned int n = 0; n < 5; ++n)
916    {
917        if((r.width - delta) >= 0.5)
918        {
919            r.x1 += -r.dy * delta_2;
920            r.y1 +=  r.dx * delta_2;
921            r.x2 += -r.dy * delta_2;
922            r.y2 +=  r.dx * delta_2;
923            r.width -= delta;
924            double log_nfa_new = rect_nfa(r);
925            if(log_nfa_new > log_nfa)
926            {
927                rec = rect(r);
928                log_nfa = log_nfa_new;
929            }
930        }
931    }
932    if(log_nfa > LOG_EPS) return log_nfa;
933
934    // Try to reduce other side of rectangle
935    r = rect(rec);
936    for(unsigned int n = 0; n < 5; ++n)
937    {
938        if((r.width - delta) >= 0.5)
939        {
940            r.x1 -= -r.dy * delta_2;
941            r.y1 -=  r.dx * delta_2;
942            r.x2 -= -r.dy * delta_2;
943            r.y2 -=  r.dx * delta_2;
944            r.width -= delta;
945            double log_nfa_new = rect_nfa(r);
946            if(log_nfa_new > log_nfa)
947            {
948                rec = rect(r);
949                log_nfa = log_nfa_new;
950            }
951        }
952    }
953    if(log_nfa > LOG_EPS) return log_nfa;
954
955    // Try finer precision
956    r = rect(rec);
957    for(unsigned int n = 0; n < 5; ++n)
958    {
959        if((r.width - delta) >= 0.5)
960        {
961            r.p /= 2;
962            r.prec = r.p * CV_PI;
963            double log_nfa_new = rect_nfa(r);
964            if(log_nfa_new > log_nfa)
965            {
966                rec = rect(r);
967                log_nfa = log_nfa_new;
968            }
969        }
970    }
971
972    return log_nfa;
973}
974
975double LineSegmentDetectorImpl::rect_nfa(const rect& rec) const
976{
977    int total_pts = 0, alg_pts = 0;
978    double half_width = rec.width / 2.0;
979    double dyhw = rec.dy * half_width;
980    double dxhw = rec.dx * half_width;
981
982    std::vector<edge> ordered_x(4);
983    edge* min_y = &ordered_x[0];
984    edge* max_y = &ordered_x[0]; // Will be used for loop range
985
986    ordered_x[0].p.x = int(rec.x1 - dyhw); ordered_x[0].p.y = int(rec.y1 + dxhw); ordered_x[0].taken = false;
987    ordered_x[1].p.x = int(rec.x2 - dyhw); ordered_x[1].p.y = int(rec.y2 + dxhw); ordered_x[1].taken = false;
988    ordered_x[2].p.x = int(rec.x2 + dyhw); ordered_x[2].p.y = int(rec.y2 - dxhw); ordered_x[2].taken = false;
989    ordered_x[3].p.x = int(rec.x1 + dyhw); ordered_x[3].p.y = int(rec.y1 - dxhw); ordered_x[3].taken = false;
990
991    std::sort(ordered_x.begin(), ordered_x.end(), AsmallerB_XoverY);
992
993    // Find min y. And mark as taken. find max y.
994    for(unsigned int i = 1; i < 4; ++i)
995    {
996        if(min_y->p.y > ordered_x[i].p.y) {min_y = &ordered_x[i]; }
997        if(max_y->p.y < ordered_x[i].p.y) {max_y = &ordered_x[i]; }
998    }
999    min_y->taken = true;
1000
1001    // Find leftmost untaken point;
1002    edge* leftmost = 0;
1003    for(unsigned int i = 0; i < 4; ++i)
1004    {
1005        if(!ordered_x[i].taken)
1006        {
1007            if(!leftmost) // if uninitialized
1008            {
1009                leftmost = &ordered_x[i];
1010            }
1011            else if (leftmost->p.x > ordered_x[i].p.x)
1012            {
1013                leftmost = &ordered_x[i];
1014            }
1015        }
1016    }
1017    leftmost->taken = true;
1018
1019    // Find rightmost untaken point;
1020    edge* rightmost = 0;
1021    for(unsigned int i = 0; i < 4; ++i)
1022    {
1023        if(!ordered_x[i].taken)
1024        {
1025            if(!rightmost) // if uninitialized
1026            {
1027                rightmost = &ordered_x[i];
1028            }
1029            else if (rightmost->p.x < ordered_x[i].p.x)
1030            {
1031                rightmost = &ordered_x[i];
1032            }
1033        }
1034    }
1035    rightmost->taken = true;
1036
1037    // Find last untaken point;
1038    edge* tailp = 0;
1039    for(unsigned int i = 0; i < 4; ++i)
1040    {
1041        if(!ordered_x[i].taken)
1042        {
1043            if(!tailp) // if uninitialized
1044            {
1045                tailp = &ordered_x[i];
1046            }
1047            else if (tailp->p.x > ordered_x[i].p.x)
1048            {
1049                tailp = &ordered_x[i];
1050            }
1051        }
1052    }
1053    tailp->taken = true;
1054
1055    double flstep = (min_y->p.y != leftmost->p.y) ?
1056                    (min_y->p.x - leftmost->p.x) / (min_y->p.y - leftmost->p.y) : 0; //first left step
1057    double slstep = (leftmost->p.y != tailp->p.x) ?
1058                    (leftmost->p.x - tailp->p.x) / (leftmost->p.y - tailp->p.x) : 0; //second left step
1059
1060    double frstep = (min_y->p.y != rightmost->p.y) ?
1061                    (min_y->p.x - rightmost->p.x) / (min_y->p.y - rightmost->p.y) : 0; //first right step
1062    double srstep = (rightmost->p.y != tailp->p.x) ?
1063                    (rightmost->p.x - tailp->p.x) / (rightmost->p.y - tailp->p.x) : 0; //second right step
1064
1065    double lstep = flstep, rstep = frstep;
1066
1067    double left_x = min_y->p.x, right_x = min_y->p.x;
1068
1069    // Loop around all points in the region and count those that are aligned.
1070    int min_iter = min_y->p.y;
1071    int max_iter = max_y->p.y;
1072    for(int y = min_iter; y <= max_iter; ++y)
1073    {
1074        if (y < 0 || y >= img_height) continue;
1075
1076        int adx = y * img_width + int(left_x);
1077        for(int x = int(left_x); x <= int(right_x); ++x, ++adx)
1078        {
1079            if (x < 0 || x >= img_width) continue;
1080
1081            ++total_pts;
1082            if(isAligned(adx, rec.theta, rec.prec))
1083            {
1084                ++alg_pts;
1085            }
1086        }
1087
1088        if(y >= leftmost->p.y) { lstep = slstep; }
1089        if(y >= rightmost->p.y) { rstep = srstep; }
1090
1091        left_x += lstep;
1092        right_x += rstep;
1093    }
1094
1095    return nfa(total_pts, alg_pts, rec.p);
1096}
1097
1098double LineSegmentDetectorImpl::nfa(const int& n, const int& k, const double& p) const
1099{
1100    // Trivial cases
1101    if(n == 0 || k == 0) { return -LOG_NT; }
1102    if(n == k) { return -LOG_NT - double(n) * log10(p); }
1103
1104    double p_term = p / (1 - p);
1105
1106    double log1term = (double(n) + 1) - log_gamma(double(k) + 1)
1107                - log_gamma(double(n-k) + 1)
1108                + double(k) * log(p) + double(n-k) * log(1.0 - p);
1109    double term = exp(log1term);
1110
1111    if(double_equal(term, 0))
1112    {
1113        if(k > n * p) return -log1term / M_LN10 - LOG_NT;
1114        else return -LOG_NT;
1115    }
1116
1117    // Compute more terms if needed
1118    double bin_tail = term;
1119    double tolerance = 0.1; // an error of 10% in the result is accepted
1120    for(int i = k + 1; i <= n; ++i)
1121    {
1122        double bin_term = double(n - i + 1) / double(i);
1123        double mult_term = bin_term * p_term;
1124        term *= mult_term;
1125        bin_tail += term;
1126        if(bin_term < 1)
1127        {
1128            double err = term * ((1 - pow(mult_term, double(n-i+1))) / (1 - mult_term) - 1);
1129            if(err < tolerance * fabs(-log10(bin_tail) - LOG_NT) * bin_tail) break;
1130        }
1131
1132    }
1133    return -log10(bin_tail) - LOG_NT;
1134}
1135
1136inline bool LineSegmentDetectorImpl::isAligned(const int& address, const double& theta, const double& prec) const
1137{
1138    if(address < 0) { return false; }
1139    const double& a = angles_data[address];
1140    if(a == NOTDEF) { return false; }
1141
1142    // It is assumed that 'theta' and 'a' are in the range [-pi,pi]
1143    double n_theta = theta - a;
1144    if(n_theta < 0) { n_theta = -n_theta; }
1145    if(n_theta > M_3_2_PI)
1146    {
1147        n_theta -= M_2__PI;
1148        if(n_theta < 0) n_theta = -n_theta;
1149    }
1150
1151    return n_theta <= prec;
1152}
1153
1154
1155void LineSegmentDetectorImpl::drawSegments(InputOutputArray _image, InputArray lines)
1156{
1157    CV_Assert(!_image.empty() && (_image.channels() == 1 || _image.channels() == 3));
1158
1159    Mat gray;
1160    if (_image.channels() == 1)
1161    {
1162        gray = _image.getMatRef();
1163    }
1164    else if (_image.channels() == 3)
1165    {
1166        cvtColor(_image, gray, CV_BGR2GRAY);
1167    }
1168
1169    // Create a 3 channel image in order to draw colored lines
1170    std::vector<Mat> planes;
1171    planes.push_back(gray);
1172    planes.push_back(gray);
1173    planes.push_back(gray);
1174
1175    merge(planes, _image);
1176
1177    Mat _lines;
1178    _lines = lines.getMat();
1179    int N = _lines.checkVector(4);
1180
1181    // Draw segments
1182    for(int i = 0; i < N; ++i)
1183    {
1184        const Vec4f& v = _lines.at<Vec4f>(i);
1185        Point2f b(v[0], v[1]);
1186        Point2f e(v[2], v[3]);
1187        line(_image.getMatRef(), b, e, Scalar(0, 0, 255), 1);
1188    }
1189}
1190
1191
1192int LineSegmentDetectorImpl::compareSegments(const Size& size, InputArray lines1, InputArray lines2, InputOutputArray _image)
1193{
1194    Size sz = size;
1195    if (_image.needed() && _image.size() != size) sz = _image.size();
1196    CV_Assert(sz.area());
1197
1198    Mat_<uchar> I1 = Mat_<uchar>::zeros(sz);
1199    Mat_<uchar> I2 = Mat_<uchar>::zeros(sz);
1200
1201    Mat _lines1;
1202    Mat _lines2;
1203    _lines1 = lines1.getMat();
1204    _lines2 = lines2.getMat();
1205    int N1 = _lines1.checkVector(4);
1206    int N2 = _lines2.checkVector(4);
1207
1208    // Draw segments
1209    for(int i = 0; i < N1; ++i)
1210    {
1211        Point2f b(_lines1.at<Vec4f>(i)[0], _lines1.at<Vec4f>(i)[1]);
1212        Point2f e(_lines1.at<Vec4f>(i)[2], _lines1.at<Vec4f>(i)[3]);
1213        line(I1, b, e, Scalar::all(255), 1);
1214    }
1215    for(int i = 0; i < N2; ++i)
1216    {
1217        Point2f b(_lines2.at<Vec4f>(i)[0], _lines2.at<Vec4f>(i)[1]);
1218        Point2f e(_lines2.at<Vec4f>(i)[2], _lines2.at<Vec4f>(i)[3]);
1219        line(I2, b, e, Scalar::all(255), 1);
1220    }
1221
1222    // Count the pixels that don't agree
1223    Mat Ixor;
1224    bitwise_xor(I1, I2, Ixor);
1225    int N = countNonZero(Ixor);
1226
1227    if (_image.needed())
1228    {
1229        CV_Assert(_image.channels() == 3);
1230        Mat img = _image.getMatRef();
1231        CV_Assert(img.isContinuous() && I1.isContinuous() && I2.isContinuous());
1232
1233        for (unsigned int i = 0; i < I1.total(); ++i)
1234        {
1235            uchar i1 = I1.ptr()[i];
1236            uchar i2 = I2.ptr()[i];
1237            if (i1 || i2)
1238            {
1239                unsigned int base_idx = i * 3;
1240                if (i1) img.ptr()[base_idx] = 255;
1241                else img.ptr()[base_idx] = 0;
1242                img.ptr()[base_idx + 1] = 0;
1243                if (i2) img.ptr()[base_idx + 2] = 255;
1244                else img.ptr()[base_idx + 2] = 0;
1245            }
1246        }
1247    }
1248
1249    return N;
1250}
1251
1252} // namespace cv
1253