1. 程式人生 > >PCL經典程式碼賞析四:點雲濾波

PCL經典程式碼賞析四:點雲濾波

##· 說明

##· 目錄索引

  1. PCL 濾波介紹
  2. 直通濾波器對點雲進行濾波處理
  3. VoxelGrid濾波器對點雲進行下采樣
  4. statisticalOutlierRemoval濾波器移除離群點
  5. 使用引數化模型投影點雲
  6. 從一個點雲中提取索引
  7. ConditionalRemoval 或 RadiusOutlinerRemoval 移除離群點

##· PCL 濾波介紹

在獲取點雲資料時 ,由於裝置精度,操作者經驗環境因素帶來的影響,以及電磁波的衍射特性,被測物體表面性質變化和資料拼接配準操作過程的影響,點雲資料中講不可避免的出現一些噪聲。在點雲處理流程中濾波處理作為預處理的第一步,對後續的影響比較大,只有在濾波預處理中將噪聲點 ,離群點,孔洞,資料壓縮等按照後續處理定製,才能夠更好的進行配準,特徵提取,曲面重建,視覺化等後續應用處理,PCL中點雲濾波模組提供了很多靈活實用的濾波處理演算法,例如:雙邊濾波,高斯濾波,條件濾波,直通濾波,基於隨機取樣一致性濾波等。

  • PCL中點雲濾波的方案
  • 點雲資料密度不規則需要平滑
  • 因為遮擋等問題造成離群點需要去除
  • 大量資料需要下采樣
  • 噪聲資料需要去除
  • 對應的方案如下
  • 按照給定的規則限制過濾去除點
  • 通過常用濾波演算法修改點的部分屬性
  • 對資料進行下采樣
  • 去除噪音

##· 直通濾波器對點雲進行濾波處理

最簡單的例子:比如說高程篩選

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>

int
 main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

  //生成並填充點雲
  cloud->width  = 5;
  cloud->height = 1;
  cloud->points.resize (cloud->width * cloud->height);

  for (size_t i = 0; i < cloud->points.size (); ++i)   //填充資料
  {
    cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
  }

  std::cerr << "Cloud before filtering: " << std::endl;   //列印
  for (size_t i = 0; i < cloud->points.size (); ++i)
    std::cerr << "    " << cloud->points[i].x << " " 
                        << cloud->points[i].y << " " 
                        << cloud->points[i].z << std::endl;
  /************************************************************************************
   建立直通濾波器的物件,設立引數,濾波欄位名被設定為Z軸方向,可接受的範圍為(0.0,1.0)
   即將點雲中所有點的Z軸座標不在該範圍內的點過濾掉或保留,這裡是過濾掉,由函式setFilterLimitsNegative設定
   ***********************************************************************************/
  // 設定濾波器物件
  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setInputCloud (cloud);            //設定輸入點雲
  pass.setFilterFieldName ("z");         //設定過濾時所需要點雲型別的Z欄位
  pass.setFilterLimits (0.0, 1.0);        //設定在過濾欄位的範圍
  //pass.setFilterLimitsNegative (true);   //設定保留範圍內還是過濾掉範圍內
  pass.filter (*cloud_filtered);            //執行濾波,儲存過濾結果在cloud_filtered

  std::cerr << "Cloud after filtering: " << std::endl;   //列印
  for (size_t i = 0; i < cloud_filtered->points.size (); ++i)
    std::cerr << "    " << cloud_filtered->points[i].x << " " 
                        << cloud_filtered->points[i].y << " " 
                        << cloud_filtered->points[i].z << std::endl;

  return (0);
}

##· VoxelGrid濾波器對點雲進行下采樣

使用體素化網格方法實現下采樣,即減少點的數量 減少點雲資料,並同時儲存點雲的形狀特徵,在提高配準,曲面重建,形狀識別等演算法速度中非常實用,PCL是實現的VoxelGrid類通過輸入的點雲資料建立一個三維體素柵格,容納後每個體素內用體素中所有點的重心來近似顯示體素中其他點,這樣該體素內所有點都用一個重心點最終表示,對於所有體素處理後得到的過濾後的點雲,這種方法比用體素中心(注意中心重心)逼近的方法更慢,但是對於取樣點對應曲面的表示更為準確。

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>


int
main (int argc, char** argv)
{

  pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());
  pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());

  //點雲物件的讀取
  pcl::PCDReader reader;
 
  reader.read ("table_400.pcd", *cloud);    //讀取點雲到cloud中

  std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height 
       << " data points (" << pcl::getFieldsList (*cloud) << ").";

  /******************************************************************************
  建立一個葉大小為1cm的pcl::VoxelGrid濾波器,
**********************************************************************************/
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;  //建立濾波物件
  sor.setInputCloud (cloud);            //設定需要過濾的點雲給濾波物件
  sor.setLeafSize (0.01f, 0.01f, 0.01f);  //設定濾波時建立的體素體積為1cm的立方體
  sor.filter (*cloud_filtered);           //執行濾波處理,儲存輸出

  std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height 
       << " data points (" << pcl::getFieldsList (*cloud_filtered) << ").";

  pcl::PCDWriter writer;
  writer.write ("table_scene_lms400_downsampled.pcd", *cloud_filtered, 
         Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);

  return (0);
}

##· statisticalOutlierRemoval濾波器移除離群點

**問題描述:**鐳射掃描通常會產生密度不均勻的點雲資料集,另外測量中的誤差也會產生稀疏的離群點,此時,估計區域性點雲特徵(例如取樣點處法向量或曲率變化率)時運算複雜,這會導致錯誤的數值,反過來就會導致點雲配準等後期的處理失敗。

**解決辦法:**對每個點的鄰域進行一個統計分析,並修剪掉一些不符合標準的點。具體方法為在輸入資料中對點到臨近點的距離分佈的計算,對每一個點,計算它到所有臨近點的平均距離(假設得到的結果是一個高斯分佈,其形狀是由均值和標準差決定),那麼平均距離在標準範圍之外的點,可以被定義為離群點並從資料中去除。

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>

int
main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

  // 定義讀取物件
  pcl::PCDReader reader;
  // 讀取點雲檔案
  reader.read<pcl::PointXYZ> ("table_scene_lms400.pcd", *cloud);

  std::cerr << "Cloud before filtering: " << std::endl;
  std::cerr << *cloud << std::endl;

  // 建立濾波器,對每個點分析的臨近點的個數設定為50 ,並將標準差的倍數設定為1  這意味著如果一
   //個點的距離超出了平均距離一個標準差以上,則該點被標記為離群點,並將它移除,儲存起來
  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;   //建立濾波器物件
  sor.setInputCloud (cloud);                           //設定待濾波的點雲
  sor.setMeanK (50);                               //設定在進行統計時考慮查詢點臨近點數
  sor.setStddevMulThresh (1.0);                      //設定判斷是否為離群點的閥值
  sor.filter (*cloud_filtered);                    //儲存

  std::cerr << "Cloud after filtering: " << std::endl;
  std::cerr << *cloud_filtered << std::endl;

  pcl::PCDWriter writer;
  writer.write<pcl::PointXYZ> ("table_scene_lms400_inliers.pcd", *cloud_filtered, false);

  sor.setNegative (true);
  sor.filter (*cloud_filtered);
  writer.write<pcl::PointXYZ> ("table_scene_lms400_outliers.pcd", *cloud_filtered, false);

  return (0);
}

##· 使用引數化模型投影點雲

如何將點投影到一個引數化模型上(平面或者球體等),引數化模型通過一組引數來設定,對於平面來說使用其等式形式。在PCL中有特定儲存常見模型係數的資料結構

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>             //模型係數標頭檔案
#include <pcl/filters/project_inliers.h>          //投影濾波類標頭檔案

int
 main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);

  //建立點雲並打印出來
  cloud->width  = 5;
  cloud->height = 1;
  cloud->points.resize (cloud->width * cloud->height);

  for (size_t i = 0; i < cloud->points.size (); ++i)
  {
    cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
  }

  std::cerr << "Cloud before projection: " << std::endl;
  for (size_t i = 0; i < cloud->points.size (); ++i)
    std::cerr << "    " << cloud->points[i].x << " " 
                        << cloud->points[i].y << " " 
                        << cloud->points[i].z << std::endl;

  // 填充ModelCoefficients的值,使用ax+by+cz+d=0平面模型,其中 a=b=d=0,c=1 也就是X——Y平面
  //定義模型係數物件,並填充對應的資料
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
  coefficients->values.resize (4);
  coefficients->values[0] = coefficients->values[1] = 0;
  coefficients->values[2] = 1.0;
  coefficients->values[3] = 0;

  // 建立ProjectInliers物件,使用ModelCoefficients作為投影物件的模型引數
  pcl::ProjectInliers<pcl::PointXYZ> proj;     //建立投影濾波物件
  proj.setModelType (pcl::SACMODEL_PLANE);      //設定物件對應的投影模型
  proj.setInputCloud (cloud);                   //設定輸入點雲
  proj.setModelCoefficients (coefficients);       //設定模型對應的係數
  proj.filter (*cloud_projected);                 //投影結果儲存

  std::cerr << "Cloud after projection: " << std::endl;
  for (size_t i = 0; i < cloud_projected->points.size (); ++i)
    std::cerr << "    " << cloud_projected->points[i].x << " " 
                        << cloud_projected->points[i].y << " " 
                        << cloud_projected->points[i].z << std::endl;

  return (0);
}

實驗結果可以看出投影前的Z軸都不為0 ,都是隨機產生的值,投影之後,列印的結果表明,xy的值都沒有改變,z都變為0
所以該投影濾波類就是輸入點雲和投影模型,輸出為投影到模型上之後的點雲。

##· 從一個點雲中提取索引

基於某一分割演算法提取點雲中的一個子集

#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>

int
main (int argc, char** argv)
{  

  /**********************************************************************************************************
   從輸入的.PCD 檔案載入資料後,建立一個VOxelGrid濾波器對資料進行下采樣,在這裡進行下才樣是為了加速處理過程,
   越少的點意味著分割迴圈中處理起來越快
   **********************************************************************************************************/

  pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2), cloud_filtered_blob (new pcl::PCLPointCloud2);//申明濾波前後的點雲
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);

  // 讀取PCD檔案
  pcl::PCDReader reader;
  reader.read ("table_scene_lms400.pcd", *cloud_blob);
   //統計濾波前的點雲個數
  std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl;

  // 建立體素柵格下采樣: 下采樣的大小為1cm
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;  //體素柵格下采樣物件
  sor.setInputCloud (cloud_blob);             //原始點雲
  sor.setLeafSize (0.01f, 0.01f, 0.01f);    // 設定取樣體素大小
  sor.filter (*cloud_filtered_blob);        //儲存

  // 轉換為模板點雲
  pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);

  std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;

  // 儲存下采樣後的點雲
  pcl::PCDWriter writer;
  writer.write<pcl::PointXYZ> ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false);
  
  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.setMaxIterations (1000);                            //設定最大迭代次數
  seg.setDistanceThreshold (0.01);                      //判斷是否為模型內點的距離閥值

  // 設定ExtractIndices的實際引數
  pcl::ExtractIndices<pcl::PointXYZ> extract;        //建立點雲提取物件

  int i = 0, nr_points = (int) cloud_filtered->points.size ();
  // While 30% of the original cloud is still there
  while (cloud_filtered->points.size () > 0.3 * nr_points)
  {
    // 為了處理點雲包含的多個模型,在一個迴圈中執行該過程並在每次模型被提取後,儲存剩餘的點進行迭代
    seg.setInputCloud (cloud_filtered);
    seg.segment (*inliers, *coefficients);
    if (inliers->indices.size () == 0)
    {
      std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
      break;
    }

    // Extract the inliers
    extract.setInputCloud (cloud_filtered);
    extract.setIndices (inliers);
    extract.setNegative (false);
    extract.filter (*cloud_p);
    std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;

    std::stringstream ss;
    ss << "table_scene_lms400_plane_" << i << ".pcd";
    writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);

    // Create the filtering object
    extract.setNegative (true);
    extract.filter (*cloud_f);
    cloud_filtered.swap (cloud_f);
    i++;
  }

  return (0);
}

圖一 原始點雲圖像
圖一 原始點雲圖像
圖二 點雲下采樣
圖二 點雲下采樣
圖三 分割得到平面模型1
圖三 分割得到平面模型1
圖四 分割得到平面模型2
圖四 分割得到平面模型2

##· ConditionalRemoval 或 RadiusOutlinerRemoval 移除離群點

  • ConditionalRemoval 濾波器的理解
    • 可以一次刪除滿足對輸入的點雲設定的一個或多個條件指標的所有的資料點
    • 刪除點雲中不符合使用者指定的一個或者多個條件的資料點
  • ConditionalRemoval 濾波器的理解
    • 可以一次刪除滿足對輸入的點雲設定的一個或多個條件指標的所有的資料點
    • 刪除點雲中不符合使用者指定的一個或者多個條件的資料點
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>

int
 main (int argc, char** argv)
{
  if (argc != 2)  //確保輸入的引數
  {
    std::cerr << "please specify command line arg '-r' or '-c'" << std::endl;
    exit(0);
  }
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

  //填充點雲
  cloud->width  = 5;
  cloud->height = 1;
  cloud->points.resize (cloud->width * cloud->height);

  for (size_t i = 0; i < cloud->points.size (); ++i)
  {
    cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
  }

  if (strcmp(argv[1], "-r") == 0){
    pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;  //建立濾波器
    
    outrem.setInputCloud(cloud);    //設定輸入點雲
    outrem.setRadiusSearch(0.8);     //設定半徑為0.8的範圍內找臨近點
    outrem.setMinNeighborsInRadius (2); //設定查詢點的鄰域點集數小於2的刪除
    // apply filter
    outrem.filter (*cloud_filtered);     //執行條件濾波   在半徑為0.8 在此半徑內必須要有兩個鄰居點,此點才會儲存
  }
  else if (strcmp(argv[1], "-c") == 0){
    //建立條件限定的下的濾波器
    pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond (new
      pcl::ConditionAnd<pcl::PointXYZ> ());   //建立條件定義物件
      //為條件定義物件新增比較運算元
    range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
      pcl::FieldComparison<pcl::PointXYZ> ("z", pcl::ComparisonOps::GT, 0.0)));   //新增在Z欄位上大於0的比較運算元

    range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
      pcl::FieldComparison<pcl::PointXYZ> ("z", pcl::ComparisonOps::LT, 0.8)));   //新增在Z欄位上小於0.8的比較運算元
    // 建立濾波器並用條件定義物件初始化
    pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
    condrem.setCondition (range_cond);               
    condrem.setInputCloud (cloud);                   //輸入點雲
    condrem.setKeepOrganized(true);               //設定保持點雲的結構
    // 執行濾波
    condrem.filter (*cloud_filtered);  //大於0.0小於0.8這兩個條件用於建立濾波器
  }
  else{
    std::cerr << "please specify command line arg '-r' or '-c'" << std::endl;
    exit(0);
  }
  std::cerr << "Cloud before filtering: " << std::endl;
  for (size_t i = 0; i < cloud->points.size (); ++i)
    std::cerr << "    " << cloud->points[i].x << " "
                        << cloud->points[i].y << " "
                        << cloud->points[i].z << std::endl;
  // display pointcloud after filtering
  std::cerr << "Cloud after filtering: " << std::endl;
  for (size_t i = 0; i < cloud_filtered->points.size (); ++i)
    std::cerr << "    " << cloud_filtered->points[i].x << " "
                        << cloud_filtered->points[i].y << " "
                        << cloud_filtered->points[i].z << std::endl;
  return (0);
}