PCL中点云配准精通级实例解析
PCL中点云配准精通级实例解析
- 1 如何将扫描数据与模板对象进行配准
- 2 基于VFH描述子聚类识别与位姿估计
-
- 2.1 训练阶段
-
- 2.1.1 建立树结构源代码
- 2.1.2 树结构建立程序解释说明
- 2.2 测试阶段
-
- 2.2.1 近邻搜索源代码
- 2.2.2 近邻搜索代码解释说明
- 2.3 编译运行程序
1 如何将扫描数据与模板对象进行配准
本小节综合利于PCL中多个模块，介绍如何对齐已有的模型与一个获取的深度图像数据，在这个例子中，利用包含一个人的深度图像场景作为目标，与获取的脸部模板数据进行对齐配准，通过模板匹配可以估计脸部在场景中的位置与姿态。
首先创建一个工作空间template_alignment，然后再在工作空间创建一个文件夹src用于存放源代码：
mkdir -p template_alignment/src
接着，在template_alignment/src路径下，创建一个文件并命名为template_alignment.cpp，拷贝如下代码：
#include 【解释说明】
接下来分析上述源代码中的关键语句。我们将从 FeatureCloud 类开始分析，定义这个类是为了方便计算和储存每个点云数据和对应的特征描述子，下面代码的构造函数创建了一个新的KdTreeFLANN 对象并且初始化半径参数，这些参数将会被用于计算曲面法线和特征估计。
FeatureCloud () : search_method_xyz_ (new SearchMethod),
normal_radius_ (0.02f),
feature_radius_ (0.02f)
{
}
然后我们定义设置输入点云的方法，通过一个指向点云的共享指针或PCD文件名称作为输入，实现将对应的点云数据存储为该类的对象，设置输入点云数据后，函数processInput将计算法线数据与对应的特征描述子。
/* 从输入点云指针传入对象点云数据并处理输入 */
void setInputCloud (PointCloud::Ptr xyz)
{
xyz_ = xyz;
processInput();
}
/* 加载给定pcd文件名的点云数据到对象并处理输入 */
void loadInputCloud (const std::string &pcd_file)
{
xyz_ = PointCloud::Ptr (new PointCloud);
pcl::io::loadPCDFile (pcd_file, *xyz_);
processInput ();
}
定义一些存取的方法用来得到点云的共享指针、曲面法线和局部特征描述子。
/* 获取指向点云的指针 */
PointCloud::Ptr getPointCloud() const
{
return (xyz_);
}
/* 获取指向点云法线的指针 */
SurfaceNormals::Ptr getSurfaceNormals() const
{
return (normals_);
}
/* 获取指向点云特征描述子的指针 */
LocalFeatures::Ptr getLocalFeatures () const
{
return (features_);
}
接着我们定义一个处理输入点云的方法，首先计算点云的曲面法线然后估计曲面的特征描述子。
/* 计算点云对应的法线与特征描述子 */
void processInput ()
{
computeSurfaceNormals ();
computeLocalFeatures ();
}
我们用PCL的 NormalEstimation 类来计算曲面法线，首先，必须指定输入点云，在寻找近邻点的时候将用到 kdtree 进行高效搜索，在此需要用到在构造函数中设定的近邻点的搜索半径，然后计算曲面法线并且存储这个变量作为对象的数据。
/* 计算点云的法线数据 */
void computeSurfaceNormals ()
{
normals_ = SurfaceNormals::Ptr (new SurfaceNormals);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> norm_est;
norm_est.setInputCloud (xyz_);
norm_est.setSearchMethod (search_method_xyz_);
norm_est.setRadiusSearch (normal_radius_);
norm_est.compute (*normals_);
}
相似的，我们可以用PCL的 FPFHEstimation 类通过输入的点云和它的曲面法线来计算FPFH描述子。
/* 计算FPFH特征描述子 */
void computeLocalFeatures ()
{
features_ = LocalFeatures::Ptr (new LocalFeatures);
pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_est;
fpfh_est.setInputCloud (xyz_);
fpfh_est.setInputNormals (normals_);
fpfh_est.setSearchMethod (search_method_xyz_);
fpfh_est.setRadiusSearch (feature_radius_);
fpfh_est.compute (*features_);
}
上述的类封装了计算特征描述子和储存特征描述子以及相对应的点云数据需要的工作。
现在将解析 TemplateAlignment 类，顾名思义，这个类被用于执行模板对齐（也可以是模板拟合/匹配/配准），一个模板定义为大场景下一个典型的小的像素集或点集，通过对齐一个模板到一个新的影像或点云，可以决定模板所代表的目标对象的位置和方向。
首先从定义一个用来存储配准结果的结构体开始，它包含有三个成员：第一个为一个浮点数变量，该变量代表配准效果（值越低表示配准的越准确）；第二个是一个变换矩阵，该变换矩阵是描述模板点集应该如何旋转和平移以至于更好的跟目标点云中的点集进行配准；第三个成员为一个宏，因为我们在这个结构体中使用到一个 Eigen::Matrix4f 结构，所以需要添加EIGEN_MAKE_ALIGNED_OPERATOR_NEW宏，这将重载 Eigen::Matrix4f 结构的new运算符，使其产生十六字节对齐的指针。
/* 保存配准结果的结构体 */
struct Result
{
float fitness_score;
Eigen::Matrix4f final_transformation;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
这个构造函数中，我们初始化 SampleConsensusInitialAlignment（SAC-IA）这个对象，该对象用来执行配准计算，给它的每个参数赋值。注意对应点的距离阈值事实上被指定为是距离的平方，在本例中，我们决定将距离阈值设定为0.01m，因此代码如下。
TemplateAlignment () : min_sample_distance_ (0.05f),
max_correspondence_distance_ (0.01f*0.01f),
nr_iterations_ (500)
{
sac_ia_.setMinSampleDistance (min_sample_distance_);
sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_);
sac_ia_.setMaximumIterations (nr_iterations_);
}
接下来我们定义一个设置目标点云的方法，该目标点云正是模板点云需要对齐的数据集，即设置SAC-IA算法的输入点云数据。
/* 设置模板点云需要对齐的目标点云数据 */
void setTargetCloud (FeatureCloud &target_cloud)
{
target_ = target_cloud;
sac_ia_.setInputTarget (target_cloud.getPointCloud ());
sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ());
}
然后，我们定义一个方法来指定哪些模板点云将要被配准，通过该方法将模板点云 FeatureClouds 的对象存储到 TemplateAlignment 对象的模板序列中。
/* 添加给定的点云到需要匹配的模板序列中 */
void addTemplateCloud (FeatureCloud &template_cloud)
{
templates_.push_back (template_cloud);
}
接着我们定义配准方法，这个方法以模板点云为输入，并且将它跟已经通过函数setInputTarget ()指定的目标云进行匹配，align()方法内部调用SAC-IA算法实现输入点云与目标点云的配准，虽然内部配准后可以得到变换后的模板点云数据，但这里忽略该数据，相反，此处需要SAC-IA方法得到的配准拟合系数和最终的变换矩阵（从源点云到目标点云的刚体变换矩阵），并且我们将它们存储到 Result 结构体中输出给调用函数。
/* 配准模板点云与setTargetCloud()设定的目标点云 */
void align (FeatureCloud &template_cloud, TemplateAlignment::Result &result)
{
sac_ia_.setInputCloud (template_cloud.getPointCloud ());
sac_ia_.setSourceFeatures (template_cloud.getLocalFeatures ());
pcl::PointCloud<pcl::PointXYZ> registration_output;
sac_ia_.align (registration_output);
result.fitness_score = (float) sac_ia_.getFitnessScore (max_correspondence_distance_);
result.final_transformation = sac_ia_.getFinalTransformation ();
}
因为这个类被设计为可以和多个模板进行配准，这里也定义一个方法即将所有的模板与目标云进行配准并且将结果保存成 Result 的一个向量返回。
/* 配准所有的模板点云与setTargetCloud()设定的目标点云 */
void alignAll (std::vector<TemplateAlignment::Result, Eigen::aligned_allocator<Result> > &results)
{
results.resize (templates_.size ());
for (size_t i = 0; i < templates_.size (); ++i)
{
align (templates_[i], results[i]);
}
}
最后，我们定义一个方法将所有的模板点云与目标点云进行配准并且返回其最佳的拟合
模板点云索引与相应的拟合系数。
/* 配准所有模板与目标点云获取最拟合的模板 */
int findBestAlignment (TemplateAlignment::Result &result)
{
std::vector<Result, Eigen::aligned_allocator<Result> > results;
alignAll (results);
/* 获取最佳匹配的模板，即具有最小拟合系数的点云 */
float lowest_score = std::numeric_limits<float>::infinity ();
int best_template = 0;
for (size_t i = 0; i < results.size (); ++i)
{
const Result &r = results[i];
if (r.fitness_score < lowest_score)
{
lowest_score = r.fitness_score;
best_template = (int) i;
}
}
/* 输出最佳匹配模板 */
result = results[best_template];
return (best_template);
}
现在有了一个处理配准模板与目标的类，把它应用到脸部配准问题上，在提供的数据文件中，已经包含了从不同角度观察人脸所创建的六个不同视角的点云模板，每个模板数据是经过下采样处理并且手工将非脸部位裁剪掉后得到的。在下列代码中演示了如何用 TemplateAlignment 类来查找在目标点云中人脸的位姿。
首先加载对象的模板点云，已经将所有的模板点云保存为.pcd文件，并且已经在object_templates.txt文件中列出了所有模板文件的名字。这里，读入每个文件的名字，加载到FeatureCloud，并且将 FeatureCloud 存储成矢量在后面使用。
/* 从object_templates.txt加载模板点云文件 */
std::vector<FeatureCloud> object_templates;
std::ifstream input_stream (argv[1]);
object_templates.resize (0);
std::string pcd_filename;
while (input_stream.good ())
{
std::getline (input_stream, pcd_filename);
if (pcd_filename.empty () || pcd_filename.at (0) == '#') //跳过空行或注释（#开头的行）
{
continue;
}
FeatureCloud template_cloud;
template_cloud.loadInputCloud (pcd_filename);
object_templates.push_back (template_cloud);
}
input_stream.close ();
接下来加载目标点云。
/* 获取目标点云 */
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile (argv[2], *cloud);
然后对数据进行一些预处理以便为配准做准备，第一步就是过滤掉一些噪声点和冗余数据，在这个例子中，假设这个我们试图配准的人在距离不到1米处，所以就应用一个直通的过滤器，过滤领域的“z”（即，深度）与范围为0~1。
/* 预处理点云——删除距离较远的冗余点 */
const float depth_limit = 1.0;
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, depth_limit);
pass.filter (*cloud);
下面是利用5cm的体素栅格对目标点云进行下采样，这样可以减少运算量。
/* 预处理点云——对点云进行下采样 */
const float voxel_grid_size = 0.005f;
pcl::VoxelGrid<pcl::PointXYZ> vox_grid;
vox_grid.setInputCloud (cloud);
vox_grid.setLeafSize (voxel_grid_size, voxel_grid_size, voxel_grid_size);
vox_grid.filter (*cloud);
在预处理结束后，创建 FeatureCloud 目标对象。
/* 设置目标点云为FeatureCloud对象输入点云 */
FeatureCloud target_cloud;
target_cloud.setInputCloud (cloud);
接下来，初始化 TemplateAlignment 对象，这一步需要添加每个点云模板并且设置目标点云。
/* 设置TemplateAlignment输入点云 */
TemplateAlignment template_align;
for (size_t i = 0; i < object_templates.size (); ++i)
{
template_align.addTemplateCloud (object_templates[i]);
}
template_align.setTargetCloud (target_cloud);
现在TemplateAlignment 目标对象被初始化了，准备用 findBestAlignment 方法来决定哪个模板是给出目标点云的最佳匹配模板点云，将配准结果保存到 best_alignment。
/* 寻找最佳匹配模板点云 */
TemplateAlignment::Result best_alignment;
int best_index = template_align.findBestAlignment (best_alignment);
const FeatureCloud &best_template = object_templates[best_index];
接着打印输出结果，看一下拟合系数（best_alignment.fitness_score）就可以知道配准是否成功有效，并且打印出变换矩阵（best_alignment.final_transformation），从而知道配准的模板在目标点云中的位姿。特别之处在于，因为这是一个刚体变换矩阵，所以可被分解为一个平移向量（ t x , t y , t z t_{x},t_{y},t_{z} tx,ty,tz）和一个3x3旋转矩阵 R R R。
T = [ t x R t y t z 0 0 0 1 ] T=\begin{bmatrix} & & & t_{x}\\ & R & & t_{y}\\ & & & t_{z}\\ 0 & 0 & 0 & 1 \end{bmatrix} T=⎣⎢⎢⎡0R00txtytz1⎦⎥⎥⎤
/* 打印出拟合系数，小于0.00002较好 */
printf ("Best fitness score: %f\n", best_alignment.fitness_score);
/* 打印出旋转矩阵和平移向量 */
Eigen::Matrix3f rotation = best_alignment.final_transformation.block<3,3>(0, 0);
Eigen::Vector3f translation = best_alignment.final_transformation.block<3,1>(0, 3);
printf ("\n");
printf (" | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
printf ("R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
printf (" | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
printf ("\n");
printf ("t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));
最后，我们可以采用最佳匹配模板，应用变换将其与目标云进行配准，并且将配准后的模板点云保存成.pcd文档以便于后续可视化观察配准后的效果。
/* 保存配准后的模板 */
pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
pcl::transformPointCloud (*best_template.getPointCloud (), transformed_cloud, best_alignment.final_transformation);
pcl::io::savePCDFileBinary ("output.pcd", transformed_cloud);
【编译和运行程序】
在工作空间根目录template_alignment下，编写CMakeLists.txt文件如下：
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(template_alignment)
find_package(PCL 1.2 REQUIRED)
include_directories(${
PCL_INCLUDE_DIRS})
link_directories(${
PCL_LIBRARY_DIRS})
add_definitions(${
PCL_DEFINITIONS})
add_executable (${
PROJECT_NAME}_node src/template_alignment.cpp)
target_link_libraries (${
PROJECT_NAME}_node ${
PCL_LIBRARIES})
在工作空间根目录template_alignment下创建一个build文件夹，用于存放编译过程中产生的文件，然后执行编译：
mkdir build
cd build
cmake ..
make
此时，会在build文件夹下生成一个可执行文件template_alignment_node，运行该可执行文件：
./template_alignment_node data/object_templates.txt data/person.pcd
运行上述命令后，将看到类似于如下所示的输出信息，包括最终找到的模型的匹配系数以及对应的旋转和平移矩阵，利用该变换对匹配最好的模型进行变换后，存储点云到当前目录下，生成output.pcd文件。
best_index: 5
Best fitness score: 0.000011
| 0.803 0.402 0.440 |
R = | -0.438 0.899 -0.022 |
| -0.404 -0.175 0.898 |
t = < -0.364, -0.079, 0.084 >
同时，在可视化窗口中可以看到如下信息。
将目标点云与模板对象配准后的结果
在上图中，6个白色的点云模型为导入的模板对象，蓝色点云模型为目标点云，红色点云模型为配准后的点云。
2 基于VFH描述子聚类识别与位姿估计
我在之前的教程中非常详细的总结了特征描述与提取，感兴趣的朋友可以查看之前的文章。其中，VFH是非常强大的描述子之一，其主要用于识别和估计点集的聚类，这里的聚类是指3D点的集合，常常代表着一个特殊目标对象或场景的一部分，由分割或提取算法获得。
本节的目标不是提供一个最终的识别工具，而是根据各个聚类的场景及其对应的6自由度位姿，从一组聚类中检索查询进而选取与用户输入的场景近似的候选集，这样就可以将识别问题抽象为一个近邻估计问题。给定一组训练数据，我们将使用一种有效的近邻搜索结构，例如 kdtree，来查找返回一组相似对象，将这些相似对象与查找对象之间的距离来进行排序，很明显这样的程序要比直接返回是否存在用户检索的场景的程序要更有用处。
为了更好地解释整个程序，这个应用例子划分为两部分。
（1）训练阶段
给定一个只包含一个物体的场景，这样方便后面聚类的得到。
利用一个准确的位姿记录系统获取位姿。
∙ \bullet ∙ 相对采集设备旋转物体，并为获取的不同视角的点云计算VFH描述子。
∙ \bullet ∙ 保存不同视角的点云，并基于此建立kdtree。
（2）测试阶段
∙ \bullet ∙ 从给定的场景中分割提取出聚类。
∙ \bullet ∙ 对于每个聚类，从当前的视角计算其VFH描述子。
∙ \bullet ∙ 利用VFH描述子在上面建立的kdtree进行搜索，查找候选集。
简单说，首先建立一个组目标对象集合用于后面的识别，最后利用程序实现基于场景对候选对象的检索。
下图所示是一个典型的真值测量系统，可以将物体放在平板上，移动、旋转、倾斜平板，从而得到其物体对应的准确位姿。另一个比较廉价的真值测量系统是基于标记点，例如棋盘格等，将物体和棋盘格等参照系统放在一起，然后通过人工旋转摄像头或摆放物体载体，最后通过标定算法计算获取位姿信息。
位姿真值测量系统
本小节中所涉及的 kdtree 实现，选用基于 FLANN 的格式，目前在速度各方面其都优于其他实现。
2.1 训练阶段
开始训练时假设所有的对象都已被分为独立的聚类，如下图所示，左边为原始点云数据，右边为聚类分割后的结果，杯子和桌子为两个不同的聚类。
聚类分割示意图
在本节中，主要是想详细解释基于VFH描述子的训练和测试，所以提供收集好的一组数据，相关的数据集可以在此链接进行下载，数据如下图所示。
不同物体对应的点云场景
2.1.1 建立树结构源代码
首先创建一个工作空间cluster recognition，然后再在工作空间创建一个文件夹src用于存放源代码：
mkdir -p cluster recognition/src
接着，在cluster recognition/src路径下，创建一个文件并命名为build_tree.cpp，拷贝如下代码：
#include > index (data, flann::KDTreeIndexParams (4));
index.buildIndex ();
index.save (kdtree_idx_file_name);
delete[] data.ptr ();
return (0);
}
2.1.2 树结构建立程序解释说明
接下来分析上述源代码中的关键语句。从main()函数开始，首先加载一组模型特征数据，loadFeatureModels 方法循环遍历从命令行获取的路径，从而加载路径下所有的pcd文件。
/* 加载一组模型特征数据 */
loadFeatureModels (argv[1], extension, models);
pcl::console::print_highlight ("Loaded %d VFH models. Creating training data %s/%s.\n",
(int)models.size (), training_data_h5_file_name.c_str (),
training_data_list_file_name.c_str ());
void loadFeatureModels (const boost::filesystem::path &base_dir, const std::string &extension, std::vector<vfh_model> &models)
{
if (!boost::filesystem::exists (base_dir) && !boost::filesystem::is_directory (base_dir))
return;
for (boost::filesystem::directory_iterator it (base_dir); it != boost::filesystem::directory_iterator (); ++it)
{
if (boost::filesystem::is_directory (it->status ()))
{
std::stringstream ss;
ss << it->path ();
pcl::console::print_highlight ("Loading %s (%lu models loaded so far).\n", ss.str ().c_str (), (unsigned long)models.size ());
loadFeatureModels (it->path (), extension, models);
}
if (boost::filesystem::is_regular_file (it->status ()) && boost::filesystem::extension (it->path ()) == extension)
{
vfh_model m;
if (loadHist (base_dir / it->path ().filename (), m))
models.push_back (m);
}
}
}
在 loadFeatureModels 内部，调用loadHist，该函数用于打开遍历路径得到的每个pcd文件，并读取其文件头，检查是否包含VFH标志位。如果有VFH标志位，则同时存储打开的数据和文件到vfh_model模型特征数据存储变量，该变量用一个字符串存储文件名，用一个向量存储文件对应的VFH特征向量。
bool loadHist (const boost::filesystem::path &path, vfh_model &vfh)
{
int vfh_idx;
// Load the file as a PCD
try
{
pcl::PCLPointCloud2 cloud;
int version;
Eigen::Vector4f origin;
Eigen::Quaternionf orientation;
pcl::PCDReader r;
int type; unsigned int idx;
r.readHeader (path.string (), cloud, origin, orientation, version, type, idx);
vfh_idx = pcl::getFieldIndex (cloud, "vfh");
if (vfh_idx == -1)
return (false);
if ((int)cloud.width * cloud.height != 1)
return (false);
}
catch (pcl::InvalidConversionException e)
{
return (false);
}
// Treat the VFH signature as a single Point Cloud
pcl::PointCloud <pcl::VFHSignature308> point;
pcl::io::loadPCDFile (path.string (), point);
vfh.second.resize (308);
std::vector <pcl::PCLPointField> fields;
pcl::getFieldIndex (point, "vfh", fields);
for (size_t i = 0; i < fields[vfh_idx].count; ++i)
{
vfh.second[i] = point.points[0].histogram[i];
}
vfh.first = path.string ();
return (true);
}
当所有的模型VFH特征文件加载完成后，将它们转换为 FLANN 的格式，
/* 转化数据为FLANN格式 */
flann::Matrix<float> data (new float[models.size () * models[0].second.size ()], models.size (), models[0].second.size ());
for (size_t i = 0; i < data.rows; ++i)
{
for (size_t j = 0; j < data.cols; ++j)
{
data[i][j] = models[i].second[j];
}
}
为了在测试阶段方便，不需要重新加载所有的模型特征数据，我们存储转化后的数据，方便在测试时使用。
/* 保存数据 */
flann::save_to_file (data, training_data_h5_file_name, "training_data");
std::ofstream fs;
fs.open (training_data_list_file_name.c_str ());
for (size_t i = 0; i < models.size (); ++i)
{
fs << models[i].first << "\n";
}
fs.close ();
最后，我们利用转化的数据创建kdtree ，并将其结构保存，以备后面使用。
/* 创建kdtree，并保存结果 */
pcl::console::print_error ("Building the kdtree index (%s) for %d elements...\n", kdtree_idx_file_name.c_str (), (int)data.rows);
flann::Index<flann::ChiSquareDistance<float> > index (data, flann::LinearIndexParams ());
//flann::Index > index (data, flann::KDTreeIndexParams (4));
index.buildIndex ();
index.save (kdtree_idx_file_name);
在这里我们利用 LinearIndex，使用Chi平方距离度量进行朴素模式匹配搜索（brute-forcesearch），可以通过注释掉第一行，取消注释第二行，来利用 KDTreelndex建立kdtree。 其实Linearlndex 和FLANN中的 KDTreelndex 两者最重要的区别是，利用 KDTreelndex 进行搜索匹要快得多，同时产生近似的结果，而不是绝对的结果。
此刻，就完成了训练阶段，总结为以下步骤。
（1）遍历给定的目录，查看所有pcd文件，测试它们是否有VFH标志位，并加载到内存中。
（2）转化数据为FLANN格式并存储。
（3）利用FLANN数据建立kdtree结构，并将其保存。
2.2 测试阶段
在测试阶段，我们将演示随机加载一个来自于训练阶段的模型VFH特征文件（用户可
以提供自己获取的模型VFH特征文件），从建立的树结构中检索相似的对象，并进行可视化显示。
2.2.1 近邻搜索源代码
在cluster recognition/src路径下，创建一个文件并命名为nearest_neighbors.cpp，拷贝如下代码：
#include [options] {kdtree.idx} {training_data.h5} {training_data.list}\n" , argv[0]);
pcl::console::print_info (" where [options] are: -k = number of nearest neighbors to search for in the tree (default: ");
pcl::console::print_value ("%d", k); pcl::console::print_info (")\n");
pcl::console::print_info (" -thresh = maximum distance threshold for a model to be considered VALID (default: ");
pcl::console::print_value ("%f", thresh); pcl::console::print_info (")\n\n");
return (-1);
}
std::string extension (".pcd");
transform (extension.begin (), extension.end (), extension.begin (), (int(*)(int))tolower);
// Load the test histogram
std::vector<int> pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
vfh_model histogram;
if (!loadHist (argv[pcd_indices.at (0)], histogram))
{
pcl::console::print_error ("Cannot load test file %s\n", argv[pcd_indices.at (0)]);
return (-1);
}
pcl::console::parse_argument (argc, argv, "-thresh", thresh);
// Search for the k closest matches
pcl::console::parse_argument (argc, argv, "-k", k);
pcl::console::print_highlight ("Using "); pcl::console::print_value ("%d", k); pcl::console::print_info (" nearest neighbors.\n");
std::string kdtree_idx_file_name = "kdtree.idx";
std::string training_data_h5_file_name = "training_data.h5";
std::string training_data_list_file_name = "training_data.list";
std::vector<vfh_model> models;
flann::Matrix<int> k_indices;
flann::Matrix<float> k_distances;
flann::Matrix<float> data;
// Check if the data has already been saved to disk
if (!boost::filesystem::exists ("training_data.h5") || !boost::filesystem::exists ("training_data.list"))
{
pcl::console::print_error ("Could not find training data models files %s and %s!\n",
training_data_h5_file_name.c_str (),
training_data_list_file_name.c_str ());
return (-1);
}
else
{
loadFileList (models, training_data_list_file_name);
flann::load_from_file (data, training_data_h5_file_name, "training_data");
pcl::console::print_highlight ("Training data found. Loaded %d VFH models from %s/%s.\n",
(int)data.rows, training_data_h5_file_name.c_str (),
training_data_list_file_name.c_str ());
}
// Check if the tree index has already been saved to disk
if (!boost::filesystem::exists (kdtree_idx_file_name))
{
pcl::console::print_error ("Could not find kd-tree index in file %s!", kdtree_idx_file_name.c_str ());
return (-1);
}
else
{
flann::Index<flann::ChiSquareDistance<float> > index (data, flann::SavedIndexParams ("kdtree.idx"));
index.buildIndex ();
nearestKSearch (index, histogram, k, k_indices, k_distances);
}
// Output the results on screen
pcl::console::print_highlight ("The closest %d neighbors for %s are:\n", k, argv[pcd_indices[0]]);
for (int i = 0; i < k; ++i)
{
pcl::console::print_info (" %d - %s (%d) with a distance of: %f\n",
i, models.at (k_indices[0][i]).first.c_str (),
k_indices[0][i],
k_distances[0][i]);
}
// Load the results
pcl::visualization::PCLVisualizer p (argc, argv, "VFH Cluster Classifier");
int y_s = (int)floor (sqrt ((double)k));
int x_s = y_s + (int)ceil ((k / (double)y_s) - y_s);
double x_step = (double)(1 / (double)x_s);
double y_step = (double)(1 / (double)y_s);
pcl::console::print_highlight ("Preparing to load ");
pcl::console::print_value ("%d", k);
pcl::console::print_info (" files (");
pcl::console::print_value ("%d", x_s);
pcl::console::print_info ("x");
pcl::console::print_value ("%d", y_s);
pcl::console::print_info (" / ");
pcl::console::print_value ("%f", x_step);
pcl::console::print_info ("x");
pcl::console::print_value ("%f", y_step);
pcl::console::print_info (")\n");
int viewport = 0, l = 0, m = 0;
for (int i = 0; i < k; ++i)
{
std::string cloud_name = models.at (k_indices[0][i]).first;
boost::replace_last (cloud_name, "_vfh", "");
p.createViewPort (l * x_step, m * y_step, (l + 1) * x_step, (m + 1) * y_step, viewport);
l++;
if (l >= x_s)
{
l = 0;
m++;
}
pcl::PCLPointCloud2 cloud;
pcl::console::print_highlight (stderr, "Loading "); pcl::console::print_value (stderr, "%s ", cloud_name.c_str ());
if (pcl::io::loadPCDFile (cloud_name, cloud) == -1)
break;
// Convert from blob to PointCloud
pcl::PointCloud<pcl::PointXYZ> cloud_xyz;
pcl::fromPCLPointCloud2 (cloud, cloud_xyz);
if (cloud_xyz.points.size () == 0)
break;
pcl::console::print_info ("[done, ");
pcl::console::print_value ("%d", (int)cloud_xyz.points.size ());
pcl::console::print_info (" points]\n");
pcl::console::print_info ("Available dimensions: ");
pcl::console::print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
// Demean the cloud
Eigen::Vector4f centroid;
pcl::compute3DCentroid (cloud_xyz, centroid);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_demean (new pcl::PointCloud<pcl::PointXYZ>);
pcl::demeanPointCloud<pcl::PointXYZ> (cloud_xyz, centroid, *cloud_xyz_demean);
// 添加点云到对应的视口
p.addPointCloud (cloud_xyz_demean, cloud_name, viewport);
// Check if the model found is within our inlier tolerance
std::stringstream ss;
ss << k_distances[0][i];
if (k_distances[0][i] > thresh)
{
p.addText (ss.str (), 20, 30, 1, 0, 0, ss.str (), viewport); // display the text with red
// Create a red line（创建一条红线）
pcl::PointXYZ min_p, max_p;
pcl::getMinMax3D (*cloud_xyz_demean, min_p, max_p);
std::stringstream line_name;
line_name << "line_" << i;
p.addLine (min_p, max_p, 1, 0, 0, line_name.str (), viewport);
p.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 5, line_name.str (), viewport);
}
else
p.addText (ss.str (), 20, 30, 0, 1, 0, ss.str (), viewport);
// Increase the font size for the score*
p.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_FONT_SIZE, 18, ss.str (), viewport);
// Add the cluster name（添加聚类名称）
p.addText (cloud_name, 20, 10, cloud_name, viewport);
}
// Add coordianate systems to all viewports（给所有的窗口添加坐标系统）
p.addCoordinateSystem (0.1, 0);
p.spin ();
return (0);
}
2.2.2 近邻搜索代码解释说明
接下来解释上述源代码中的关键语句，下面几行用于解析命令行参数。
// Load the test histogram
std::vector<int> pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
vfh_model histogram;
if (!loadHist (argv[pcd_indices.at (0)], histogram))
{
pcl::console::print_error ("Cannot load test file %s\n", argv[pcd_indices.at (0)]);
return (-1);
}
pcl::console::parse_argument (argc, argv, "-thresh", thresh);
// Search for the k closest matches
pcl::console::parse_argument (argc, argv, "-k", k);
pcl::console::print_highlight ("Using "); pcl::console::print_value ("%d", k); pcl::console::print_info (" nearest neighbors.\n");
打开用户给定的直方图模型特征文件，忽略其他，然后继续检查命令行参数，-k指定程序需要检查的显示的近邻数目，-thresh指定检索的模型中允许的最大距离。如果大于最大距离的相似模型，则在显示时都会在其对应的视口上显示红斜线。方便用户直接看出其不在允许的距离范围内。
loadFileList (models, training_data_list_file_name);
flann::load_from_file (data, training_data_h5_file_name, "training_data");
上面代码，加载训练数据以及前面在建立树结构数据时存储的模型VFH特征文件名列表，然后读取 kdtree 并重建索引。
flann::Index<flann::ChiSquareDistance<float> > index (data, flann::SavedIndexParams ("kdtree.idx"));
index.buildIndex ();
在这里必须确保在创建索引对象时使用的是Chi平方距离（此处用的是 ChiSquareDistance），与前面创建树结构时使用的索引对象的类型要一致，下面是最重要的一句。
nearestKSearch (index, histogram, k, k_indices, k_distances);
在 nearestKSearch 函数里面，首先将查询点转换成 FLANN 格式。其次是获得近邻搜索所得到的近邻索引和对应的距离值。
inline void nearestKSearch (flann::Index<flann::ChiSquareDistance<float> > &index,
const vfh_model &model,
int k,
flann::Matrix<int> &indices,
flann::Matrix<float> &distances)
{
// Query point
flann::Matrix<float> p = flann::Matrix<float>(new float[model.second.size ()], 1, model.second.size ());
memcpy (&p.ptr ()[0], &model.second[0], p.cols * p.rows * sizeof (float));
indices = flann::Matrix<int>(new int[k], 1, k);
distances = flann::Matrix<float>(new float[k], 1, k);
index.knnSearch (p, indices, distances, k, flann::SearchParams (512));
delete[] p.ptr ();
}
创建一个 PCLVisualizer 对象，并建立一组视口（将窗口分成不同的几个视口），用下面的语句可以做到。
p.createViewPort (l * x_step, m * y_step, (l + 1) * x_step, (m + 1) * y_step, viewport);
为了可视化目的，我们先计算点云的质心坐标，然后将点云的每一个点坐标值减去该质心坐标值，得到新的点云，该点云以其重心为坐标系的原点，这样所有的视口原点都是所有模型的重心，方便用户直观对比模型之间的相似度。
Eigen::Vector4f centroid;
pcl::compute3DCentroid (cloud_xyz, centroid);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_demean (new pcl::PointCloud<pcl::PointXYZ>);
pcl::demeanPointCloud<pcl::PointXYZ> (cloud_xyz, centroid, *cloud_xyz_demean);
// 添加点云到对应的视口
p.addPointCloud (cloud_xyz_demean, cloud_name, viewport);
最后检查通过 nearestKSearch 获得的距离是否大于用户给定的阈值，如果是的话，在对应的视口显示一条斜的红线段，直观表示其为距离较大的模型，即与用户给定的模型特征文件对应的模型之间相差甚远。
pcl::PointXYZ min_p, max_p;
pcl::getMinMax3D (*cloud_xyz_demean, min_p, max_p);
std::stringstream line_name;
line_name << "line_" << i;
p.addLine (min_p, max_p, 1, 0, 0, line_name.str (), viewport);
p.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 5, line_name.str (), viewport);
2.3 编译运行程序
在工作空间根目录cluster recognition下，编写CMakeLists.txt文件如下：
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
# we need FindFLANN.cmake
list(APPEND CMAKE_MODULE_PATH ${
CMAKE_CURRENT_SOURCE_DIR})
project(vfh_cluster_classifier)
find_package(PCL 1.2 REQUIRED)
include_directories(${
PCL_INCLUDE_DIRS})
link_directories(${
PCL_LIBRARY_DIRS})
add_definitions(${
PCL_DEFINITIONS})
find_package(HDF5)
if(HDF5_FOUND)
find_package(Flann)
include_directories(${
FLANN_INCLUDE_DIRS})
include_directories(${
HDF5_INCLUDE_DIR})
add_executable(build_tree src/build_tree.cpp)
target_link_libraries(build_tree ${
PCL_LIBRARIES}
${
Boost_LIBRARIES}
${
FLANN_LIBRARIES}
${
HDF5_hdf5_LIBRARY}
mpi_cxx)
add_executable(nearest_neighbors src/nearest_neighbors.cpp)
target_link_libraries(nearest_neighbors ${
PCL_LIBRARIES}
${
Boost_LIBRARIES}
${
FLANN_LIBRARIES}
${
HDF5_hdf5_LIBRARY}
mpi_cxx)
endif(HDF5_FOUND)
在工作空间根目录cluster recognition下创建一个build文件夹，用于存放编译过程中产生的文件，然后执行编译：
mkdir build
cd build
cmake ..
make
此时，会在build文件夹下生成两个可执行文件：build_tree 和 nearest_neighbors，其中build_tree可执行文件实现训练过程，而nearest_neighbors可执行文件实现测试过程。在运行可执行文件之前，需要在build文件下放入测试数据，本例将测试数据都存放在build/data文件夹下，然后运行下列命令执行训练过程。
./build_tree data/
运行结果如下所示，循环遍历加载data文件夹下所有的195个模型VFH特征文件，创建相应的数据和 kdtree 索引结构。运行完成后在当前目录下产生h5格式的数据文件trainingdata.h5、所有模型文件的路径保存文件 training_data.list、最终创建的具有195个元素的 kdtree 的索引文件 kdtree.idx。
> Loading "data/000.580.67" (0 models loaded so far).
> Loading "data/200.921.07" (16 models loaded so far).
> Loading "data/401.324.52" (31 models loaded so far).
> Loading "data/901.125.07" (47 models loaded so far).
> Loading "data/800.919.49" (65 models loaded so far).
> Loading "data/100.919.00" (79 models loaded so far).
> Loading "data/401.431.44" (100 models loaded so far).
> Loading "data/463.156.00" (116 models loaded so far).
> Loading "data/201.327.78" (132 models loaded so far).
> Loading "data/100.922.16" (148 models loaded so far).
> Loading "data/001.324.25" (168 models loaded so far).
> Loading "data/300.151.23" (181 models loaded so far).
> Loaded 195 VFH models. Creating training data training_data.h5/training_data.list.
Building the kdtree index (kdtree.idx) for 195 elements...
要运行最近邻测试的程序，选择我们提供的数据集之一作为测试样本，键入如下命令。
./nearest_neighbors -k 16 -thresh 50 data/000.580.67/1258730231333_cluster_0_nxyz_vfh.pcd
命令表示为搜索提供的模型VFH特征文件的16个近邻模型，并将距离大于50的视为不符合用户要求的模型，运行之后可看见可视化结果如下图所示。图中将所有搜索到的近邻模型，按照距离大小从窗口最左下角开始显示所有的近邻模型，对于大于阈值的模型则用红色斜线段。
近邻检测测试结果1
大家可以调整不同的参数k以及对应的阈值。如果调整k的话，最大值为194，即将索引树中所有模型进行排序显示，距离最小的模型与检索的模型最相似。下图是设置参数k = 32时的测试结果。
近邻检测测试结果2
至此，大家就可以建立自己的模型库来完成不同的简单应用了。