PCL中使用FLANN库(2)

本文涉及的产品
云数据库 Tair(兼容Redis),内存型 2GB
Redis 开源版,标准版 2GB
推荐场景:
搭建游戏排行榜
简介: 接着上一篇的介绍继续关于在使用readHeader函数读取点云数据头的类型的代码(Read a point cloud data header from a PCD file.) pcl::PCLPointCloud2 cloud; int version; Eigen:...

接着上一篇的介绍继续

关于在使用readHeader函数读取点云数据头的类型的代码(Read a point cloud data header from a PCD file.)

    pcl::PCLPointCloud2 cloud;
    int version;
    Eigen::Vector4f origin;
    Eigen::Quaternionf orientation;
    pcl::PCDReader r;
    int type; unsigned int idx;
    //读取PCD文件的头的基本信息
     /*(in)path.string ()文件名
      cloud 点云数据集  origin传感器的采集中心点 orientation传感器的方位 version为PCD版本
     type数据类型(0 = ASCII,1 =二进制,2 =二进制压缩)(in)idx为在文件内的偏移点云数据*/
    r.readHeader (path.string (), cloud, origin, orientation, version, type, idx);

查看PCD文件里的内容(其实对于如何生成这种高纬度的文件呢?)

# .PCD v.7 - Point Cloud Data file format   注释掉说明这是关于点云的文件格式
VERSION .7                                 PCD文件的版本号               
FIELDS x y z rgb u v vx vy vz normal_x normal_y normal_z curvature   文件中点云的维度数
SIZE 4 4 4 4 4 4 4 4 4 4 4 4 4         每个维度数据的大小
TYPE F F F F F F F F F F F F F         数据的类型
COUNT 1 1 1 1 1 1 1 1 1 1 1 1 1    
WIDTH 3484                             点云大小
HEIGHT 1                               无序点云
VIEWPOINT 0 0 0 1 0 0 0                 视点
POINTS 3484                             大小
DATA ascii                              数据
-0.0042959 -0.041022 0.97549 7.3757e-39 275 261 0 0 0 -0.15142 0.63581 -0.75685 0.018435
-0.0031521 -0.040989 0.97472 7.0991e-39 276 261 0 0 0 -0.12262 0.63448 -0.76315 0.017282
-0.0042959 -0.03988 0.97549 5.9927e-39 299 261 0 0 0 -0.15385 0.62475 -0.76552 0.017079
-0.0020133 -0.03988 0.97549 5.3473e-39 347 261 0 0 0 -0.11114 0.62014 -0.77658 0.015706
-0.00087171 -0.039294 0.9751 5.6239e-39 277.5 261.5 0 0 0 -0.089597 0.61557 -0.78297 0.015285

另外一种PCD文件的比如VFH的PCD文件

# .PCD v.6 - Point Cloud Data file format
FIELDS vfh
SIZE 4
TYPE F
COUNT 308
WIDTH 1
HEIGHT 1
POINTS 1
DATA ascii
0 0 0 0 0 0 0 0 0.086133 0.31582 ........................................................

 那么接下来我们就可以使用PCL给定的数据集,以及FLANN的库是实现对点云的识别,方法是按照(1)的思路来做的

首先我们是假设已经有了数据集,以及相应每个数据集的VFH全局表述子的PCD文件,这样我们就可以使用(1)中的思路把数据集训练并保存到数中,方便之后我们再输入给定的点云的VFH的PCD文件进行查找

那么其实这里面,我们如果是自己生成数据集,并对每个数据生成对应的VFH文件就会有点难度,毕竟这是对采集到的数据,对于一些无关点云需要剔除,

然后对有用的有价值的点云数据进行聚类,以及各个角度的点云聚类,然后对聚类的对象生成对应的VFH的特征PCD文件,这就是大致思路,

那么我们来看一下源代码是如何读取并训练数据源的,并生成可用于FLANN使用的文件,并存在磁盘中

源代码分析如下

#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>
//pair的成员有两个first second两个,很明显第一个vfh_model.first就是std::string 
 //vfh_model.second就是存入的float的变量
typedef std::pair<std::string, std::vector<float> > vfh_model;//用于存储VFH模型的容器

/** \brief Loads an n-D histogram file as a VFH signature 以VFH作为特征直方图
  * \param path the input file name 输入的文件的名称
  * \param vfh the resultant 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;      //中心float的向量
    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) //点的数目不为0
      return (false);
  }
  catch (const pcl::InvalidConversionException&)  //抛出异常
  {
    return (false);
  }

  // Treat the VFH signature as a single Point Cloud把相应的VFH特征代表单个点云
  pcl::PointCloud <pcl::VFHSignature308> point;  //申明VFH 的点云
  pcl::io::loadPCDFile (path.string (), point); 
  vfh.second.resize (308); //因为VFH有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);
}

/** \brief Load a set of VFH features that will act as the model (training data)  //以VFH特征作为模型的训练数据集
  * \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)  //对文件下每一个VFH的PCD文件计数
  {
    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.h5";
  std::string training_data_list_file_name = "training_data.list";

  std::vector<vfh_model> models;  //VFH的模型

  // Load the model histograms 载入模型的直方图
  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 ());

  // Convert data into FLANN format  把数据转为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];

  // Save data to disk (list of models)保存数据集到本地
  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 ();
 
  // Build the tree index and save it to disk  建立树索引并保存
  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<flann::ChiSquareDistance<float> > index (data, flann::KDTreeIndexParams (4));
  index.buildIndex ();
  index.save (kdtree_idx_file_name);
  delete[] data.ptr ();

  return (0);
}

这里面就很明显的生成了两个可用于FLANN进行搜索匹配的文件,以及模型的名称的列表,就是会生成以下三个文件

kdtree.idx(这个是kdtree模型的索引)

training_data.h5(用于FLANN库中的一种高效的文件格式,上一章有介绍),

training_data.list(这是训练数据集的列表)

(2)那么对于已经生成好的点云的数据集,我们就需要使用写一个程序来实现给定一个点云的VFH的PCD文件来寻找这个点云所在位置并且是什么角度拍照的结果,闲话少说明,直接就上程序

#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>

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
  {
    pcl::PCLPointCloud2 cloud;
    int version;
    Eigen::Vector4f origin;
    Eigen::Quaternionf orientation;
    pcl::PCDReader r;
    int type; unsigned int idx;
    //读取PCD文件的头的基本信息
     /*(in)path.string ()文件名
      cloud 点云数据集  origin传感器的采集中心点 orientation传感器的方位 version为PCD版本
     type数据类型(0 = ASCII,1 =二进制,2 =二进制压缩)(in)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);
}


/** \brief Search for the closest k neighbors搜索最近的K邻域
  * \param index the tree    树 的索引
  * \param model the query model  //给定的模型
  * \param k the number of neighbors to search for
  * \param indices the resultant neighbor indices
  * \param distances the resultant neighbor distances
  */
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 ();
}

/** \brief Load the list of file model names from an ASCII file  载入模型文件名
  * \param models the resultant list of model name
  * \param filename the input file name
  */
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);

  // 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  设置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";
  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);  //搜索K邻域
  }

  // 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);//Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector. 
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_demean (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::demeanPointCloud<pcl::PointXYZ> (cloud_xyz, centroid, *cloud_xyz_demean);
    // Add to renderer*
    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, "global", 0);

  p.spin ();
  return (0);
}

这里面就涉及到FLANN的库的函数的使用,执行的结果

打印的结果,这里面会显示最接近的六个数据集,并且计算这六个最近点云与给定点云之间的“距离”,这也是衡量两者之间的相似度的大小

可视化的结果

很明显左下角就是我们给定的数据点云,而且运行查找的速度非常快~

好了就这样了

 关注微信公众号,欢迎大家的无私分享

相关实践学习
基于Redis实现在线游戏积分排行榜
本场景将介绍如何基于Redis数据库实现在线游戏中的游戏玩家积分排行榜功能。
云数据库 Redis 版使用教程
云数据库Redis版是兼容Redis协议标准的、提供持久化的内存数据库服务,基于高可靠双机热备架构及可无缝扩展的集群架构,满足高读写性能场景及容量需弹性变配的业务需求。 产品详情:https://www.aliyun.com/product/kvstore &nbsp; &nbsp; ------------------------------------------------------------------------- 阿里云数据库体验:数据库上云实战 开发者云会免费提供一台带自建MySQL的源数据库&nbsp;ECS 实例和一台目标数据库&nbsp;RDS实例。跟着指引,您可以一步步实现将ECS自建数据库迁移到目标数据库RDS。 点击下方链接,领取免费ECS&amp;RDS资源,30分钟完成数据库上云实战!https://developer.aliyun.com/adc/scenario/51eefbd1894e42f6bb9acacadd3f9121?spm=a2c6h.13788135.J_3257954370.9.4ba85f24utseFl
相关文章
|
4月前
|
C++
C++ PCL SACSegmentationFromNormals setAxis 轴向的选择
C++ PCL SACSegmentationFromNormals setAxis 轴向的选择
73 2
|
6月前
|
机器学习/深度学习 编译器 算法框架/工具
OpenCV算法库
numba是一个用于编译Python数组和数值计算函数的编译器,这个编译器能够大幅提高直接使用Python编写的函数的运算速度。
|
7月前
|
人工智能 缓存 Ubuntu
【Ubuntu】Ubuntu安装PCL(安装PCL/卸载PCL/查看PCL版本/PCL报错处理相关操作)(史上最详细)
【Ubuntu】Ubuntu安装PCL(安装PCL/卸载PCL/查看PCL版本/PCL报错处理相关操作)(史上最详细)
|
C++ 计算机视觉
vs安装pcl库,遇到的问题总结(全)
vs安装pcl库,遇到的问题总结(全)
385 0
|
数据可视化 C语言 计算机视觉
|
C++ Windows
VS2010使用boost::geometry库时为什么编译不通过说明
VS2010使用boost::geometry库时为什么编译不通过说明
132 0
VS2010使用boost::geometry库时为什么编译不通过说明
|
机器学习/深度学习 存储 算法
PCL中使用FLANN库(1)
FLANN库全称是Fast Library for Approximate Nearest Neighbors,它是目前最完整的(近似)最近邻开源库。不但实现了一系列查找算法,还包含了一种自动选取最快算法的机制,在一个度量空间X给定一组点P=p1,p2,…,pn,这些点必须通过以下方式进行预处理,给第一个新的查询点q属于X,快速在P中找到距离q最近的点,即最近邻搜索问题。
2197 0
|
Ubuntu C++ Windows
VS2017安装PCL1.8.1
很多使用在windows环境下编译和使用PCL,这样让我想试试,所以就迫不得已的放弃使用Ubuntu环境,但是我还是建议使用Ubuntu系统,毕竟在Ubuntu下几条命令就搞定了,为了迎合在window使用PCL开发kinect,今天就试着在vS下配置和使用PCL,习惯了一边安装一边记录,首先安装VS2017,直接就是百度的界面提示所安装的VS2017 (1)下载PCL-1.
2136 0