• 工程（十四）——ubuntu20.04 PL-VINS

    博主创建了一个科研互助群Q：772356582，欢迎大家加入讨论。这是一个科研互助群，主要围绕机器人，无人驾驶，无人机方面的感知定位，决策规划，以及论文发表经验，以方便大家很好很快的科研，少走弯路。欢迎在群里积极提问与回答，相互交流共同学习。

    一、简介

    PL-VINS是基于最先进的基于点的VINS- mono，开发的一种基于点和线特征的实时、高效优化的单目VINS方法。原始的 PL-VINS 是在ubuntu18.04基于opencv3去开发的。源码的下载地址：https://github.com/cnqiangfu/PL-VINS

    我的配置：ubuntu20.04+opencv4.2+eigen3.3.7

    更改好的代码如下，可直接用，需要修改mage_node_b.cpp的main函数第一行的地址

    huashu996/Ubuntu20.04PL_VINS · GitHub

    二、编译

    如果你使用官方的代码，编译时候可能会遇到如下问题

    1. mkdir -p ~/catkin_plvins/src 
    2. cd catkin_plvins/src  //进入创建的catkin_plvins/src文件夹下
    3. catkin_init_workspace    进行空间创建
    4.  
    5. cd ~/catkin_plvins    //在文件夹catkin_plvins下建立终端输入
    6. catkin_make  //终端输入
    7. source devel/setup.bash
    8. echo $ROS_PACKAGE_PATH
    9. 		
    10. //将代码下载到src目录下 或者执行下面代码
    11. cd ~/catkin_plvins/src
    12. git clone https://github.com/cnqiangfu/PL-VINS.git
    13. //编译
    14. cd ..  //回到文件夹catkin_plvins
    15. catkin_make  
    16. source devel/setup.bash
    • 将feature_tracker里面的Cmakelist修改一下：主要修改以下2处的路径

    • 打开catkin_plvins/src/PL-VINS/image_node_b文件下的CMakeLists.txt添加
    set(CMAKE_CXX_STANDARD 14)

    如果你配置正确，应该能够编译顺利通过

    三、opencv4适配

    但运行往往会出现如下图问题，对于每个问题我们一次解决

    • [image_node_b-7]  挂掉的原因是linefeature_tracker_node中只发布了归一化坐标，没有发布linefeature的startpoint和endpoint的像素坐标。

    1.mage_node_b.cpp的main函数第一行的地址，修改原绝对地址。

     2.加入project函数

    1. void project(cv::Point2f&  pt, cv::Mat  const&  k)
    2. {
    3.     pt.x=k.at<float>(0,0)*pt.x+k.at<float>(0,2);
    4.     pt.y=k.at<float>(1,1)*pt.y+k.at<float>(1,2);
    5. }

    3.替换 

    1. 将下面两行        
    2. cv::Point startPoint = cv::Point(line_feature_msg->channels[3].values[i], line_feature_msg->channels[4].values[i]);
    3. cv::Point endPoint = cv::Point(line_feature_msg->channels[5].values[i], line_feature_msg->channels[6].values[i]);
    4. 替换
    5. cv::Point2f  startPoint = cv::Point2f(line_feature_msg->points[i].x,line_feature_msg->points[i].y );
    6.         project (startPoint,K_);
    7. cv::Point2f  endPoint = cv::Point2f(line_feature_msg->channels[1].values[i],line_feature_msg->channels[2].values[i]);
    • [linefeature_tracker-4] 

    此问题由于opencv和cv_bridge冲突的问题，因为在ubuntu20.04 cv_bridge是4，如果你使用opencv3那么将产生冲突，如果使用opencv4跑，代码本身又不支持opencv4所以需要更改。相应的解决方式也有两种：

    1）把vins-mono代码全部改成opencv4的版本 或者  2）把cv_bridge改成opencv3版本。参考如下博客修改

    【精选】Ubuntu20.04下成功运行VINS-mono_ubuntu vinsmono-CSDN博客

    1.将所有包含opencv的Cmakelists.txt中opencv引入都换成4，如下

    2.更新头文件使使用opencv4

    1. (1)将camera_model包改成兼容opencv4
    2.  
    3. 在camera_model包中的头文件Chessboard.h中添加
    4. #include 
    5. #include 
    6. 在CameraCalibration.h中添加
    7. #include 
    8. #include 
    9.  
    10. (2)将包中所有报错的头文件
    11. #include 
    12. #include 
    13. 替换为
    14. #include 
    15. #include

    3.加入线特征检测函数

    imgproc.hpp关于线特征的部分由于opencv4权限的问题无法使用了，这里只需要自己定义一个相同功能的头文件去把opencv3中的实现拷贝过去。在路径

    PL-VINS/src/PL-VINS/feature_tracker/src/line_descriptor/src

    创建一个my_lsd.hpp

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

     在LSDDetector_custom.cpp文件中引用头文件

    #include "my_lsd.hpp"

    再次编译运行

    四、运行

    1. source devel/setup.bash
    2. roslaunch plvins_estimator plvins_show_linepoint.launch
    3. rosbag play MH_05_difficult.bag

    注意：我们此时需要需要将src/PL-VINSvins_estimator/launch/下的plvins-show-linepoint.launch改为plvins_show_linepoint.launch（注意是下划线）文件名

    数据集下载地址如下：

    kmavvisualinertialdatasets – ASL Datasets

    博主github仓库中的代码全部都修改完毕，适用ubuntu20.04 opencv4

  • 相关阅读:
    HTTP图解基础知识
    c# winform 实际操作XML代码，包括创建、保存、查询、删除窗体演示
    正则表达式
    Pycharm 远程连接服务器（ssh）运行深度学习代码 | 详细步骤
    Web3 的通行证——DID 带来数字身份革命
    【Hello Algorithm】暴力递归到动态规划（一）
    controller接收List入参
    Apollo配置中心-手把手教你搭建Apollo配置中心运行环境
    【logrotate】linux定时文件切割（解决openresty单个日志文件过大问题）
    关于打印输出
  • 原文地址：https://blog.csdn.net/HUASHUDEYANJING/article/details/134254818