有斜率限制地进行点云数据直线提取

需求：

       通过2D激光雷达提取机器人到右侧墙面的距离

使用场景：

         一台沿墙工作的机器人需要通过到右侧墙面的距离做横向控制

方案描述：

        对雷达数据做角度滤波，只提取机器人右侧的点云数据做拟合

方案不足：

        在离右墙不同距离下的点云角度范围不一样，角度滤波适应性不强

方案思考：

       问： 什么是不变的？

        答：角度在一定误差范围内是“不变”的

方案改进一：

        限制角度地拟合直线；

方案改进一实现：

        实现工具：

                pcl::ransac;  pcl::ExtractIndices;

        方案流程：

                1、角度滤波

                2、ransac提取

ransac.setDistanceThreshold(0.01);
ransac.setMaxIterations(1000);
ransac.setNumberOfThreads(50);
ransac.computeModel();

std::vector<int> inliers;
ransac.getInliers(inliers);
ransac.getModelCoefficients(coef);

                3、计算coef[3]/coef[4]，如果不是需要的角度范围则利用ExtractIndices将已经用于直线拟合的点去除

indices_.reset(new pcl::PointIndices());
indices_->indices = inliers;
extract_.setInputCloud(origin_cloud_);
extract_.setIndices(indices_);
extract_.setNegative(true);
extract_.filter(*origin_cloud_);

               4、剩下的点云数据进行递归再次进行拟合直到满足斜率或者点云不足以进行拟合，效果图如下（视角动了一下，别介意，上图黄色点云为提取的直线对应的是下图右上角的点云）

k= 1.0

k=-1.0

另外：蹲一个更好的解决方案

另外：蹲一个更好的解决方案

另外：蹲一个更好的解决方案

ps:经过点云滤波后数据不是很多了，博主没有对递归次数进行限制，只对拟合出直线的点云数量做了限制，有需要的小伙伴根据需求改进；