本文主要是介绍点云库(PCL)学习——Advanced Usage(二),希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!
1.写一个新的PCL class
为了说明代码转换过程,我们选择了以下示例:对给定输入点云的强度数据应用双边过滤器,并将结果保存到磁盘。
#include <pcl/point_types.h>#include <pcl/io/pcd_io.h>#include <pcl/kdtree/kdtree_flann.h>typedef pcl::PointXYZI PointT;floatG (float x, float sigma){return std::exp (- (x*x)/(2*sigma*sigma));}intmain (int argc, char *argv[]){std::string incloudfile = argv[1];std::string outcloudfile = argv[2];float sigma_s = atof (argv[3]);float sigma_r = atof (argv[4]);// Load cloudpcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);pcl::io::loadPCDFile (incloudfile.c_str (), *cloud);int pnumber = (int)cloud->size ();// Output Cloud = Input Cloudpcl::PointCloud<PointT> outcloud = *cloud;// Set up KDTreepcl::KdTreeFLANN<PointT>::Ptr tree (new pcl::KdTreeFLANN<PointT>);tree->setInputCloud (cloud);// Neighbors containersstd::vector<int> k_indices;std::vector<float> k_distances;// Main Loopfor (int point_id = 0; point_id < pnumber; ++point_id){float BF = 0;float W = 0;tree->radiusSearch (point_id, 2 * sigma_s, k_indices, k_distances);// For each neighborfor (std::size_t n_id = 0; n_id < k_indices.size (); ++n_id){float id = k_indices.at (n_id);float dist = sqrt (k_distances.at (n_id));float intensity_dist = std::abs ((*cloud)[point_id].intensity - (*cloud)[id].intensity);float w_a = G (dist, sigma_s);float w_b = G (intensity_dist, sigma_r);float weight = w_a * w_b;BF += weight * (*cloud)[id].intensity;W += weight;}outcloud[point_id].intensity = BF / W;}// Save filtered outputpcl::io::savePCDFile (outcloudfile.c_str (), outcloud);return (0);}
上述代码包括:
- I/O部分:21-27行(从磁盘读取数据),64行(将数据写入磁盘)
- 初始化部分:29-35行(通过KdTree位最近邻建立一个搜索方式)
- 实际算法部分:7-11行和37-61行
2.建立一个结构
有两种方法可以建立结构:
- 单独设置代码,作为独立的PCL类,但在PCL代码树之外;
- 直接在PCL代码树中设置文件。
因为我们假设最终结果会反馈给PCL,所以最好关注第二种,同样也因为它有一点复杂(比如,它包含了一些附加得步骤)。显然,您也可以对前一种情况重复这些步骤,但不需要在PCL树中复制文件,也不需要更高级的cmake逻辑。
假设我们希望这个新的算法成为PCL滤波库得一部分,我们要通过建立三个不同得文件:
- include/pcl/filters/bilateral.h - 包含所有得定义
- include/pcl/filters/impl/bilateral.hpp - 包含模板实现
- src/bilateral.cpp - 包含显式模板实例化
bilateral.h
如前所述,bilateral.h头文件将包含所有的关于BilateralFilter类得定义。这是一个最小框架:
#ifndef PCL_FILTERS_BILATERAL_H_#define PCL_FILTERS_BILATERAL_H_#include <pcl/filters/filter.h>namespace pcl{template<typename PointT>class BilateralFilter : public Filter<PointT>{};}#endif // PCL_FILTERS_BILATERAL_H_
bilateral.hpp
现在,让我们设置两个骨架,bilateral.hpp和bilateral.cpp,首先是bilateral.hpp:
#ifndef PCL_FILTERS_BILATERAL_IMPL_H_#define PCL_FILTERS_BILATERAL_IMPL_H_#include <pcl/filters/bilateral.h>#endif // PCL_FILTERS_BILATERAL_IMPL_H_
我们还没声明任何BilateralFilter得方法,因此还没有实现。
bilateral.cpp
#include <pcl/filters/bilateral.h>#include <pcl/filters/impl/bilateral.hpp>
因为我们在PCL中写模板化代码,模板参数是一个点类型,我们希望显式地实例化在bilateral.cpp中最常用的案例,所以在编译代码时用户不必花费额外地周期。要做到这一点,我们需要访问头文件(bilateral.h)和实现(bilateral.hpp)。
3.填写类结构
如果您正确地编辑了上面的所有文件,那么使用新的滤波器类重新编译PCL应该不会有问题。在本节中,我们将开始填充每个文件中的实际代码。让我们从bilateral.cpp文件开始,因为它的内容最短。
bilateral.cpp
#include <pcl/point_types.h>#include <pcl/filters/bilateral.h>#include <pcl/filters/impl/bilateral.hpp>template class PCL_EXPORTS pcl::BilateralFilter<pcl::PointXYZ>;template class PCL_EXPORTS pcl::BilateralFilter<pcl::PointXYZI>;template class PCL_EXPORTS pcl::BilateralFilter<pcl::PointXYZRGB>;// ...
如前所述,我们将显式实例化和预编译BilateralFilter类的许多模板化专门化。虽然这可能会增加PCL过滤库的编译时间,但当用户在编写的代码中使用类时,这将节省用户处理和编译模板的痛苦。
最简单的方法是在bilateral.cpp文件中声明我们要手动预编译的每个实例,如下所示:
#include <pcl/point_types.h>#include <pcl/filters/bilateral.h>#include <pcl/filters/impl/bilateral.hpp>template class PCL_EXPORTS pcl::BilateralFilter<pcl::PointXYZ>;template class PCL_EXPORTS pcl::BilateralFilter<pcl::PointXYZI>;template class PCL_EXPORTS pcl::BilateralFilter<pcl::PointXYZRGB>;// ...
然而,随着PCL支持的点类型数量的增加,很快就会出现问题在PCL中的多个文件中保持这个列表是最新的是非常麻烦的。因此,我们将使用一个特殊的宏PCL_INSTANTIATE并将上述代码更改如下:
#include <pcl/point_types.h>#include <pcl/impl/instantiate.hpp>#include <pcl/filters/bilateral.h>#include <pcl/filters/impl/bilateral.hpp>PCL_INSTANTIATE(BilateralFilter, PCL_XYZ_POINT_TYPES);
这个例子,将会为在point_types.h中定义的所有XYZ点类型实例化一个BilateralFilter。
通过仔细观察代码 Example: a bilateral filter ,我们注意到了存在(*cloud)[point_id].intensity这种结构,也就是说我们的滤波器存在具有强度的点类型。因此,使用PCL_XYZ_POINT_TYPES将不会产生作用,因为并非所有的类型都具有强度值。事实上,很容易注意到只有两个类型包含了强度,分别是:
:pcl:PointXYZI<pcl::PointXYZI>和 :pcl:PointXYZINormal<pcl::PointXYZINormal
更改之后最终的bilateral.cpp文件为:
#include <pcl/point_types.h>#include <pcl/impl/instantiate.hpp>#include <pcl/filters/bilateral.h>#include <pcl/filters/impl/bilateral.hpp>PCL_INSTANTIATE(BilateralFilter, (pcl::PointXYZI)(pcl::PointXYZINormal));
请注意,目前我们还没有为BilateralFilter声明PCL_INSTANTIATE实例化模板,也没有在抽象类PCL:PCL::Filter<PCL::Filter>
中实际实现纯虚函数,因此尝试编译代码将导致如下错误:
filters/src/bilateral.cpp:6:32: error: expected constructor, destructor, or type conversion before ‘(’ token
bilateral.h
我们开始通过声明构造函数以及成员变量来田中BilateralFilter类。因为双边滤波算法有两个参数,我们将这些存储为类成员,并为它们实现setters和getters以保证与PCL1.xAPI范式兼容。
...namespace pcl{template<typename PointT>class BilateralFilter : public Filter<PointT>{public:BilateralFilter () : sigma_s_ (0),sigma_r_ (std::numeric_limits<double>::max ()){}voidsetSigmaS (const double sigma_s){sigma_s_ = sigma_s;}doublegetSigmaS () const{return (sigma_s_);}voidsetSigmaR (const double sigma_r){sigma_r_ = sigma_r;}doublegetSigmaR () const{return (sigma_r_);}private:double sigma_s_;double sigma_r_;};}#endif // PCL_FILTERS_BILATERAL_H_
到现在为止还没有什么异常,除了8-9行我们将默认值赋予了这两个参数。因为我们的类从:pcl:pcl::Filter<pcl::Filter>
继承而来,而这个又是从 :pcl:pcl::PCLBase<pcl::PCLBase>
继承而来,我们可以利用:pcl:setInputCloud<pcl::PCLBase::setInputCloud>
方式将输入数据传给我们的算法(pcl:input_<pcl::PCLBase::input_>
)。因此我们添加了一个using
声明。
...template<typename PointT>class BilateralFilter : public Filter<PointT>{using Filter<PointT>::input_;public:BilateralFilter () : sigma_s_ (0),...
这会确保我们的类不需要输入整个构造函数就能够访问成员变量input_。接下来,我们观察到每个从:pcl:pcl::Filter<pcl::Filter>
继承的类必须要继承一个:pcl:applyFilter<pcl::Filter::applyFilter>
方法。因此我们定义:
...using Filter<PointT>::input_;typedef typename Filter<PointT>::PointCloud PointCloud;public:BilateralFilter () : sigma_s_ (0),sigma_r_ (std::numeric_limits<double>::max ()){}voidapplyFilter (PointCloud &output);...
applyFilter的实现将在后面的双边.hpp文件中给出。第3行构建了一个typedef,这样我们就可以使用PointCloud类型而不必输入整个构造函数。查看 Example: a bilateral filter部分的原始代码,我们注意到这个算法包含了对点云中的每个点做相同的操作。为了保持applyFilter调用整洁,我们定义了一个名为computePointWeight的方法,其实现将包含在第45-58行之间定义的语料库:
...voidapplyFilter (PointCloud &output);doublecomputePointWeight (const int pid, const std::vector<int> &indices, const std::vector<float> &distances);...
此外,我们注意到 Example: a bilateral filter 部分的第29-31行和43行为了获得一个给定点的最近邻点构建了一个:pcl:KdTree<pcl::KdTree>
结构,因此我们添加代码:
#include <pcl/kdtree/kdtree.h>...using Filter<PointT>::input_;typedef typename Filter<PointT>::PointCloud PointCloud;typedef typename pcl::KdTree<PointT>::Ptr KdTreePtr;public:...voidsetSearchMethod (const KdTreePtr &tree){tree_ = tree;}private:...KdTreePtr tree_;...
最后,我们希望在内联中添加内核方法(G(float x,float sigma)),以便加快滤波器的计算速度。因为该方法只在算法的上下文中有用,所以我们将使其私有化。头文件变成了:
#ifndef PCL_FILTERS_BILATERAL_H_#define PCL_FILTERS_BILATERAL_H_#include <pcl/filters/filter.h>#include <pcl/kdtree/kdtree.h>namespace pcl{template<typename PointT>class BilateralFilter : public Filter<PointT>{using Filter<PointT>::input_;typedef typename Filter<PointT>::PointCloud PointCloud;typedef typename pcl::KdTree<PointT>::Ptr KdTreePtr;public:BilateralFilter () : sigma_s_ (0),sigma_r_ (std::numeric_limits<double>::max ()){}voidapplyFilter (PointCloud &output);doublecomputePointWeight (const int pid, const std::vector<int> &indices, const std::vector<float> &distances);voidsetSigmaS (const double sigma_s){sigma_s_ = sigma_s;}doublegetSigmaS () const{return (sigma_s_);}voidsetSigmaR (const double sigma_r){sigma_r_ = sigma_r;}doublegetSigmaR () const{return (sigma_r_);}voidsetSearchMethod (const KdTreePtr &tree){tree_ = tree;}private:inline doublekernel (double x, double sigma){return (std::exp (- (x*x)/(2*sigma*sigma)));}double sigma_s_;double sigma_r_;KdTreePtr tree_;};}#endif // PCL_FILTERS_BILATERAL_H_
bilateral.hpp
有两个方法我们需要在这里实现,分别叫applyFilter和computePointWeight。
template <typename PointT> doublepcl::BilateralFilter<PointT>::computePointWeight (const int pid,const std::vector<int> &indices,const std::vector<float> &distances){double BF = 0, W = 0;// For each neighborfor (std::size_t n_id = 0; n_id < indices.size (); ++n_id){double id = indices[n_id];double dist = std::sqrt (distances[n_id]);double intensity_dist = std::abs ((*input_)[pid].intensity - (*input_)[id].intensity);double weight = kernel (dist, sigma_s_) * kernel (intensity_dist, sigma_r_);BF += weight * (*input_)[id].intensity;W += weight;}return (BF / W);}template <typename PointT> voidpcl::BilateralFilter<PointT>::applyFilter (PointCloud &output){tree_->setInputCloud (input_);std::vector<int> k_indices;std::vector<float> k_distances;output = *input_;for (std::size_t point_id = 0; point_id < input_->size (); ++point_id){tree_->radiusSearch (point_id, sigma_s_ * 2, k_indices, k_distances);output[point_id].intensity = computePointWeight (point_id, k_indices, k_distances);}}
computePointWeight方法应该是简单的,因为它几乎与第45-58行的部分示例:双边过滤器相同。我们基本上通过一个点索引,我们想计算强度权重,以及一组相邻点与距离。
在applyFilter中,我们首先设置树中的输入数据,将所有输入数据复制到输出中,然后继续计算新的加权点强度。
回顾填充类结构,现在是时候为类声明PCL_INSTANTIATE条目了:
#ifndef PCL_FILTERS_BILATERAL_IMPL_H_#define PCL_FILTERS_BILATERAL_IMPL_H_#include <pcl/filters/bilateral.h>...#define PCL_INSTANTIATE_BilateralFilter(T) template class PCL_EXPORTS pcl::BilateralFilter<T>;#endif // PCL_FILTERS_BILATERAL_IMPL_H_
还有一件我们可以做的事情就是错误检查:
- sigma_s_和sigma_r_两个参数是否给出;
- 搜索方法对象是否设置。
对于前者,我们将检查sigma_s_的值,默认值应被设置为0,这对算法的表现有关键作用。因此,如果在代码执行时,它的值仍未0,我们将使用宏pcl:PCL_ERROR<PCL_ERROR>
打印一个错误并返回。
在搜索方法中,我们可以使用同样的做法,或者给用户提供一个默认选项。最好的默认选项是:
- 如果点云是有序的,则使用有序搜索方法
pcl:pcl::OriganizedNeighbor<pcl::OrganizedNeighbor>
- 如果点云是无序的,则使用通用kdtree
pcl:pcl::KdTreeFLANN<pcl::KdTreeFLANN>
。
#include <pcl/kdtree/kdtree_flann.h>#include <pcl/kdtree/organized_data.h>...template <typename PointT> voidpcl::BilateralFilter<PointT>::applyFilter (PointCloud &output){if (sigma_s_ == 0){PCL_ERROR ("[pcl::BilateralFilter::applyFilter] Need a sigma_s value given before continuing.\n");return;}if (!tree_){if (input_->isOrganized ())tree_.reset (new pcl::OrganizedNeighbor<PointT> ());elsetree_.reset (new pcl::KdTreeFLANN<PointT> (false));}tree_->setInputCloud (input_);...
实现头文件则变成:
#ifndef PCL_FILTERS_BILATERAL_IMPL_H_#define PCL_FILTERS_BILATERAL_IMPL_H_#include <pcl/filters/bilateral.h>#include <pcl/kdtree/kdtree_flann.h>#include <pcl/kdtree/organized_data.h>template <typename PointT> doublepcl::BilateralFilter<PointT>::computePointWeight (const int pid,const std::vector<int> &indices,const std::vector<float> &distances){double BF = 0, W = 0;// For each neighborfor (std::size_t n_id = 0; n_id < indices.size (); ++n_id){double id = indices[n_id];double dist = std::sqrt (distances[n_id]);double intensity_dist = std::abs ((*input_)[pid].intensity - (*input_)[id].intensity);double weight = kernel (dist, sigma_s_) * kernel (intensity_dist, sigma_r_);BF += weight * (*input_)[id].intensity;W += weight;}return (BF / W);}template <typename PointT> voidpcl::BilateralFilter<PointT>::applyFilter (PointCloud &output){if (sigma_s_ == 0){PCL_ERROR ("[pcl::BilateralFilter::applyFilter] Need a sigma_s value given before continuing.\n");return;}if (!tree_){if (input_->isOrganized ())tree_.reset (new pcl::OrganizedNeighbor<PointT> ());elsetree_.reset (new pcl::KdTreeFLANN<PointT> (false));}tree_->setInputCloud (input_);std::vector<int> k_indices;std::vector<float> k_distances;output = *input_;for (std::size_t point_id = 0; point_id < input_->size (); ++point_id){tree_->radiusSearch (point_id, sigma_s_ * 2, k_indices, k_distances);output[point_id].intensity = computePointWeight (point_id, k_indices, k_distances);}}#define PCL_INSTANTIATE_BilateralFilter(T) template class PCL_EXPORTS pcl::BilateralFilter<T>;#endif // PCL_FILTERS_BILATERAL_IMPL_H_
4.利用其他PCL概念
Point indices
将点云数据传递到PCL算法的标准方法是通过pcl:setInputCloud<pcl::PCLBase::setInputCloud>
调用的。除此之外,PCL还定义了一种方式来定义一个算法可以操作的感兴趣区域/点索引列表,而不需要在整个点云上操作,pcl:setIndices<pcl::PCLBase::setIndices>
。
所有继承自pcl::PCLBase<pcl::PCLBase>
的类将遵从下面的方式:在没有用户设置索引集的情况下,将创建一个伪索引,于算法期间使用。这意味着我们可以很容易地将上面的实现代码更改为在<cloud,indexs>元组上操作,这样做还有一个额外的优点,即如果用户确实传递了一组索引,那么将只使用那些索引,如果不传递,那么将使用整个点云。
新的bilateral.hpp将变成:
#include <pcl/kdtree/kdtree_flann.h>#include <pcl/kdtree/organized_data.h>...template <typename PointT> voidpcl::BilateralFilter<PointT>::applyFilter (PointCloud &output){if (sigma_s_ == 0){PCL_ERROR ("[pcl::BilateralFilter::applyFilter] Need a sigma_s value given before continuing.\n");return;}if (!tree_){if (input_->isOrganized ())tree_.reset (new pcl::OrganizedNeighbor<PointT> ());elsetree_.reset (new pcl::KdTreeFLANN<PointT> (false));}tree_->setInputCloud (input_);...
头文件的实现因此变成:
#ifndef PCL_FILTERS_BILATERAL_IMPL_H_#define PCL_FILTERS_BILATERAL_IMPL_H_#include <pcl/filters/bilateral.h>#include <pcl/kdtree/kdtree_flann.h>#include <pcl/kdtree/organized_data.h>template <typename PointT> doublepcl::BilateralFilter<PointT>::computePointWeight (const int pid,const std::vector<int> &indices,const std::vector<float> &distances){double BF = 0, W = 0;// For each neighborfor (std::size_t n_id = 0; n_id < indices.size (); ++n_id){double id = indices[n_id];double dist = std::sqrt (distances[n_id]);double intensity_dist = std::abs ((*input_)[pid].intensity - (*input_)[id].intensity);double weight = kernel (dist, sigma_s_) * kernel (intensity_dist, sigma_r_);BF += weight * (*input_)[id].intensity;W += weight;}return (BF / W);}template <typename PointT> voidpcl::BilateralFilter<PointT>::applyFilter (PointCloud &output){if (sigma_s_ == 0){PCL_ERROR ("[pcl::BilateralFilter::applyFilter] Need a sigma_s value given before continuing.\n");return;}if (!tree_){if (input_->isOrganized ())tree_.reset (new pcl::OrganizedNeighbor<PointT> ());elsetree_.reset (new pcl::KdTreeFLANN<PointT> (false));}tree_->setInputCloud (input_);std::vector<int> k_indices;std::vector<float> k_distances;output = *input_;for (std::size_t i = 0; i < indices_->size (); ++i){tree_->radiusSearch ((*indices_)[i], sigma_s_ * 2, k_indices, k_distances);output[(*indices_)[i]].intensity = computePointWeight ((*indices_)[i], k_indices, k_distances);}}#define PCL_INSTANTIATE_BilateralFilter(T) template class PCL_EXPORTS pcl::BilateralFilter<T>;#endif // PCL_FILTERS_BILATERAL_IMPL_H_
为了不需输入整个结构也能使pcl:indices_<pcl::PCLBase::indices_>
工作,我们需要在bilateral.h添加一行新代码明确这个类,其中indices_声明为:
...template<typename PointT>class BilateralFilter : public Filter<PointT>{using Filter<PointT>::input_;using Filter<PointT>::indices_;public:BilateralFilter () : sigma_s_ (0),...
这篇关于点云库(PCL)学习——Advanced Usage(二)的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!