读自动驾驶激光雷达物体检测技术(Lidar Obstacle Detection)(2):点云滤波FilterCloud()函数
FilterCloud()所包括的功能:
1.首先使用體素濾波(相當于做稀釋減少點的數量)(體素網格過濾將創建一個立方體網格, 過濾點云的方法是每個體素立方體內只留下一個點, 因此立方體每一邊的長度越大, 點云的分辨率就越低. 但是如果體素網格太大, 就會損失掉物體原本的特征。)
2.過濾掉給定立方體之外的點云數據。(定義感興趣區域, 并刪除感興趣區域外的任何點. 感興趣區域的選擇兩側需要盡量覆蓋車道的寬度, 而前后的區域要保證你可以及時檢測到前后車輛的移動。 在最終結果中, 我們使用pcl CropBox 查找自身車輛車頂的點云數據索引, 然后將這些索引提供給 pcl ExtractIndices 對象刪除, 因為這些對于我們分析點云數據沒有用處。)(相當于將自身車輛作為坐標軸的中心點(原點),然后以自身為中心 ,圈出一個立方體的范圍(該立方體只需要知道對角線上的兩個點AB坐標即可確定),成為每一次運動時候的感興趣區域,也就是只關心區域內點的聚類等后續操作。如下圖:立方體之外的就不要了。)
?
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??
3.提取車身周圍小范圍內的所有的點,并將提取到的所有點保存在indices中。相當于在上圖立方體內再扣掉一個立方體(扣掉的這個立方體即為車身范圍內的即把車這部分沒采取到的盲區給去掉。)如上圖右圖。(整個過程就是先踢掉左圖中黑色點,剩下紅色立方體。再從紅色立方體中再扣點右圖中鉛筆畫的那部分。)
下面會有以單個pcd為例進行的展示。
原始函數聲明在processPointCloud.h
原始函數定義在processPointCloud.cpp
函數調用在environment.cpp中的cityBlock()函數中
本部分涉及到的知識:
1、Eigen是可以用來進行線性代數、矩陣、向量操作等運算的C++庫,它里面包含了很多算法。
Eigen:矩陣(Matrix)類的介紹及使用(在Eigen中,所有矩陣和向量均為Matrix模板類的對象,向量是矩陣的行(或列)為1是的特殊情況。)
1.1、矩陣的三參數模板
Matrix類有六個模板參數,其中三個有默認值,因此只要學習三個參數就足夠了。
/* 強制性的三參數模板的原型 (三個參數分別表示:標量的類型,編譯時的行,編譯時的列) */
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"直接定義了一個維度為3的列向量,3行1列
typedef Matrix<int, 1, 2> RowVector2i;//行向量1行2列
1.3、特殊動態值(special value Dynamic)
Eigen的矩陣不僅能夠在編譯是確定大小(fixed size),也可以在運行時確定大小,就是所說的動態矩陣(dynamic size)。
typedef Matrix<double, Dynamic, Dynamic> MatrixXd; //行列都不固定
typedef Matrix<int, Dynamic, 1> VectorXi; //行不限制固定列/* 也可使用‘行’固定‘列’動態的矩陣 */
Matrix<float, 3, Dynamic>
1.4、構造函數(Constructors)
可以使用默認的構造函數,不執行動態分配內存,也沒有初始化矩陣參數:
Matrix3f a; // a是3-by-3矩陣,包含未初始化的 float[9] 數組
MatrixXf b; // b是動態矩陣,當前大小為 0-by-0, 沒有為數組的系數分配內存/* 矩陣的第一個參數表示“行”,數組只有一個參數。根據跟定的大小分配內存,但不初始化 */
MatrixXf a(10,15); // a 是10-by-15陣,分配了內存,沒有初始化
VectorXf b(30); // b是動態矩陣,當前大小為 30, 分配了內存,沒有初始化/* 對于給定的矩陣,傳遞的參數無效 */
Matrix3f a(3,3); /* 對于維數最大為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、系數訪問
系數都是從0開始,矩陣默認按列存儲
#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、逗號分隔的初始化
Matrix3f m;
m << 1, 2, 3, 4, 5, 6, 7, 8, 9;
cout << m;
1.7、Resizing
可以用rows(), cols() and size() 改變現有矩陣的大小。這些類方法返回行、列、系數的數值。也可以用resize()來改變動態矩陣的大小。
矩陣與向量間的點成叉乘轉置以及矩陣求逆等見https://zhuanlan.zhihu.com/p/36772345
2、CropBoxfilterr---------過濾指定立方體內的點
參照博客:https://blog.csdn.net/ethan_guo/article/details/80359313
#include <pcl/filters/crop_box.h>//要包含的頭文件。要包含哪些頭文件,需要去查官方文檔對該類的介紹。
pcl::PointCloud<pcl::PointXYZ>::Ptr body {new pcl::PointCloud<pcl::PointXYZ>};
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_body {new pcl::PointCloud<pcl::PointXYZ>};//指針還是對象,有時候只能指針,有時候都行。報錯就換。
pcl::CropBox<pcl::PointXYZRGBA> box_filter;//濾波器對象
box_filter.setMin(Eigen::Vector4f(x_min, y_min, z_min, 1.0));//Min和Max是指立方體的兩個對角點。每個點由一個四維向量表示,通常最后一個是1.(不知道為什么要有四個,大神知道的給解答一下下)
box_filter.setMax(Eigen::Vector4f(x_max, y_max, z_max, 1.0));
clipper.setNegative(false);//是保留立方體內的點而去除其他點,還是反之。false是將盒子內的點去除,默認為false
box_filter.setInputCloud(body);//輸入源
box_filter.filter(*filtered_body);//濾它!
以單個pcd文件進行濾波操作展示:
原始點云圖片如下圖:
濾波后點云圖片如下圖:
代碼(只是針對于濾波代碼)以及注釋如下:
filterRes是體素網格的大小, minPoint/maxPoint為感興趣區域的最近點和最遠點.
我們首先執行VoxelGrid減少點云數量, 然后設置最近和最遠點之間的感興趣區域, 最后再從中刪除車頂的點云.
頭文件:
#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、點云濾波
pcl::PointCloud<pcl::PointXYZ>::Ptr FilterCloud(pcl::PointCloud<PointXYZ>::Ptr cloud, float filterRes, Eigen::Vector4f minPoint, Eigen::Vector4f maxPoint);//聲明濾波點云函數,參數1原始點云cloud,參數2體素濾波中的葉子大小,參數3和4"Vector4f"直接定義了一個維度為4的列向量表示的是定義的立方體對角坐標大小
源文件:
#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; //創建濾波對象pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFiltered(new pcl::PointCloud<pcl::PointXYZ>); //創建保存體素濾波后的點對象cloudFilteredvg.setInputCloud(cloud);//輸入cloudvg.setLeafSize(filterRes, filterRes, filterRes);//設置葉子大小(這么大個葉子節點內只提取一個點)vg.filter(*cloudFiltered);//濾波后的點云保存在cloudFiltered//2.過濾掉在用戶給定立方體內的點云數據//理解:將自身車輛作為坐標軸的中心點,然后在身邊自身為中心 ,圈出一個立方體的范圍,成為每一次運動時候的感興趣區域,也就是只關心區域內點的聚類等后續操作pcl::PointCloud<pcl::PointXYZ>::Ptr cloudRegion(new pcl::PointCloud<pcl::PointXYZ>);pcl::CropBox<pcl::PointXYZ> region(true);//濾波器對象region.setMin(minPoint);region.setMax(maxPoint);region.setInputCloud(cloudFiltered);region.filter(*cloudRegion);//3.提取車身周圍范圍內的所有的點,并將提取到的所有點保存在indices中std::vector<int> indices;pcl::CropBox<pcl::PointXYZ> roof(true);//濾波器對象roof.setMin(Eigen::Vector4f(-1.5, -1.7, -1, 1));//看數據像是車身大小roof.setMax(Eigen::Vector4f(2.6, 1.7, -0.4, 1));roof.setInputCloud(cloudRegion);//輸入的是上部中的立方體roof.filter(indices);pcl::PointIndices::Ptr inliers{ new pcl::PointIndices }; //創建一個內點對象,將提取到車身周圍點,放到內點對象中for (int point : indices){inliers->indices.push_back(point);}pcl::ExtractIndices<pcl::PointXYZ> extract;extract.setInputCloud(cloudRegion);extract.setIndices(inliers);extract.setNegative(true); //false 提取內點也就是提取車身周圍的幾個點, true提取出了車身周圍的點extract.filter(*cloudRegion);pcl::PCDWriter writer;writer.write("F:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection\\data\\pcd\\filter.pcd", *cloudRegion);return cloudRegion;
}
主函數:
#include "filter.h"
int main(int argc, char **argv)
{// 1、濾波 濾波后點云存入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("顯示窗口")); //窗口顯示點云 viewer->addPointCloud(cloudRegion, "*cloud"); viewer->resetCamera(); system("pause"); return (0);}
?
總結
以上是生活随笔為你收集整理的读自动驾驶激光雷达物体检测技术(Lidar Obstacle Detection)(2):点云滤波FilterCloud()函数的全部內容,希望文章能夠幫你解決所遇到的問題。
- 上一篇: boost--文件、目录操作
- 下一篇: 读自动驾驶激光雷达物体检测技术(Lida