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

2.1 激光雷達時間序列

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

目前大部分主流激光雷達應(yīng)該都可以直接在點云中提供每個點對應(yīng)的掃描線已經(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ù)，如果是平面的話應(yīng)該是指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;    //可選設(shè)置    seg.setOptimizeCoefficients (true);    //必須設(shè)置    seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);    seg.setMethodType(pcl::SAC_RANSAC);    // 設(shè)置迭代次數(shù)的上限    seg.setMaxIterations(200);    // 設(shè)置距離閾值    seg.setDistanceThreshold (0.04);    //設(shè)置所搜索平面垂直的軸     seg.setAxis(Eigen::Vector3f(0,0,1));    //設(shè)置待檢測的平面模型和上述軸的最大角度    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:濾波結(jié)果取反，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è)置四個參數(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è)置模型系數(shù)需要優(yōu)化    seg.setOptimizeCoefficients(true);    // 必要的配置，設(shè)置分割的模型類型，所用的隨機參數(shù)估計方法，距離閥值，輸入點云    seg.setModelType(pcl::SACMODEL_PLANE); //設(shè)置模型類型    seg.setMethodType(pcl::SAC_RANSAC);    //設(shè)置隨機采樣一致性方法類型    seg.setDistanceThreshold(0.01);        //設(shè)定距離閥值，距離閥值決定了點被認為是局內(nèi)點是必須滿足的條件                                           //表示點到估計模型的距離最大值，    seg.setInputCloud(cloud);    //引發(fā)分割實現(xiàn)，存儲分割結(jié)果到點幾何inliers及存儲平面模型的系數(shù)coefficients    seg.segment(*inliers, *coefficients);

2.4圓柱體提取

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

再使用一個預(yù)先設(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);                        // 設(shè)置搜索方式    ne.setInputCloud(cloud);                        // 設(shè)置輸入點云    ne.setKSearch(50);                                // 設(shè)置K近鄰搜索點的個數(shù)    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);    ne.compute(*cloud_normals);                        // 計算法向量，并將結(jié)果保存到cloud_normals中    //=====================================================================    //----------------------------圓柱體分割--------------------------------    cout << "- >正在圓柱體分割..." << endl;    pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;        // 創(chuàng)建圓柱體分割對象    seg.setInputCloud(cloud);                                        // 設(shè)置輸入點云：待分割點云    seg.setOptimizeCoefficients(true);                                // 設(shè)置對估計的模型系數(shù)需要進行優(yōu)化    seg.setModelType(pcl::SACMODEL_CYLINDER);                        // 設(shè)置分割模型為圓柱體模型    seg.setMethodType(pcl::SAC_RANSAC);                                // 設(shè)置采用RANSAC算法進行參數(shù)估計    seg.setNormalDistanceWeight(0.1);                                // 設(shè)置表面法線權(quán)重系數(shù)    seg.setMaxIterations(10000);                                    // 設(shè)置迭代的最大次數(shù)    seg.setDistanceThreshold(0.05);                                    // 設(shè)置內(nèi)點到模型距離的最大值    seg.setRadiusLimits(3.0, 4.0);                                    // 設(shè)置圓柱模型半徑的范圍    seg.setInputNormals(cloud_normals);                                // 設(shè)置輸入法向量    pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices);    // 保存分割結(jié)果    pcl::ModelCoefficients::Ptr coefficients_cylinder(new pcl::ModelCoefficients);    // 保存圓柱體模型系數(shù)    seg.segment(*inliers_cylinder, *coefficients_cylinder);            // 執(zhí)行分割，將分割結(jié)果的索引保存到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);                //設(shè)置搜索空間    pcl::PointXYZ searchPoint;                    //定義查詢點    searchPoint = cloud->points[0];    cout << "- >查詢點坐標為：" << searchPoint << endl;    float R = 0.1;                                //設(shè)置搜索半徑大小    vector<int> pointIdxRadiusSearch;            //存儲近鄰索引    vector<float> pointRadiusSquaredDistance;    //存儲近鄰對應(yīng)的平方距離    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);    //設(shè)置近鄰搜索的半徑    ec.setMinClusterSize(200);        //設(shè)置最小聚類點數(shù)    ec.setMaxClusterSize(999999);    //設(shè)置最大聚類點數(shù)    ec.setSearchMethod(tree);    ec.setInputCloud(cloud);    ec.extract(cluster_indices);    /// 執(zhí)行歐式聚類分割，并保存分割結(jié)果    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ū)域生長的基本思想是將具有相似性質(zhì)的點集合起來構(gòu)成區(qū)域。

首先對每個需要分割的區(qū)域找出一個種子作為生長的起點,然后將種子周圍鄰域中與種子有相同或相似性質(zhì)的點(根據(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>);    //設(shè)置搜索方法    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è)置滿足聚類的最小點數(shù)    rg.setMaxClusterSize(99999999);                        //設(shè)置滿足聚類的最大點數(shù)    rg.setSearchMethod(tree);                            //設(shè)置搜索方法    rg.setNumberOfNeighbours(30);                        //設(shè)置鄰域搜索的點數(shù)    rg.setInputCloud(cloud);                            //設(shè)置輸入點云    rg.setInputNormals(normals);                        //設(shè)置輸入點云的法向量    rg.setSmoothnessThreshold(3.0 / 180.0 * M_PI);        //設(shè)置平滑閾值，弧度，用于提取同一區(qū)域的點    rg.setCurvatureThreshold(1.0);                        //設(shè)置曲率閾值，如果兩個點的法線偏差很小，則測試其曲率之間的差異。如果該值小于曲率閾值，則該算法將使用新添加的點繼續(xù)簇的增長    vector<pcl::PointIndices> clusters;                    //聚類索引向量    rg.extract(clusters);                                //獲取聚類結(jié)果，并保存到索引向量中    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)點對應(yīng)的索引    /// 根據(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 >