ROS 八叉樹地圖構建 - 給 octomap_server 增長半徑濾波器！

爲了在每幀點雲中濾除噪聲點，選擇了半徑濾波器，也用高斯濾波器測試過，可是沒有半徑效果好，這裏記錄下在 octomap_server 中增長半徑濾波器的步驟，並在 launch 中配置濾波器參數。

1、半徑濾波器基本原理

放一張彙報用的 PPT 截圖：

原理很簡單就是判斷一個點雲周圍（半徑 R）有沒有足夠多（K）的鄰居點，若是沒有就刪除這個點，不然就保留。

2、基本用法

我通常學習技術喜歡到官網看最原始的教程：Removing outliers using a Conditional or RadiusOutlier removal，這個教程介紹了半徑濾波器（我不清楚中文名到底叫什麼濾波器）的基本用法：

#include <pcl/point_types.h>
#include <pcl/filters/radius_outlier_removal.h>

// 輸入待濾波的原始點雲指針
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

// 保存濾波後的點雲指針
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

// 建立濾波器對象
pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;

// 設置要濾波的點雲
outrem.setInputCloud(cloud);

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

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

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

若是第一次使用 PCL 的濾波器，能夠把這個教程本身運行一遍，我以前運行過了，此次就不貼代碼了，下面分享下我在實際項目中若是使用這個半徑濾波器對個人 octomap_server 構建的八叉樹地圖進行濾波。

3、給個人地圖濾波

3.1 定義半徑濾波器參數

半徑濾波器有 2 個參數：濾波半徑和半徑內部鄰居數，注意數據類型

// 濾波半徑
double m_outrem_radius;

// 半徑內的鄰居數
int m_outrem_neighbors;

在構造函數初始化列表中初始化：

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 中讀取啓動參數：

// 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 執行半徑濾波

在 InsertPointCloudCallBack 函數的 PassThough 前執行半徑濾波，即對每一幀點雲在構建八叉樹地圖前進行濾波，主要是爲了去掉單獨的離羣點：

// 對一幀 pc 點雲進行半徑濾波
pcl::RadiusOutlierRemoval<pcl::PointXYZRGB> outrem;

// 這裏須要傳遞指針，由於個人 pc 不是指針，因此這裏作了 makeShared
outrem.setInputCloud(pc.makeShared());

// 設置濾波半徑，這裏設置爲 1m
outrem.setRadiusSearch(m_outrem_radius); 

// 設置濾波近鄰數，這裏設置爲 10 個
outrem.setMinNeighborsInRadius (m_outrem_neighbors);

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

3.3 在 launch 中配置半徑濾波器參數

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

這樣之後就能夠從 launch 中直接配置濾波器的參數了，不用每次修改再從新編譯，這樣調試起來很是方便。

3.4 濾波結果

這是原始地圖，15cm 分辨率，紅框內部有不少單個的點：

這是濾波後的效果，濾波半徑 1m，近鄰點 10 個：

效果仍是能夠的，但願能對你有幫助，若是使用其餘的濾波器，按照官方的教程來就好了，掌握學習方法纔是最重要的：）