自动驾驶激光雷达特征提取

2.1 激光雷达时间序列

这一帧数据中点的排列顺序为从最高的线束到最低的线束进行排列，每条线束之间点按逆时针的顺序排列。

目前大部分主流激光雷达应该都可以直接在点云中提供每个点对应的扫描线已经时间戳，所以其实可以不用这种粗略的方法来进行计算。

template <typename PointType>void sortPointCloudByScanLine(boost::shared_ptr<pcl::PointCloud<PointType>>& src_cloud_ptr,                              std::vector<boost::shared_ptr<pcl::PointCloud<PointType>>>& dst_cloud_ptr_vec) {    const int N_SCAN = 64;    dst_cloud_ptr_vec.resize(N_SCAN);    dst_cloud_ptr_vec.clear();    PointType pt;    int line_id;    for (int i = 0; i < src_cloud_ptr->points.size(); ++i) {        pt      = src_cloud_ptr->points[i];        line_id = 0;        double elevation_angle = std::atan2(pt.z, std::sqrt(pt.x * pt.x + pt.y * pt.y)) / M_PI * 180.0;        // adapt from a-loam        if (elevation_angle >= -8.83)            line_id = int((2 - elevation_angle) * 3.0 + 0.5);        else            line_id = 64 / 2 + int((-8.83 - elevation_angle) * 2.0 + 0.5);        if (elevation_angle > 2 || elevation_angle < -24.33 || line_id > 63 || line_id < 0) {            continue;        }        if (dst_cloud_ptr_vec[line_id] == nullptr)            dst_cloud_ptr_vec[line_id] = boost::make_shared<pcl::PointCloud<PointType>>();        dst_cloud_ptr_vec[line_id]->points.push_back(pt);    }}

2.2 三维激光雷达压缩成二维

void filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& ground, PCLPointCloud& nonground) const{  ground.header = pc.header;  nonground.header = pc.header;  if (pc.size() < 50){    ROS_WARN("Pointcloud in OctomapServer too small, skipping ground plane extraction");    nonground = pc;  } else {      // https://blog.csdn.net/weixin_41552975/article/details/120428619    // 指模型参数，如果是平面的话应该是指a b c d四个参数值    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);    // 创建分割对象    pcl::SACSegmentation<PCLPoint> seg;    //可选设置    seg.setOptimizeCoefficients (true);    //必须设置    seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);    seg.setMethodType(pcl::SAC_RANSAC);    // 设置迭代次数的上限    seg.setMaxIterations(200);    // 设置距离阈值    seg.setDistanceThreshold (0.04);    //设置所搜索平面垂直的轴     seg.setAxis(Eigen::Vector3f(0,0,1));    //设置待检测的平面模型和上述轴的最大角度    seg.setEpsAngle(0.15);    // pc 赋值    PCLPointCloud cloud_filtered(pc);    //创建滤波器    pcl::ExtractIndices<PCLPoint> extract;    bool groundPlaneFound = false;    while(cloud_filtered.size() > 10 && !groundPlaneFound){         // 所有点云传入，并通过coefficients提取到所有平面      seg.setInputCloud(cloud_filtered.makeShared());      seg.segment (*inliers, *coefficients);      if (inliers->indices.size () == 0){        ROS_INFO("PCL segmentation did not find any plane.");        break;      }      //输入要滤波的点云      extract.setInputCloud(cloud_filtered.makeShared());      //被提取的点的索引集合      extract.setIndices(inliers);      if (std::abs(coefficients->values.at(3)) < 0.07){        ROS_DEBUG("Ground plane found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), cloud_filtered.size(),                  coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2), coefficients->values.at(3));        //true:滤波结果取反，false，则是取正        extract.setNegative (false);        //获取地面点集合，并传入ground        extract.filter (ground);        // 存在有不是平面的点        if(inliers->indices.size() != cloud_filtered.size()){          extract.setNegative(true);          PCLPointCloud cloud_out;          // 传入cloud_out          extract.filter(cloud_out);          // 不断减少cloud_filtered数目，同时累加nonground数目          cloud_filtered = cloud_out;          nonground += cloud_out;        }        groundPlaneFound = true;      } else{ // 否则提取那些不是平面的，然后剩下的就是平面点        ROS_DEBUG("Horizontal plane (not ground) found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), cloud_filtered.size(),                  coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2), coefficients->values.at(3));        pcl::PointCloud<PCLPoint> cloud_out;        extract.setNegative (false);        extract.filter(cloud_out);        nonground +=cloud_out;        if(inliers->indices.size() != cloud_filtered.size()){          extract.setNegative(true);          cloud_out.points.clear();          extract.filter(cloud_out);          cloud_filtered = cloud_out;        } else{          cloud_filtered.points.clear();        }      }    }    // 由于没有找到平面，则会进入下面    if (!groundPlaneFound){      ROS_WARN("No ground plane found in scan");      // 对高度进行粗略调整，以防止出现虚假障碍物      pcl::PassThrough<PCLPoint> second_pass;      second_pass.setFilterFieldName("z");      second_pass.setFilterLimits(-m_groundFilterPlaneDistance, m_groundFilterPlaneDistance);      second_pass.setInputCloud(pc.makeShared());      second_pass.filter(ground);      second_pass.setFilterLimitsNegative (true);      second_pass.filter(nonground);    }    // Create a set of planar coefficients with X=Y=0,Z=1    pcl::ModelCoefficients::Ptr coefficients1(new pcl::ModelCoefficients());    coefficients1->values.resize(4);    coefficients1->values[0] = 1;    coefficients1->values[1] = 0;    coefficients1->values[2] = 0;    coefficients1->values[3] = 0;    // Create the filtering object    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);    pcl::ProjectInliers<pcl::PointXYZ> proj;    proj.setModelType(pcl::SACMODEL_PLANE);    proj.setInputCloud(nonground);    proj.setModelCoefficients(coefficients1);    proj.filter(*cloud_projected);    if (cloud_projected.size() > 0)             writer.write<PCLPoint>("cloud_projected.pcd",cloud_projected, false);  }}

2.3 面特征提取

PCL中Sample——consensus模块提供了RANSAC平面拟合模块。

SACMODEL_PLANE 模型：定义为平面模型，共设置四个参数 [normal_x，normal_y，normal_z，d]。其中，（normal_x，normal_y，normal_z）为平面法向量 ( A , B , C )，d 为常数项D。

//创建分割时所需要的模型系数对象，coefficients及存储内点的点索引集合对象inliers    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);    // 创建分割对象    pcl::SACSegmentation<pcl::PointXYZ> seg;    // 可选择配置，设置模型系数需要优化    seg.setOptimizeCoefficients(true);    // 必要的配置，设置分割的模型类型，所用的随机参数估计方法，距离阀值，输入点云    seg.setModelType(pcl::SACMODEL_PLANE); //设置模型类型    seg.setMethodType(pcl::SAC_RANSAC);    //设置随机采样一致性方法类型    seg.setDistanceThreshold(0.01);        //设定距离阀值，距离阀值决定了点被认为是局内点是必须满足的条件                                           //表示点到估计模型的距离最大值，    seg.setInputCloud(cloud);    //引发分割实现，存储分割结果到点几何inliers及存储平面模型的系数coefficients    seg.segment(*inliers, *coefficients);

2.4圆柱体提取

圆柱体的提取也是基于Ransec来实现提取，RANSAC从样本中随机抽选出一个样本子集，使用最小方差估计算法对这个子集计算模型参数，然后计算所有样本与该模型的偏差。

再使用一个预先设定好的阈值与偏差比较，当偏差小于阈值时，该样本点属于模型内样本点（inliers），简称内点，否则为模型外样本点（outliers），简称外点。

cout << "- >正在计算法线..." << endl;    pcl::NormalEstimation<PointT, pcl::Normal> ne;    // 创建法向量估计对象    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());    ne.setSearchMethod(tree);                        // 设置搜索方式    ne.setInputCloud(cloud);                        // 设置输入点云    ne.setKSearch(50);                                // 设置K近邻搜索点的个数    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);    ne.compute(*cloud_normals);                        // 计算法向量，并将结果保存到cloud_normals中    //=====================================================================    //----------------------------圆柱体分割--------------------------------    cout << "- >正在圆柱体分割..." << endl;    pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;        // 创建圆柱体分割对象    seg.setInputCloud(cloud);                                        // 设置输入点云：待分割点云    seg.setOptimizeCoefficients(true);                                // 设置对估计的模型系数需要进行优化    seg.setModelType(pcl::SACMODEL_CYLINDER);                        // 设置分割模型为圆柱体模型    seg.setMethodType(pcl::SAC_RANSAC);                                // 设置采用RANSAC算法进行参数估计    seg.setNormalDistanceWeight(0.1);                                // 设置表面法线权重系数    seg.setMaxIterations(10000);                                    // 设置迭代的最大次数    seg.setDistanceThreshold(0.05);                                    // 设置内点到模型距离的最大值    seg.setRadiusLimits(3.0, 4.0);                                    // 设置圆柱模型半径的范围    seg.setInputNormals(cloud_normals);                                // 设置输入法向量    pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices);    // 保存分割结果    pcl::ModelCoefficients::Ptr coefficients_cylinder(new pcl::ModelCoefficients);    // 保存圆柱体模型系数    seg.segment(*inliers_cylinder, *coefficients_cylinder);            // 执行分割，将分割结果的索引保存到inliers_cylinder中，同时存储模型系数coefficients_cylinder    cout << "ntt-----圆柱体系数-----" << endl;    cout << "轴线一点坐标：(" << coefficients_cylinder->values[0] << ", "        << coefficients_cylinder->values[1] << ", "        << coefficients_cylinder->values[2] << ")"        << endl;    cout << "轴线方向向量：(" << coefficients_cylinder->values[3] << ", "        << coefficients_cylinder->values[4] << ", "        << coefficients_cylinder->values[5] << ")"        << endl;    cout << "圆柱体半径：" << coefficients_cylinder->values[6] << endl;

2.5 半径近邻

半径内近邻搜索（Neighbors within Radius Search），是指搜索点云中一点在球体半径 R内的所有近邻点。

pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;        //创建kdtree对象    kdtree.setInputCloud(cloud);                //设置搜索空间    pcl::PointXYZ searchPoint;                    //定义查询点    searchPoint = cloud->points[0];    cout << "- >查询点坐标为：" << searchPoint << endl;    float R = 0.1;                                //设置搜索半径大小    vector<int> pointIdxRadiusSearch;            //存储近邻索引    vector<float> pointRadiusSquaredDistance;    //存储近邻对应的平方距离    cout << "n- >正在进行半径R邻域近邻搜索..." << endl << endl;    if (kdtree.radiusSearch(searchPoint, R, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)    {        //打印所有近邻点坐标，方式2        for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)        {            cout << "第" << i + 1 << "个近邻点："                << cloud->points[pointIdxRadiusSearch[i]]                << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << endl;        }    }    else    {        PCL_ERROR("未搜索到邻近点！an");    }

2.6 聚类

首先选取种子点，利用kd-tree对种子点进行半径r邻域搜索，若邻域内存在点，则与种子点归为同一聚类簇Q；

pcl::search::KdTree&lt;PointT&gt;::Ptr tree(new pcl::search::KdTree&lt;PointT&gt;);    // 创建kd树    tree-&gt;setInputCloud(cloud);    vector&lt;pcl::PointIndices&gt; cluster_indices;    pcl::EuclideanClusterExtraction&lt;PointT&gt; ec;    ec.setClusterTolerance(0.2);    //设置近邻搜索的半径    ec.setMinClusterSize(200);        //设置最小聚类点数    ec.setMaxClusterSize(999999);    //设置最大聚类点数    ec.setSearchMethod(tree);    ec.setInputCloud(cloud);    ec.extract(cluster_indices);    /// 执行欧式聚类分割，并保存分割结果    int j = 0;    for (vector< pcl::PointIndices >::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)    {        PointCloudT::Ptr cloud_cluster(new PointCloudT);        for (vector< int >::const_iterator pit = it- >indices.begin(); pit != it- >indices.end(); pit++)        {            cloud_cluster- >points.push_back(cloud- >points[*pit]);        }        /*cloud_cluster-&gt;width = cloud_cluster-&gt;points.size();        cloud_cluster-&gt;height = 1;        cloud_cluster-&gt;is_dense = true;*/        stringstream ss;        ss &lt;&lt; "tree_" &lt;&lt; j + 1 &lt;&lt; ".pcd";        pcl::io::savePCDFileBinary(ss.str(), *cloud_cluster);        cout &lt;&lt; "- >" &lt;&lt; ss.str() &lt;&lt; "详情：" &lt;&lt; endl;        cout &lt;&lt; *cloud_cluster &lt;&lt; endl;        j++;    }

2.7 区域生长

区域生长的基本思想是将具有相似性质的点集合起来构成区域。

首先对每个需要分割的区域找出一个种子作为生长的起点,然后将种子周围邻域中与种子有相同或相似性质的点(根据事先确定的生长或相似准则来确定，多为法向量、曲率)归并到种子所在的区域中。

cout << "- >正在估计点云法线..." << endl;    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;                                    //创建法线估计对象ne    pcl::search::Search<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);    //设置搜索方法    pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);            //存放法线    ne.setSearchMethod(tree);    ne.setInputCloud(cloud);    ne.setKSearch(20);    ne.compute(*normals);    //========================================================================    //------------------------------- 区域生长 -------------------------------    cout << "- >正在进行区域生长..." << endl;    pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> rg;    //创建区域生长分割对象    rg.setMinClusterSize(50);                            //设置满足聚类的最小点数    rg.setMaxClusterSize(99999999);                        //设置满足聚类的最大点数    rg.setSearchMethod(tree);                            //设置搜索方法    rg.setNumberOfNeighbours(30);                        //设置邻域搜索的点数    rg.setInputCloud(cloud);                            //设置输入点云    rg.setInputNormals(normals);                        //设置输入点云的法向量    rg.setSmoothnessThreshold(3.0 / 180.0 * M_PI);        //设置平滑阈值，弧度，用于提取同一区域的点    rg.setCurvatureThreshold(1.0);                        //设置曲率阈值，如果两个点的法线偏差很小，则测试其曲率之间的差异。如果该值小于曲率阈值，则该算法将使用新添加的点继续簇的增长    vector<pcl::PointIndices> clusters;                    //聚类索引向量    rg.extract(clusters);                                //获取聚类结果，并保存到索引向量中    cout << "- >聚类个数为" << clusters.size() << endl;

2.8 线特征拟合

一般线特征拟合的方式前提是先要滤除不必要的点，而这个就需要使用K-D tree来先实现搜索

pcl::SampleConsensusModelLine&lt;pcl::PointXYZ&gt;::Ptr model_line(new pcl::SampleConsensusModelLine&lt;pcl::PointXYZ&gt;(cloud));    //指定拟合点云与几何模型    pcl::RandomSampleConsensus&lt;pcl::PointXYZ&gt; ransac(model_line);    //创建随机采样一致性对象    ransac.setDistanceThreshold(0.01);    //内点到模型的最大距离    ransac.setMaxIterations(1000);        //最大迭代次数    ransac.computeModel();                //执行RANSAC空间直线拟合    vector&lt;int&gt; inliers;                //存储内点索引的向量    ransac.getInliers(inliers);            //提取内点对应的索引    /// 根据索引提取内点    pcl::PointCloud< pcl::PointXYZ >::Ptr cloud_line(new pcl::PointCloud< pcl::PointXYZ >);    pcl::copyPointCloud< pcl::PointXYZ >(*cloud, inliers, *cloud_line);    /// 模型参数    Eigen::VectorXf coefficient;    ransac.getModelCoefficients(coefficient);    cout &lt;&lt; "直线点向式方程为：n"        &lt;&lt; "   (x - " &lt;&lt; coefficient[0] &lt;&lt; ") / " &lt;&lt; coefficient[3]        &lt;&lt; " = (y - " &lt;&lt; coefficient[1] &lt;&lt; ") / " &lt;&lt; coefficient[4]        &lt;&lt; " = (z - " &lt;&lt; coefficient[2] &lt;&lt; ") / " &lt;&lt; coefficient[5];

2.9 点特征提取

点特征的提取和线特征的提取原理一样

pcl::HarrisKeypoint3D< pcl::PointXYZ, pcl::PointXYZI, pcl::Normal >