自動駕駛激光雷達特征提取

2.1 激光雷達時間序列

這一幀數(shù)據(jù)中點的排列順序為從最高的線束到最低的線束進行排列，每條線束之間點按逆時針的順序排列。

目前大部分主流激光雷達應該都可以直接在點云中提供每個點對應的掃描線已經(jīng)時間戳，所以其實可以不用這種粗略的方法來進行計算。

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    // 指模型參數(shù)，如果是平面的話應該是指a b c d四個參數(shù)值    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);    // 創(chuàng)建分割對象    pcl::SACSegmentation<PCLPoint> seg;    //可選設置    seg.setOptimizeCoefficients (true);    //必須設置    seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);    seg.setMethodType(pcl::SAC_RANSAC);    // 設置迭代次數(shù)的上限    seg.setMaxIterations(200);    // 設置距離閾值    seg.setDistanceThreshold (0.04);    //設置所搜索平面垂直的軸     seg.setAxis(Eigen::Vector3f(0,0,1));    //設置待檢測的平面模型和上述軸的最大角度    seg.setEpsAngle(0.15);    // pc 賦值    PCLPointCloud cloud_filtered(pc);    //創(chuàng)建濾波器    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數(shù)目，同時累加nonground數(shù)目          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");      // 對高度進行粗略調(diào)整，以防止出現(xiàn)虛假障礙物      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 模型：定義為平面模型，共設置四個參數(shù) [normal_x，normal_y，normal_z，d]。其中，（normal_x，normal_y，normal_z）為平面法向量 ( A , B , C )，d 為常數(shù)項D。

//創(chuàng)建分割時所需要的模型系數(shù)對象，coefficients及存儲內(nèi)點的點索引集合對象inliers    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);    // 創(chuàng)建分割對象    pcl::SACSegmentation<pcl::PointXYZ> seg;    // 可選擇配置，設置模型系數(shù)需要優(yōu)化    seg.setOptimizeCoefficients(true);    // 必要的配置，設置分割的模型類型，所用的隨機參數(shù)估計方法，距離閥值，輸入點云    seg.setModelType(pcl::SACMODEL_PLANE); //設置模型類型    seg.setMethodType(pcl::SAC_RANSAC);    //設置隨機采樣一致性方法類型    seg.setDistanceThreshold(0.01);        //設定距離閥值，距離閥值決定了點被認為是局內(nèi)點是必須滿足的條件                                           //表示點到估計模型的距離最大值，    seg.setInputCloud(cloud);    //引發(fā)分割實現(xiàn)，存儲分割結果到點幾何inliers及存儲平面模型的系數(shù)coefficients    seg.segment(*inliers, *coefficients);

2.4圓柱體提取

圓柱體的提取也是基于Ransec來實現(xiàn)提取，RANSAC從樣本中隨機抽選出一個樣本子集，使用最小方差估計算法對這個子集計算模型參數(shù)，然后計算所有樣本與該模型的偏差。

再使用一個預先設定好的閾值與偏差比較，當偏差小于閾值時，該樣本點屬于模型內(nèi)樣本點（inliers），簡稱內(nèi)點，否則為模型外樣本點（outliers），簡稱外點。

cout << "- >正在計算法線..." << endl;    pcl::NormalEstimation<PointT, pcl::Normal> ne;    // 創(chuàng)建法向量估計對象    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());    ne.setSearchMethod(tree);                        // 設置搜索方式    ne.setInputCloud(cloud);                        // 設置輸入點云    ne.setKSearch(50);                                // 設置K近鄰搜索點的個數(shù)    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;        // 創(chuàng)建圓柱體分割對象    seg.setInputCloud(cloud);                                        // 設置輸入點云：待分割點云    seg.setOptimizeCoefficients(true);                                // 設置對估計的模型系數(shù)需要進行優(yōu)化    seg.setModelType(pcl::SACMODEL_CYLINDER);                        // 設置分割模型為圓柱體模型    seg.setMethodType(pcl::SAC_RANSAC);                                // 設置采用RANSAC算法進行參數(shù)估計    seg.setNormalDistanceWeight(0.1);                                // 設置表面法線權重系數(shù)    seg.setMaxIterations(10000);                                    // 設置迭代的最大次數(shù)    seg.setDistanceThreshold(0.05);                                    // 設置內(nèi)點到模型距離的最大值    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);    // 保存圓柱體模型系數(shù)    seg.segment(*inliers_cylinder, *coefficients_cylinder);            // 執(zhí)行分割，將分割結果的索引保存到inliers_cylinder中，同時存儲模型系數(shù)coefficients_cylinder    cout << "ntt-----圓柱體系數(shù)-----" << 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 半徑近鄰

半徑內(nèi)近鄰搜索（Neighbors within Radius Search），是指搜索點云中一點在球體半徑 R內(nèi)的所有近鄰點。

pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;        //創(chuàng)建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鄰域搜索，若鄰域內(nèi)存在點，則與種子點歸為同一聚類簇Q；

pcl::search::KdTree&lt;PointT&gt;::Ptr tree(new pcl::search::KdTree&lt;PointT&gt;);    // 創(chuàng)建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);        //設置最小聚類點數(shù)    ec.setMaxClusterSize(999999);    //設置最大聚類點數(shù)    ec.setSearchMethod(tree);    ec.setInputCloud(cloud);    ec.extract(cluster_indices);    /// 執(zhí)行歐式聚類分割，并保存分割結果    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 區(qū)域生長

區(qū)域生長的基本思想是將具有相似性質的點集合起來構成區(qū)域。

首先對每個需要分割的區(qū)域找出一個種子作為生長的起點,然后將種子周圍鄰域中與種子有相同或相似性質的點(根據(jù)事先確定的生長或相似準則來確定，多為法向量、曲率)歸并到種子所在的區(qū)域中。

cout << "- >正在估計點云法線..." << endl;    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;                                    //創(chuàng)建法線估計對象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);    //========================================================================    //------------------------------- 區(qū)域生長 -------------------------------    cout << "- >正在進行區(qū)域生長..." << endl;    pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> rg;    //創(chuàng)建區(qū)域生長分割對象    rg.setMinClusterSize(50);                            //設置滿足聚類的最小點數(shù)    rg.setMaxClusterSize(99999999);                        //設置滿足聚類的最大點數(shù)    rg.setSearchMethod(tree);                            //設置搜索方法    rg.setNumberOfNeighbours(30);                        //設置鄰域搜索的點數(shù)    rg.setInputCloud(cloud);                            //設置輸入點云    rg.setInputNormals(normals);                        //設置輸入點云的法向量    rg.setSmoothnessThreshold(3.0 / 180.0 * M_PI);        //設置平滑閾值，弧度，用于提取同一區(qū)域的點    rg.setCurvatureThreshold(1.0);                        //設置曲率閾值，如果兩個點的法線偏差很小，則測試其曲率之間的差異。如果該值小于曲率閾值，則該算法將使用新添加的點繼續(xù)簇的增長    vector<pcl::PointIndices> clusters;                    //聚類索引向量    rg.extract(clusters);                                //獲取聚類結果，并保存到索引向量中    cout << "- >聚類個數(shù)為" << clusters.size() << endl;

2.8 線特征擬合

一般線特征擬合的方式前提是先要濾除不必要的點，而這個就需要使用K-D tree來先實現(xiàn)搜索

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);    //創(chuàng)建隨機采樣一致性對象    ransac.setDistanceThreshold(0.01);    //內(nèi)點到模型的最大距離    ransac.setMaxIterations(1000);        //最大迭代次數(shù)    ransac.computeModel();                //執(zhí)行RANSAC空間直線擬合    vector&lt;int&gt; inliers;                //存儲內(nèi)點索引的向量    ransac.getInliers(inliers);            //提取內(nèi)點對應的索引    /// 根據(jù)索引提取內(nèi)點    pcl::PointCloud< pcl::PointXYZ >::Ptr cloud_line(new pcl::PointCloud< pcl::PointXYZ >);    pcl::copyPointCloud< pcl::PointXYZ >(*cloud, inliers, *cloud_line);    /// 模型參數(shù)    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 >