2.1 激光雷達(dá)時(shí)間序列

這一幀數(shù)據(jù)中點(diǎn)的排列順序?yàn)閺淖罡叩木€(xiàn)束到最低的線(xiàn)束進(jìn)行排列，每條線(xiàn)束之間點(diǎn)按逆時(shí)針的順序排列。

目前大部分主流激光雷達(dá)應(yīng)該都可以直接在點(diǎn)云中提供每個(gè)點(diǎn)對(duì)應(yīng)的掃描線(xiàn)已經(jīng)時(shí)間戳，所以其實(shí)可以不用這種粗略的方法來(lái)進(jìn)行計(jì)算。

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 三維激光雷達(dá)壓縮成二維

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ù)，如果是平面的話(huà)應(yīng)該是指a b c d四個(gè)參數(shù)值    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);    // 創(chuàng)建分割對(duì)象    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è)置待檢測(cè)的平面模型和上述軸的最大角度    seg.setEpsAngle(0.15);    // pc 賦值    PCLPointCloud cloud_filtered(pc);    //創(chuàng)建濾波器    pcl::ExtractIndices<PCLPoint> extract;    bool groundPlaneFound = false;    while(cloud_filtered.size() > 10 && !groundPlaneFound){         // 所有點(diǎn)云傳入，并通過(guò)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;      }      //輸入要濾波的點(diǎn)云      extract.setInputCloud(cloud_filtered.makeShared());      //被提取的點(diǎn)的索引集合      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);        //獲取地面點(diǎn)集合，并傳入ground        extract.filter (ground);        // 存在有不是平面的點(diǎn)        if(inliers->indices.size() != cloud_filtered.size()){          extract.setNegative(true);          PCLPointCloud cloud_out;          // 傳入cloud_out          extract.filter(cloud_out);          // 不斷減少cloud_filtered數(shù)目，同時(shí)累加nonground數(shù)目          cloud_filtered = cloud_out;          nonground += cloud_out;        }        groundPlaneFound = true;      } else{ // 否則提取那些不是平面的，然后剩下的就是平面點(diǎn)        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();        }      }    }    // 由于沒(méi)有找到平面，則會(huì)進(jìn)入下面    if (!groundPlaneFound){      ROS_WARN("No ground plane found in scan");      // 對(duì)高度進(jìn)行粗略調(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è)置四個(gè)參數(shù) [normal_x，normal_y，normal_z，d]。其中，（normal_x，normal_y，normal_z）為平面法向量 ( A , B , C )，d 為常數(shù)項(xiàng)D。

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

2.4圓柱體提取

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

再使用一個(gè)預(yù)先設(shè)定好的閾值與偏差比較，當(dāng)偏差小于閾值時(shí)，該樣本點(diǎn)屬于模型內(nèi)樣本點(diǎn)（inliers），簡(jiǎn)稱(chēng)內(nèi)點(diǎn)，否則為模型外樣本點(diǎn)（outliers），簡(jiǎn)稱(chēng)外點(diǎn)。

cout << "- >正在計(jì)算法線(xiàn)..." << endl;    pcl::NormalEstimation<PointT, pcl::Normal> ne;    // 創(chuàng)建法向量估計(jì)對(duì)象    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());    ne.setSearchMethod(tree);                        // 設(shè)置搜索方式    ne.setInputCloud(cloud);                        // 設(shè)置輸入點(diǎn)云    ne.setKSearch(50);                                // 設(shè)置K近鄰搜索點(diǎn)的個(gè)數(shù)    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);    ne.compute(*cloud_normals);                        // 計(jì)算法向量，并將結(jié)果保存到cloud_normals中    //=====================================================================    //----------------------------圓柱體分割--------------------------------    cout << "- >正在圓柱體分割..." << endl;    pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;        // 創(chuàng)建圓柱體分割對(duì)象    seg.setInputCloud(cloud);                                        // 設(shè)置輸入點(diǎn)云：待分割點(diǎn)云    seg.setOptimizeCoefficients(true);                                // 設(shè)置對(duì)估計(jì)的模型系數(shù)需要進(jìn)行優(yōu)化    seg.setModelType(pcl::SACMODEL_CYLINDER);                        // 設(shè)置分割模型為圓柱體模型    seg.setMethodType(pcl::SAC_RANSAC);                                // 設(shè)置采用RANSAC算法進(jìn)行參數(shù)估計(jì)    seg.setNormalDistanceWeight(0.1);                                // 設(shè)置表面法線(xiàn)權(quán)重系數(shù)    seg.setMaxIterations(10000);                                    // 設(shè)置迭代的最大次數(shù)    seg.setDistanceThreshold(0.05);                                    // 設(shè)置內(nèi)點(diǎn)到模型距離的最大值    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í)存儲(chǔ)模型系數(shù)coefficients_cylinder    cout << "ntt-----圓柱體系數(shù)-----" << endl;    cout << "軸線(xiàn)一點(diǎn)坐標(biāo)：(" << coefficients_cylinder->values[0] << ", "        << coefficients_cylinder->values[1] << ", "        << coefficients_cylinder->values[2] << ")"        << endl;    cout << "軸線(xiàn)方向向量：(" << 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），是指搜索點(diǎn)云中一點(diǎn)在球體半徑 R內(nèi)的所有近鄰點(diǎn)。

pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;        //創(chuàng)建kdtree對(duì)象    kdtree.setInputCloud(cloud);                //設(shè)置搜索空間    pcl::PointXYZ searchPoint;                    //定義查詢(xún)點(diǎn)    searchPoint = cloud->points[0];    cout << "- >查詢(xún)點(diǎn)坐標(biāo)為：" << searchPoint << endl;    float R = 0.1;                                //設(shè)置搜索半徑大小    vector<int> pointIdxRadiusSearch;            //存儲(chǔ)近鄰索引    vector<float> pointRadiusSquaredDistance;    //存儲(chǔ)近鄰對(duì)應(yīng)的平方距離    cout << "n- >正在進(jìn)行半徑R鄰域近鄰搜索..." << endl << endl;    if (kdtree.radiusSearch(searchPoint, R, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)    {        //打印所有近鄰點(diǎn)坐標(biāo)，方式2        for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)        {            cout << "第" << i + 1 << "個(gè)近鄰點(diǎn)："                << cloud->points[pointIdxRadiusSearch[i]]                << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << endl;        }    }    else    {        PCL_ERROR("未搜索到鄰近點(diǎn)！an");    }

2.6 聚類(lèi)

首先選取種子點(diǎn)，利用kd-tree對(duì)種子點(diǎn)進(jìn)行半徑r鄰域搜索，若鄰域內(nèi)存在點(diǎn)，則與種子點(diǎn)歸為同一聚類(lèi)簇Q；

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

區(qū)域生長(zhǎng)的基本思想是將具有相似性質(zhì)的點(diǎn)集合起來(lái)構(gòu)成區(qū)域。

首先對(duì)每個(gè)需要分割的區(qū)域找出一個(gè)種子作為生長(zhǎng)的起點(diǎn),然后將種子周?chē)徲蛑信c種子有相同或相似性質(zhì)的點(diǎn)(根據(jù)事先確定的生長(zhǎng)或相似準(zhǔn)則來(lái)確定，多為法向量、曲率)歸并到種子所在的區(qū)域中。

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

2.8 線(xiàn)特征擬合

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

pcl::SampleConsensusModelLine&lt;pcl::PointXYZ&gt;::Ptr model_line(new pcl::SampleConsensusModelLine&lt;pcl::PointXYZ&gt;(cloud));    //指定擬合點(diǎn)云與幾何模型    pcl::RandomSampleConsensus&lt;pcl::PointXYZ&gt; ransac(model_line);    //創(chuàng)建隨機(jī)采樣一致性對(duì)象    ransac.setDistanceThreshold(0.01);    //內(nèi)點(diǎn)到模型的最大距離    ransac.setMaxIterations(1000);        //最大迭代次數(shù)    ransac.computeModel();                //執(zhí)行RANSAC空間直線(xiàn)擬合    vector&lt;int&gt; inliers;                //存儲(chǔ)內(nèi)點(diǎn)索引的向量    ransac.getInliers(inliers);            //提取內(nèi)點(diǎn)對(duì)應(yīng)的索引    /// 根據(jù)索引提取內(nèi)點(diǎn)    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; "直線(xiàn)點(diǎn)向式方程為：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 點(diǎn)特征提取

點(diǎn)特征的提取和線(xiàn)特征的提取原理一樣

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