为了说明代码转换过程,选择了以下示例:
对来自给定输入点云的强度数据应用双边滤波器,并将结果保存到磁盘。
代码:
一个 I/O 组件: 第21-27行(从磁盘读取数据)和第64行(将数据写入磁盘)
初始化组件:第29-35行(使用KdTree设置最近邻居的搜索方法)
实际算法组件:第7-11行和第37-61行
目标是将给定的算法转换为有用的PCL类,以便可以在其他地方重用。
1 #include <pcl/point_types.h>
2 #include <pcl/io/pcd_io.h>
3 #include <pcl/kdtree/kdtree_flann.h>
4
5 typedef pcl::PointXYZI PointT;
6
7 float
8 G (float x, float sigma)
9 {
10 return std::exp (- (x*x)/(2*sigma*sigma));
11 }
12
13 int
14 main (int argc, char *argv[])
15 {
16 std::string incloudfile = argv[1];
17 std::string outcloudfile = argv[2];
18 float sigma_s = atof (argv[3]);
19 float sigma_r = atof (argv[4]);
20
21 // 载入点云
22 pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
23 pcl::io::loadPCDFile (incloudfile.c_str (), *cloud);
24 int pnumber = (int)cloud->size ();
25
26 // 输出点云
27 pcl::PointCloud<PointT> outcloud = *cloud;
28
29 // 设置 KDTree
30 pcl::KdTreeFLANN<PointT>::Ptr tree (new pcl::KdTreeFLANN<PointT>);
31 tree->setInputCloud (cloud);
32
33 // 容器
34 std::vector<int> k_indices;
35 std::vector<float> k_distances;
36
37 // 主程序
38 for (int point_id = 0; point_id < pnumber; ++point_id)
39 {
40 float BF = 0;
41 float W = 0;
42
43 tree->radiusSearch (point_id, 2 * sigma_s, k_indices, k_distances);
44
45 // For each neighbor
46 for (std::size_t n_id = 0; n_id < k_indices.size (); ++n_id)
47 {
48 float id = k_indices.at (n_id);
49 float dist = sqrt (k_distances.at (n_id));
50 float intensity_dist = std::abs ((*cloud)[point_id].intensity - (*cloud)[id].intensity);
51
52 float w_a = G (dist, sigma_s);
53 float w_b = G (intensity_dist, sigma_r);
54 float weight = w_a * w_b;
55
56 BF += weight * (*cloud)[id].intensity;
57 W += weight;
58 }
59
60 outcloud[point_id].intensity = BF / W;
61 }
62
63 // Save filtered output
64 pcl::io::savePCDFile (outcloudfile.c_str (), outcloud);
65 return (0);
66 }
一:设置结构:
如果想让新算法成为PCL过滤库的一部分,首先在过滤器下创建3个不同的文件:
- Include/pcl/filters/bilateral.h 将包含所有定义;
- include/pcl/filters/impl/bilateral.hpp 将包含模板化实现
- src/bilateral.cpp 将包含显式的模板实例化
设置一个新类的名称,让我们称之为BilateralFilter
1、bilateral.h
bilateral.h 头文件将包含与 BilateralFilter 类相关的所有定义。这是一个最小的框架:
1 #pragma once
2
3 #include <pcl/filters/filter.h>
4
5 namespace pcl
6 {
7 template<typename PointT>
8 class BilateralFilter : public Filter<PointT>
9 {
10 };
11 }
2、bilateral.hpp
1 #pragma once
2
3 #include <pcl/filters/bilateral.h>
3、bilateral.cpp
1 #include <pcl/filters/bilateral.h>
2 #include <pcl/filters/impl/bilateral.hpp>
4、CMakeLists.txt
将所有文件添加到 PCL Filtering CMakeLists.txt 文件中,这样就可以启用bui