点云库（PCL）学习——Advanced Usage（二）

最新推荐文章于 2024-06-24 10:00:33 发布

Michael_Wang9898

最新推荐文章于 2024-06-24 10:00:33 发布

阅读量284

点赞数 1

分类专栏：三维点云

原文链接：https://pcl.readthedocs.io/projects/tutorials/en/latest/writing_new_classes.html#filling-in-the-class-structure

版权

三维点云专栏收录该内容

11 篇文章 0 订阅

订阅专栏

1.写一个新的PCL class

为了说明代码转换过程，我们选择了以下示例：对给定输入点云的强度数据应用双边过滤器，并将结果保存到磁盘。

 #include <pcl/point_types.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/kdtree/kdtree_flann.h>

 typedef pcl::PointXYZI PointT;

 float
 G (float x, float sigma)
 {
   return std::exp (- (x*x)/(2*sigma*sigma));
 }

 int
 main (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 cloud
   pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
   pcl::io::loadPCDFile (incloudfile.c_str (), *cloud);
   int pnumber = (int)cloud->size ();

   // Output Cloud = Input Cloud
   pcl::PointCloud<PointT> outcloud = *cloud;

   // Set up KDTree
   pcl::KdTreeFLANN<PointT>::Ptr tree (new pcl::KdTreeFLANN<PointT>);
   tree->setInputCloud (cloud);

   // Neighbors containers
   std::vector<int> k_indices;
   std::vector<float> k_distances;

   // Main Loop
   for (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 neighbor
     for (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 output
   pcl::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 ())
       {
       }

       void
       setSigmaS (const double sigma_s)
       {
         sigma_s_ = sigma_s;
       }

       double
       getSigmaS () const
       {
         return (sigma_s_);
       }

       void
       setSigmaR (const double sigma_r)
       {
         sigma_r_ = sigma_r;
       }

       double
       getSigmaR () 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 ())
       {
       }

       void
       applyFilter (PointCloud &output);
 ...

applyFilter的实现将在后面的双边.hpp文件中给出。第3行构建了一个typedef，这样我们就可以使用PointCloud类型而不必输入整个构造函数。查看 Example: a bilateral filter部分的原始代码，我们注意到这个算法包含了对点云中的每个点做相同的操作。为了保持applyFilter调用整洁，我们定义了一个名为computePointWeight的方法，其实现将包含在第45-58行之间定义的语料库：

...
       void
       applyFilter (PointCloud &output);

       double
       computePointWeight (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:
 ...

     void
     setSearchMethod (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 ())
       {
       }


       void
       applyFilter (PointCloud &output);

       double
       computePointWeight (const int pid, const std::vector<int> &indices, const std::vector<float> &distances);

       void
       setSigmaS (const double sigma_s)
       {
         sigma_s_ = sigma_s;
       }

       double
       getSigmaS () const
       {
         return (sigma_s_);
       }

       void
       setSigmaR (const double sigma_r)
       {
         sigma_r_ = sigma_r;
       }

       double
       getSigmaR () const
       {
         return (sigma_r_);
       }

       void
       setSearchMethod (const KdTreePtr &tree)
       {
         tree_ = tree;
       }


     private:

       inline double
       kernel (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> double
 pcl::BilateralFilter<PointT>::computePointWeight (const int pid,
                                                   const std::vector<int> &indices,
                                                   const std::vector<float> &distances)
 {
   double BF = 0, W = 0;

   // For each neighbor
   for (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> void
 pcl::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>
如果点云是无序的，则使用通用kdtreepcl:pcl::KdTreeFLANN<pcl::KdTreeFLANN>。

 #include <pcl/kdtree/kdtree_flann.h>
 #include <pcl/kdtree/organized_data.h>

 ...
 template <typename PointT> void
 pcl::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> ());
     else
       tree_.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> double
 pcl::BilateralFilter<PointT>::computePointWeight (const int pid,
                                                   const std::vector<int> &indices,
                                                   const std::vector<float> &distances)
 {
   double BF = 0, W = 0;

   // For each neighbor
   for (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> void
 pcl::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> ());
     else
       tree_.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> void
 pcl::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> ());
     else
       tree_.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> double
 pcl::BilateralFilter<PointT>::computePointWeight (const int pid,
                                                   const std::vector<int> &indices,
                                                   const std::vector<float> &distances)
 {
   double BF = 0, W = 0;

   // For each neighbor
   for (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> void
 pcl::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> ());
     else
       tree_.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),
 ...

Michael_Wang9898

关注

1
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
点云库（PCL）学习——Advanced Usage（二）

写一个新的PCL class为了说明代码转换过程，我们选择了以下示例：对给定输入点云的强度数据应用双边过滤器，并将结果保存到磁盘。 #include <pcl/point_types.h> #include <pcl/io/pcd_io.h> #include <pcl/kdtree/kdtree_flann.h> typedef pcl::PointXYZI PointT; float G (float x, float sigma) { re
复制链接

扫一扫

专栏目录