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>
- 如果点云是无序的,则使用通用kdtree
pcl: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),
...