基于VFH描述子的聚类识别与6自由度位姿估计的学习

        本次学习中，我们的目标不是提供一个最终的识别工具，而是根据各个聚类的场景及其对应的6自由度位姿，从一组聚类中检索查询进而选取与用户输入的场景近似的候选集，这样就可以将识别问题抽象为一个近邻估计问题。给定一组训练数据，我们将使用一种有效的近邻搜索结构，例如kdtree，来查找返回一组相似对象，这些相似对象以与查找对象之间的距离来进行排序，很明显这样的程序要比直接返回是否存在用户检索的场景的程序要更有用处。具体内容可参考http://www.pclcn.org/study/shownews.php?lang=cn&id=188的学习教程。

         1.训练阶段，build_tree.cpp

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/console/print.h>
#include <pcl/io/pcd_io.h>
#include <boost/filesystem.hpp>
#include <flann/flann.h>
#include <flann/io/hdf5.h>
#include <fstream>

typedef std::pair<std::string, std::vector<float> > vfh_model;

/** \brief Loads an n-D histogram file as a VFH signature
* \param path the input file name
* \param vfh the resultant VFH model
*/
bool
loadHist (const boost::filesystem::path &path, vfh_model &vfh)
{
int vfh_idx;
// Load the file as a PCD
try
{
sensor_msgs::PointCloud2 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 <sensor_msgs::PointField> 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);
}

/** \brief Load a set of VFH features that will act as the model (training data)
* \param argc the number of arguments (pass from main ())
* \param argv the actual command line arguments (pass from main ())
* \param extension the file extension containing the VFH features
* \param models the resultant vector of histogram models
*/
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);
}
}
}

int
main (int argc, char** argv)
{
if (argc < 2)
{
PCL_ERROR ("Need at least two parameters! Syntax is: %s [model_directory] [options]\n", argv[0]);
return (-1);
}

std::string extension (".pcd");
transform (extension.begin (), extension.end (), extension.begin (), (int(*)(int))tolower);

std::string kdtree_idx_file_name = "kdtree.idx";
std::string training_data_h5_file_name = "training_data.h