ROS 八叉樹(shù)地圖構(gòu)建 - 給 octomap_server 增加半徑濾波器！

為了在每幀點(diǎn)云中濾除噪聲點(diǎn)，選擇了半徑濾波器，也用高斯濾波器測(cè)試過(guò)，但是沒(méi)有半徑效果好，這里記錄下在 octomap_server 中增加半徑濾波器的步驟，并在 launch 中配置濾波器參數(shù)。

一、半徑濾波器基本原理

放一張匯報(bào)用的 PPT 截圖：

原理很簡(jiǎn)單就是判斷一個(gè)點(diǎn)云周圍（半徑 R）有沒(méi)有足夠多（K）的鄰居點(diǎn)，如果沒(méi)有就刪除這個(gè)點(diǎn)，否則就保留。

二、基本用法

我一般學(xué)習(xí)技術(shù)喜歡到官網(wǎng)看最原始的教程：Removing outliers using a Conditional or RadiusOutlier removal，這個(gè)教程介紹了半徑濾波器（我不清楚中文名到底叫什么濾波器）的基本用法：

#include 
       
#include 
       

// 輸入待濾波的原始點(diǎn)云指針
pcl::PointCloud
     
      ::
      Ptr cloud (new pcl::PointCloud
        
         )
        ;
      

      

      // 保存濾波后的點(diǎn)云指針
      
pcl::PointCloud
      
       ::
       Ptr cloud_filtered (new pcl::PointCloud
         
          )
         ;
       

       

       // 創(chuàng)建濾波器對(duì)象
       
pcl::RadiusOutlierRemoval
       
         outrem;
        

        

        // 設(shè)置要濾波的點(diǎn)云
        
outrem.setInputCloud(cloud);
        

        

        // 設(shè)置濾波半徑
        
outrem.setRadiusSearch(
        0.8);
        

        

        // 設(shè)置濾波最少近鄰數(shù)
        
outrem.setMinNeighborsInRadius (
        2);
        

        

        // 執(zhí)行半徑濾波
        
outrem.filter (*cloud_filtered);
        

       
      
     

如果第一次使用 PCL 的濾波器，可以把這個(gè)教程自己運(yùn)行一遍，我之前運(yùn)行過(guò)了，這次就不貼代碼了，下面分享下我在實(shí)際項(xiàng)目中如果使用這個(gè)半徑濾波器對(duì)我的 octomap_server 構(gòu)建的八叉樹(shù)地圖進(jìn)行濾波。

三、給我的地圖濾波

3.1 定義半徑濾波器參數(shù)

半徑濾波器有 2 個(gè)參數(shù)：濾波半徑和半徑內(nèi)部鄰居數(shù)，注意數(shù)據(jù)類型

// 濾波半徑
double m_outrem_radius;

// 半徑內(nèi)的鄰居數(shù)
int m_outrem_neighbors;

在構(gòu)造函數(shù)初始化列表中初始化：

OctomapServer::OctomapServer(const ros::NodeHandle private_nh_, const ros::NodeHandle &nh_)
: ...,
  m_outrem_radius(-std::numeric_limits<double>::max()),
  m_outrem_neighbors(-std::numeric_limits<int>::max()),
  ...

從 launch 中讀取啟動(dòng)參數(shù)：

// add outrem filter
m_nh_private.param("outrem_radius", m_outrem_radius, m_outrem_radius);
m_nh_private.param("outrem_neighbors", m_outrem_neighbors, m_outrem_neighbors);

3.2 執(zhí)行半徑濾波

在 InsertPointCloudCallBack 函數(shù)的 PassThough 前執(zhí)行半徑濾波，即對(duì)每一幀點(diǎn)云在構(gòu)建八叉樹(shù)地圖前進(jìn)行濾波，主要是為了去掉單獨(dú)的離群點(diǎn)：

// 對(duì)一幀 pc 點(diǎn)云進(jìn)行半徑濾波
pcl::RadiusOutlierRemoval
     
       outrem;
      

      

      // 這里需要傳遞指針，因?yàn)槲业?nbsp;pc 不是指針，所以這里做了 makeShared
      
outrem.setInputCloud(pc.makeShared());
      

      

      // 設(shè)置濾波半徑，這里設(shè)置為 1m
      
outrem.setRadiusSearch(m_outrem_radius); 
      

      

      // 設(shè)置濾波近鄰數(shù)，這里設(shè)置為 10 個(gè)
      
outrem.setMinNeighborsInRadius (m_outrem_neighbors);
      

      

      // 執(zhí)行濾波
      
outrem.filter(pc);
      

     

3.3 在 launch 中配置半徑濾波器參數(shù)

<param name = "outrem_radius" type = "double" value = "1.0">
<param name = "outrem_neighbors" type = "int" value = "10">

這樣以后就可以從 launch 中直接配置濾波器的參數(shù)了，不用每次修改再重新編譯，這樣調(diào)試起來(lái)非常方便。

3.4 濾波結(jié)果

這是原始地圖，15cm 分辨率，紅框內(nèi)部有很多單個(gè)的點(diǎn)：

這是濾波后的效果，濾波半徑 1m，近鄰點(diǎn) 10 個(gè)：

效果還是可以的，希望能對(duì)你有幫助，如果使用其他的濾波器，按照官方的教程來(lái)就行了，掌握學(xué)習(xí)方法才是最重要的：）