基于PCL的物体识别与位姿估计tutorial学习

基于PCL的物体识别与位姿估计tutorial学习

本篇来源PCL点云库的tutorial。该tutorial的大致思想是将点云用VFH特征进行描述，然后训练生成树模型，当给一个新点云，计算其VFH，通过最邻近搜索，找到最相似的点云模型，完成识别。

---

---

前言

物体识别这块应用非常广泛，相关的方法也比较多。这个tutorial给pcl特征描述子一个应用的方法，至少让我知道库里面的特征描述子都有那些应用。pcl库比opencv库容易很多，用起来上手很快。编译这个tutorial不容易，特此记录一下，帮助将来的学习者。第一部分先介绍如何编译，第二部分再对代码进行剖析（未完待续）。

---

一、编译步骤

pcl库的安装不介绍，我不是源码编译安装的，直接装的二进制。这个例子需要安装HDF5，按照这个链接步骤，亲测有效。但是安装完用PCL官网的CMakeLists还是没有办法通过编译。其中用到find_package()，来找到HFD5这个库。但我本地usr/share/make-3.10/Module的FindHDF5.cmake，可能有点问题，不能用这个方法。所以我是手动设置头文件和库文件的地址，见CMakeLists.txt。

然后是FLANN这个库的FindFLANN.cmake这个文件，我的cmake，Module里面没有这个文件，从这个flann库的github源码地方，把这个文件找到，丢到cmake，Module里面，find_package()这个命令就可以用了。我本地FLANN库头文件位置倒是清楚，库文件位置我不太确定。

另外，有了上述的库，我自己编译时，还遇到一个错误，如下图所示。一开始，我以为是我前面的步骤错了，后来看到有人分享这个错误与mpi_cxx 有关。

把这个库，链接上就可以编译运行了。

我的CMakeLists.txt文件如下：

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

#设置可执行文件放到bin下
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/../bin)

# we need FindFLANN.cmake 

list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR})
project(vfh_cluster_classifier)
find_package(PCL 1.8 REQUIRED)
include_directories(
	${PCL_INCLUDE_DIRS}
)

link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

#可以先试试，能不能用
#find_package(HDF5)
find_package(FLANN REQUIRED)

include_directories(${FLANN_INCLUDE_DIRS})
  
#手动设置头文件地址与库文件地址
include_directories(${HDF5_INCLUDE_DIR} /usr/local/hdf5/include)
set(${HDF5_LIBRARIES} /usr/local/hdf5/lib)

  add_executable(build_tree src/build_tree.cpp)
  target_link_libraries(build_tree 
  ${PCL_LIBRARIES}
  mpi_cxx 
  ${Boost_LIBRARIES}
  ${FLANN_LIBRARIES}                              
  ${HDF5_LIBRARIES}
)

  add_executable(nearest_neighbors src/nearest_neighbors.cpp)
  target_link_libraries(nearest_neighbors 
  	${PCL_LIBRARIES} 
  	mpi_cxx 
  	${Boost_LIBRARIES} 
	${FLANN_LIBRARIES} 
	${HDF5_LIBRARIES} 
)

二、使用步骤

1.训练阶段

代码如下：

#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
  {
   
    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;
  pcl::getFieldIndex<pcl::VFHSignature308> ("vfh", fields);

  for (std::size_t i = 0; i < fields[vfh_idx].count; ++i)
  {
   
    vfh.second[i] = point[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