PCL库学习（6）_关于VFH特征识别官网代码调试（3 近邻搜索代码测试）

对于一个已经生成好的点云集，用以下代码来实现给定一个点云的VFH的PCD文件来寻找这个点云的位置以及拍摄角度。

注释掉build tree，打开nearest_neighbors.

但是出现以下无法运行。

经反复检查，发现问题在于build tree中

造成上述原因就是注释掉了第一行，导致后面nearest_neighbor中kd_tree index的数据类型不一致。

改：注释掉第二行,程序运行正确。

可视化结果：

附代码：

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/console/print.h>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <flann/flann.h>
#include <flann/io/hdf5.h>
#include <boost/filesystem.hpp>
//模型特征数据存储变量vfh_model，该变量用一个字符串存储文件名，一个向量存储文件对应的VFH特征
typedef std::pair<std::string, std::vector<float> > vfh_model;


//打开遍历路径得到每个PCD文件，并读取文件头，检测是否含有VFH标志位
bool loadHist(const boost::filesystem::path &path, vfh_model &vfh)
{
int vfh_idx;
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 ( const pcl::InvalidConversionException &)
{
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;
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);
}


//循环遍历从命令行获取的路径，从而加载路径下所有PCD文件
inline void nearestKSearch(flann::Index<flann::ChiSquareDistance<float> > &index, const vfh_model &model,int k, flann::Matrix<int> &indices, flann::Matrix<float> &distances)
{
//查询点转换为FLANN格式
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();
}
//载入模型文件名
bool loadFileList(std::vector<vfh_model> &models, const std::string &filename)
{
ifstream fs;
fs.open(filename.c_str());
if (!fs.is_open() || fs.fail())
return (false);


std::string line;
while (!fs.eof())
{
getline(fs, line);
if (line.empty())
continue;
vfh_model m;
m.first = line;
models.push_back(m);
}
fs.close();
return (true);
}


int
main(int argc, char** argv)
{
int k = 6;


double thresh = DBL_MAX;     // No threshold, disabled by default


if (argc < 2)
{
pcl::console::print_error("Need at least three parameters! Syntax is: %s <query_vfh_model.pcd> [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);


//打开用户给定的直方图模型特征文件
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);
// 设置K个最近邻匹配的个数
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";//kdtree模型的索引
std::string training_data_h5_file_name = "training_data.h5";//FLANN库中一种高效的文件格式
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;
// 检查数据是否保存
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());
}


// 检查树索引是否保存到磁盘
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);//调用nearestKSearch函数，共5个参数
}


// 打印结果到标准输出设备
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]);


// 加载结果
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());


// 为了可视化点云，通过计算点云的质心并减去它，来使点云以其重心为坐标系的原点，这样所有视口原点都是所有模型的重心
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获得的距离是否大于用户给定的阈值，是（在对应视口显示一条斜红线）
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


//创建一条红线
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);


//添加聚类名称
p.addText(cloud_name, 20, 10, cloud_name, viewport);
}
// 给所有的窗口添加坐标系统
p.addCoordinateSystem(0.1,"global", 0);


p.spin();
//system("pause");
return (0);
}