FilterCloud()所包括的功能：

1.首先使用體素濾波（相當(dāng)于做稀釋減少點(diǎn)的數(shù)量）（體素網(wǎng)格過(guò)濾將創(chuàng)建一個(gè)立方體網(wǎng)格, 過(guò)濾點(diǎn)云的方法是每個(gè)體素立方體內(nèi)只留下一個(gè)點(diǎn), 因此立方體每一邊的長(zhǎng)度越大, 點(diǎn)云的分辨率就越低. 但是如果體素網(wǎng)格太大, 就會(huì)損失掉物體原本的特征。）

2.過(guò)濾掉給定立方體之外的點(diǎn)云數(shù)據(jù)。（定義感興趣區(qū)域, 并刪除感興趣區(qū)域外的任何點(diǎn). 感興趣區(qū)域的選擇兩側(cè)需要盡量覆蓋車(chē)道的寬度, 而前后的區(qū)域要保證你可以及時(shí)檢測(cè)到前后車(chē)輛的移動(dòng)。 在最終結(jié)果中, 我們使用pcl CropBox 查找自身車(chē)輛車(chē)頂?shù)狞c(diǎn)云數(shù)據(jù)索引, 然后將這些索引提供給 pcl ExtractIndices 對(duì)象刪除, 因?yàn)檫@些對(duì)于我們分析點(diǎn)云數(shù)據(jù)沒(méi)有用處。）（相當(dāng)于將自身車(chē)輛作為坐標(biāo)軸的中心點(diǎn)（原點(diǎn)），然后以自身為中心 ，圈出一個(gè)立方體的范圍（該立方體只需要知道對(duì)角線上的兩個(gè)點(diǎn)AB坐標(biāo)即可確定），成為每一次運(yùn)動(dòng)時(shí)候的感興趣區(qū)域，也就是只關(guān)心區(qū)域內(nèi)點(diǎn)的聚類(lèi)等后續(xù)操作。如下圖：立方體之外的就不要了。）
?

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??

3.提取車(chē)身周?chē)》秶鷥?nèi)的所有的點(diǎn)，并將提取到的所有點(diǎn)保存在indices中。相當(dāng)于在上圖立方體內(nèi)再扣掉一個(gè)立方體（扣掉的這個(gè)立方體即為車(chē)身范圍內(nèi)的即把車(chē)這部分沒(méi)采取到的盲區(qū)給去掉。）如上圖右圖。（整個(gè)過(guò)程就是先踢掉左圖中黑色點(diǎn)，剩下紅色立方體。再?gòu)募t色立方體中再扣點(diǎn)右圖中鉛筆畫(huà)的那部分。）

下面會(huì)有以單個(gè)pcd為例進(jìn)行的展示。

原始函數(shù)聲明在processPointCloud.h

原始函數(shù)定義在processPointCloud.cpp

函數(shù)調(diào)用在environment.cpp中的cityBlock()函數(shù)中

本部分涉及到的知識(shí)：

1、Eigen是可以用來(lái)進(jìn)行線性代數(shù)、矩陣、向量操作等運(yùn)算的C++庫(kù)，它里面包含了很多算法。

Eigen：矩陣(Matrix)類(lèi)的介紹及使用（在Eigen中，所有矩陣和向量均為Matrix模板類(lèi)的對(duì)象，向量是矩陣的行（或列）為1是的特殊情況。）

1.1、矩陣的三參數(shù)模板
Matrix類(lèi)有六個(gè)模板參數(shù)，其中三個(gè)有默認(rèn)值，因此只要學(xué)習(xí)三個(gè)參數(shù)就足夠了。

/* 強(qiáng)制性的三參數(shù)模板的原型 (三個(gè)參數(shù)分別表示：標(biāo)量的類(lèi)型，編譯時(shí)的行，編譯時(shí)的列) */
Matrix<typename Scalar, int RowsAtCompileTime, int ColsAtCompileTime> /* 用typedef定義了很多模板，例如：Matrix4f 表示 4×4 的floats 矩陣 */ 
typedef Matrix<float, 4, 4> Matrix4f;

1.2、向量（Vectors）
向量是矩陣的特殊情況，也是用矩陣定義的。

typedef Matrix<float, 3, 1> Vector3f;  //"Vector3f"直接定義了一個(gè)維度為3的列向量，3行1列
typedef Matrix<int, 1, 2> RowVector2i;//行向量1行2列

1.3、特殊動(dòng)態(tài)值（special value Dynamic）
Eigen的矩陣不僅能夠在編譯是確定大小（fixed size），也可以在運(yùn)行時(shí)確定大小，就是所說(shuō)的動(dòng)態(tài)矩陣（dynamic size）。

typedef Matrix<double, Dynamic, Dynamic> MatrixXd; //行列都不固定 
typedef Matrix<int, Dynamic, 1> VectorXi;  //行不限制固定列/* 也可使用‘行’固定‘列’動(dòng)態(tài)的矩陣 */
Matrix<float, 3, Dynamic>

1.4、構(gòu)造函數(shù)（Constructors）
可以使用默認(rèn)的構(gòu)造函數(shù)，不執(zhí)行動(dòng)態(tài)分配內(nèi)存，也沒(méi)有初始化矩陣參數(shù)：

Matrix3f a;   // a是3-by-3矩陣，包含未初始化的 float[9] 數(shù)組
MatrixXf b;   // b是動(dòng)態(tài)矩陣，當(dāng)前大小為 0-by-0, 沒(méi)有為數(shù)組的系數(shù)分配內(nèi)存/* 矩陣的第一個(gè)參數(shù)表示“行”，數(shù)組只有一個(gè)參數(shù)。根據(jù)跟定的大小分配內(nèi)存，但不初始化 */
MatrixXf a(10,15);    // a 是10-by-15陣，分配了內(nèi)存，沒(méi)有初始化
VectorXf b(30);       // b是動(dòng)態(tài)矩陣，當(dāng)前大小為 30, 分配了內(nèi)存，沒(méi)有初始化/* 對(duì)于給定的矩陣，傳遞的參數(shù)無(wú)效 */
Matrix3f a(3,3); /* 對(duì)于維數(shù)最大為4的向量，可以直接初始化 */
Vector2d a(5.0, 6.0);  
Vector3d b(5.0, 6.0, 7.0);  
Vector4d c(5.0, 6.0, 7.0, 8.0);

1.5、系數(shù)訪問(wèn)
系數(shù)都是從0開(kāi)始，矩陣默認(rèn)按列存儲(chǔ)

#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;int main()
{MatrixXd m(2, 2);m(0, 0) = 3;m(1, 0) = 2.5;m(0, 1) = -1;m(1, 1) = m(1, 0) + m(0, 1);cout << "Here is the matrix m:" << endl;cout << m << endl;VectorXd v(2);v(0) = 4;v[1] = v[0] - 1;     //operator[] 在 vectors 中重載，意義和()相同cout << "Here is the vector v:" << endl;cout << v << endl;getchar();getchar();
}

1.6、逗號(hào)分隔的初始化

Matrix3f m;
m << 1, 2, 3,   4, 5, 6,   7, 8, 9;
cout << m;

1.7、Resizing
可以用rows(), cols() and size() 改變現(xiàn)有矩陣的大小。這些類(lèi)方法返回行、列、系數(shù)的數(shù)值。也可以用resize()來(lái)改變動(dòng)態(tài)矩陣的大小。

矩陣與向量間的點(diǎn)成叉乘轉(zhuǎn)置以及矩陣求逆等見(jiàn)https://zhuanlan.zhihu.com/p/36772345

2、CropBoxfilterr---------過(guò)濾指定立方體內(nèi)的點(diǎn)

參照博客：https://blog.csdn.net/ethan_guo/article/details/80359313

#include <pcl/filters/crop_box.h>//要包含的頭文件。要包含哪些頭文件，需要去查官方文檔對(duì)該類(lèi)的介紹。
pcl::PointCloud<pcl::PointXYZ>::Ptr body {new pcl::PointCloud<pcl::PointXYZ>};
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_body {new pcl::PointCloud<pcl::PointXYZ>};//指針還是對(duì)象，有時(shí)候只能指針，有時(shí)候都行。報(bào)錯(cuò)就換。
pcl::CropBox<pcl::PointXYZRGBA> box_filter;//濾波器對(duì)象
box_filter.setMin(Eigen::Vector4f(x_min, y_min, z_min, 1.0));//Min和Max是指立方體的兩個(gè)對(duì)角點(diǎn)。每個(gè)點(diǎn)由一個(gè)四維向量表示，通常最后一個(gè)是1.（不知道為什么要有四個(gè)，大神知道的給解答一下下）
box_filter.setMax(Eigen::Vector4f(x_max, y_max, z_max, 1.0));

clipper.setNegative(false);//是保留立方體內(nèi)的點(diǎn)而去除其他點(diǎn)，還是反之。false是將盒子內(nèi)的點(diǎn)去除，默認(rèn)為false
box_filter.setInputCloud(body);//輸入源
box_filter.filter(*filtered_body);//濾它！

以單個(gè)pcd文件進(jìn)行濾波操作展示：

原始點(diǎn)云圖片如下圖：

濾波后點(diǎn)云圖片如下圖：

代碼（只是針對(duì)于濾波代碼）以及注釋如下：

filterRes是體素網(wǎng)格的大小, minPoint/maxPoint為感興趣區(qū)域的最近點(diǎn)和最遠(yuǎn)點(diǎn).

我們首先執(zhí)行VoxelGrid減少點(diǎn)云數(shù)量, 然后設(shè)置最近和最遠(yuǎn)點(diǎn)之間的感興趣區(qū)域, 最后再?gòu)闹袆h除車(chē)頂?shù)狞c(diǎn)云.

頭文件：

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h>
#include <pcl/visualization/cloud_viewer.h>
#include<vector>
#include <iostream>
#include <string>
#include<Eigen/Core>
#include<Eigen/Dense>
#include <pcl/filters/crop_box.h>using namespace std;
using namespace pcl;//1、點(diǎn)云濾波
pcl::PointCloud<pcl::PointXYZ>::Ptr FilterCloud(pcl::PointCloud<PointXYZ>::Ptr cloud, float filterRes, Eigen::Vector4f minPoint, Eigen::Vector4f maxPoint);//聲明濾波點(diǎn)云函數(shù)，參數(shù)1原始點(diǎn)云cloud，參數(shù)2體素濾波中的葉子大小，參數(shù)3和4"Vector4f"直接定義了一個(gè)維度為4的列向量表示的是定義的立方體對(duì)角坐標(biāo)大小

源文件：

#include "filter.h"
pcl::PointCloud<pcl::PointXYZ>::Ptr FilterCloud(pcl::PointCloud<PointXYZ>::Ptr cloud, float filterRes, Eigen::Vector4f minPoint, Eigen::Vector4f maxPoint)
{//1.使用體素濾波下采樣pcl::VoxelGrid<pcl::PointXYZ> vg;   //創(chuàng)建濾波對(duì)象pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFiltered(new pcl::PointCloud<pcl::PointXYZ>); //創(chuàng)建保存體素濾波后的點(diǎn)對(duì)象cloudFilteredvg.setInputCloud(cloud);//輸入cloudvg.setLeafSize(filterRes, filterRes, filterRes);//設(shè)置葉子大小（這么大個(gè)葉子節(jié)點(diǎn)內(nèi)只提取一個(gè)點(diǎn)）vg.filter(*cloudFiltered);//濾波后的點(diǎn)云保存在cloudFiltered//2.過(guò)濾掉在用戶(hù)給定立方體內(nèi)的點(diǎn)云數(shù)據(jù)//理解：將自身車(chē)輛作為坐標(biāo)軸的中心點(diǎn)，然后在身邊自身為中心 ，圈出一個(gè)立方體的范圍，成為每一次運(yùn)動(dòng)時(shí)候的感興趣區(qū)域，也就是只關(guān)心區(qū)域內(nèi)點(diǎn)的聚類(lèi)等后續(xù)操作pcl::PointCloud<pcl::PointXYZ>::Ptr cloudRegion(new pcl::PointCloud<pcl::PointXYZ>);pcl::CropBox<pcl::PointXYZ> region(true);//濾波器對(duì)象region.setMin(minPoint);region.setMax(maxPoint);region.setInputCloud(cloudFiltered);region.filter(*cloudRegion);//3.提取車(chē)身周?chē)秶鷥?nèi)的所有的點(diǎn)，并將提取到的所有點(diǎn)保存在indices中std::vector<int> indices;pcl::CropBox<pcl::PointXYZ> roof(true);//濾波器對(duì)象roof.setMin(Eigen::Vector4f(-1.5, -1.7, -1, 1));//看數(shù)據(jù)像是車(chē)身大小roof.setMax(Eigen::Vector4f(2.6, 1.7, -0.4, 1));roof.setInputCloud(cloudRegion);//輸入的是上部中的立方體roof.filter(indices);pcl::PointIndices::Ptr inliers{ new pcl::PointIndices }; //創(chuàng)建一個(gè)內(nèi)點(diǎn)對(duì)象，將提取到車(chē)身周?chē)c(diǎn)，放到內(nèi)點(diǎn)對(duì)象中for (int point : indices){inliers->indices.push_back(point);}pcl::ExtractIndices<pcl::PointXYZ> extract;extract.setInputCloud(cloudRegion);extract.setIndices(inliers);extract.setNegative(true);  //false 提取內(nèi)點(diǎn)也就是提取車(chē)身周?chē)膸讉€(gè)點(diǎn)， true提取出了車(chē)身周?chē)狞c(diǎn)extract.filter(*cloudRegion);pcl::PCDWriter writer;writer.write("F:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection\\data\\pcd\\filter.pcd", *cloudRegion);return cloudRegion;
}

主函數(shù)：

#include "filter.h"
int main(int argc, char **argv)
{// 1、濾波  濾波后點(diǎn)云存入filteredCloud		pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::PCDReader reader;reader.read("F:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection\\data\\pcd\\data_1\\0000000006.pcd", *inputCloud);float filterRes = 0.4;Eigen::Vector4f minpoint(-10, -6.5, -2, 1);Eigen::Vector4f maxpoint(30, 6.5, 1, 1);pcl::PointCloud<pcl::PointXYZ>::Ptr cloudRegion(new pcl::PointCloud<pcl::PointXYZ>);cloudRegion=FilterCloud(inputCloud, filterRes, minpoint, maxpoint);pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("顯示窗口"));  //窗口顯示點(diǎn)云	viewer->addPointCloud(cloudRegion, "*cloud");	viewer->resetCamera();		system("pause");	return (0);}

?

總結(jié)

以上是生活随笔為你收集整理的读自动驾驶激光雷达物体检测技术（Lidar Obstacle Detection）（2）：点云滤波FilterCloud（）函数的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。