（一）读取PCD文件

#include <iostream>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>

void main()
{
    /* Create Point Cloud */
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);

    /* Read PCD File */
    /* Read Wrong */
    if (-1 == pcl::io::loadPCDFile<pcl::PointXYZRGBA>("table_scene_lms400.pcd", *cloud))
    {
        return;
    }
    
    /* Then show the point cloud */
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("office chair model"));
    viewer->setBackgroundColor(1, 1, 1);

    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> rgba(cloud);  //Maybe set the cloud to he handler rgba?
    viewer->addPointCloud<pcl::PointXYZRGBA>(cloud, rgba, "sample cloud");              //Add a Point Cloud (templated) to screen. Q:Some questions here
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");           //Set the rendering properties

    //viewer->addCoordinateSystem(1.0);                                                 //Adds 3D axes describing a coordinate system to screen at 0,0,0
    viewer->initCameraParameters ();                                                    //Initialize camera parameters with some default values. 


    /* Show the point cloud */
    while (!viewer->wasStopped ())
    {
        viewer->spinOnce (100);                                                           //updates the screen loop
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
}

template<typename PointT> inline int
loadPCDFile (const std::string &file_name, pcl::PointCloud<PointT> &cloud)
{
    pcl::PCDReader p;
    return (p.read (file_name, cloud));
}

int
pcl::PCDReader::read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
                      Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, 
                      const int offset)
{
  pcl::console::TicToc tt;
  tt.tic ();

  int data_type;
  unsigned int data_idx;
  
  int res = readHeader (file_name, cloud, origin, orientation, pcd_version, data_type, data_idx, offset);

  if (res < 0)
    return (res);

  unsigned int idx = 0;

  // Get the number of points the cloud should have
  unsigned int nr_points = cloud.width * cloud.height;

  // Setting the is_dense property to true by default
  cloud.is_dense = true;

  if (file_name == "" || !boost::filesystem::exists (file_name))
  {
    PCL_ERROR ("[pcl::PCDReader::read] Could not find file ‘%s‘.\n", file_name.c_str ());
    return (-1);
  }
  
  // if ascii
  if (data_type == 0)
  {
    // Re-open the file (readHeader closes it)
    std::ifstream fs;
    fs.open (file_name.c_str ());
    if (!fs.is_open () || fs.fail ())
    {
      PCL_ERROR ("[pcl::PCDReader::read] Could not open file %s.\n", file_name.c_str ());
      return (-1);
    }

    fs.seekg (data_idx);

    std::string line;
    std::vector<std::string> st;

    // Read the rest of the file
    try
    {
      while (idx < nr_points && !fs.eof ())
      {
        getline (fs, line);
        // Ignore empty lines
        if (line == "")
          continue;

        // Tokenize the line
        boost::trim (line);
        boost::split (st, line, boost::is_any_of ("\t\r "), boost::token_compress_on);
        
        if (idx >= nr_points)
        {
          PCL_WARN ("[pcl::PCDReader::read] input file %s has more points (%d) than advertised (%d)!\n", file_name.c_str (), idx, nr_points);
          break;
        }

        size_t total = 0;
        // Copy data
        for (unsigned int d = 0; d < static_cast<unsigned int> (cloud.fields.size ()); ++d)
        {
          // Ignore invalid padded dimensions that are inherited from binary data
          if (cloud.fields[d].name == "_")
          {
            total += cloud.fields[d].count; // jump over this many elements in the string token
            continue;
          }
          for (unsigned int c = 0; c < cloud.fields[d].count; ++c)
          {
            switch (cloud.fields[d].datatype)
            {
              case pcl::PCLPointField::INT8:
              {
                copyStringValue<pcl::traits::asType<pcl::PCLPointField::INT8>::type> (
                    st.at (total + c), cloud, idx, d, c);
                break;
              }
              case pcl::PCLPointField::UINT8:
              {
                copyStringValue<pcl::traits::asType<pcl::PCLPointField::UINT8>::type> (
                    st.at (total + c), cloud, idx, d, c);
                break;
              }
              case pcl::PCLPointField::INT16:
              {
                copyStringValue<pcl::traits::asType<pcl::PCLPointField::INT16>::type> (
                    st.at (total + c), cloud, idx, d, c);
                break;
              }
              case pcl::PCLPointField::UINT16:
              {
                copyStringValue<pcl::traits::asType<pcl::PCLPointField::UINT16>::type> (
                    st.at (total + c), cloud, idx, d, c);
                break;
              }
              case pcl::PCLPointField::INT32:
              {
                copyStringValue<pcl::traits::asType<pcl::PCLPointField::INT32>::type> (
                    st.at (total + c), cloud, idx, d, c);
                break;
              }
              case pcl::PCLPointField::UINT32:
              {
                copyStringValue<pcl::traits::asType<pcl::PCLPointField::UINT32>::type> (
                    st.at (total + c), cloud, idx, d, c);
                break;
              }
              case pcl::PCLPointField::FLOAT32:
              {
                copyStringValue<pcl::traits::asType<pcl::PCLPointField::FLOAT32>::type> (
                    st.at (total + c), cloud, idx, d, c);
                break;
              }
              case pcl::PCLPointField::FLOAT64:
              {
                copyStringValue<pcl::traits::asType<pcl::PCLPointField::FLOAT64>::type> (
                    st.at (total + c), cloud, idx, d, c);
                break;
              }
              default:
                PCL_WARN ("[pcl::PCDReader::read] Incorrect field data type specified (%d)!\n",cloud.fields[d].datatype);
                break;
            }
          }
          total += cloud.fields[d].count; // jump over this many elements in the string token
        }
        idx++;
      }
    }
    catch (const char *exception)
    {
      PCL_ERROR ("[pcl::PCDReader::read] %s\n", exception);
      fs.close ();
      return (-1);
    }

    // Close file
    fs.close ();
  }
  else 
  /// ---[ Binary mode only
  /// We must re-open the file and read with mmap () for binary
  {
    // Open for reading
    int fd = pcl_open (file_name.c_str (), O_RDONLY);
    if (fd == -1)
    {
      PCL_ERROR ("[pcl::PCDReader::read] Failure to open file %s\n", file_name.c_str () );
      return (-1);
    }
    
    // Seek at the given offset
    off_t result = pcl_lseek (fd, offset, SEEK_SET);
    if (result < 0)
    {
      pcl_close (fd);
      PCL_ERROR ("[pcl::PCDReader::read] lseek errno: %d strerror: %s\n", errno, strerror (errno));
      PCL_ERROR ("[pcl::PCDReader::read] Error during lseek ()!\n");
      return (-1);
    }
    
    size_t data_size = data_idx + cloud.data.size ();
    // Prepare the map
#ifdef _WIN32
    // As we don‘t know the real size of data (compressed or not), 
    // we set dwMaximumSizeHigh = dwMaximumSizeLow = 0 so as to map the whole file
    HANDLE fm = CreateFileMapping ((HANDLE) _get_osfhandle (fd), NULL, PAGE_READONLY, 0, 0, NULL);
    // As we don‘t know the real size of data (compressed or not), 
    // we set dwNumberOfBytesToMap = 0 so as to map the whole file
    char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ, 0, 0, 0));
    if (map == NULL)
    {
      CloseHandle (fm);
      pcl_close (fd);
      PCL_ERROR ("[pcl::PCDReader::read] Error mapping view of file, %s\n", file_name.c_str ());
      return (-1);
    }
#else
    char *map = static_cast<char*> (mmap (0, data_size, PROT_READ, MAP_SHARED, fd, 0));
    if (map == reinterpret_cast<char*> (-1))    // MAP_FAILED
    {
      pcl_close (fd);
      PCL_ERROR ("[pcl::PCDReader::read] Error preparing mmap for binary PCD file.\n");
      return (-1);
    }
#endif

    /// ---[ Binary compressed mode only
    if (data_type == 2)
    {
      // Uncompress the data first
      unsigned int compressed_size, uncompressed_size;
      memcpy (&compressed_size, &map[data_idx + 0], sizeof (unsigned int));
      memcpy (&uncompressed_size, &map[data_idx + 4], sizeof (unsigned int));
      PCL_DEBUG ("[pcl::PCDReader::read] Read a binary compressed file with %u bytes compressed and %u original.\n", compressed_size, uncompressed_size);
      // For all those weird situations where the compressed data is actually LARGER than the uncompressed one
      // (we really ought to check this in the compressor and copy the original data in those cases)
      if (data_size < compressed_size || uncompressed_size < compressed_size)
      {
        PCL_DEBUG ("[pcl::PCDReader::read] Allocated data size (%zu) or uncompressed size (%zu) smaller than compressed size (%u). Need to remap.\n", data_size, uncompressed_size, compressed_size);
#ifdef _WIN32
        UnmapViewOfFile (map);
        data_size = compressed_size + data_idx + 8;
        map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ, 0, 0, data_size));
#else
        munmap (map, data_size);
        data_size = compressed_size + data_idx + 8;
        map = static_cast<char*> (mmap (0, data_size, PROT_READ, MAP_SHARED, fd, 0));
#endif
      }

      if (uncompressed_size != cloud.data.size ())
      {
        PCL_WARN ("[pcl::PCDReader::read] The estimated cloud.data size (%u) is different than the saved uncompressed value (%u)! Data corruption?\n", 
                  cloud.data.size (), uncompressed_size);
        cloud.data.resize (uncompressed_size);
      }

      char *buf = static_cast<char*> (malloc (data_size));
      // The size of the uncompressed data better be the same as what we stored in the header
      unsigned int tmp_size = pcl::lzfDecompress (&map[data_idx + 8], compressed_size, buf, static_cast<unsigned int> (data_size));
      if (tmp_size != uncompressed_size)
      {
        free (buf);
        pcl_close (fd);
        PCL_ERROR ("[pcl::PCDReader::read] Size of decompressed lzf data (%u) does not match value stored in PCD header (%u). Errno: %d\n", tmp_size, uncompressed_size, errno);
        return (-1);
      }

      // Get the fields sizes
      std::vector<pcl::PCLPointField> fields (cloud.fields.size ());
      std::vector<int> fields_sizes (cloud.fields.size ());
      int nri = 0, fsize = 0;
      for (size_t i = 0; i < cloud.fields.size (); ++i)
      {
        if (cloud.fields[i].name == "_")
          continue;
        fields_sizes[nri] = cloud.fields[i].count * pcl::getFieldSize (cloud.fields[i].datatype);
        fsize += fields_sizes[nri];
        fields[nri] = cloud.fields[i];
        ++nri;
      }
      fields.resize (nri);
      fields_sizes.resize (nri);

      // Unpack the xxyyzz to xyz
      std::vector<char*> pters (fields.size ());
      int toff = 0;
      for (size_t i = 0; i < pters.size (); ++i)
      {
        pters[i] = &buf[toff];
        toff += fields_sizes[i] * cloud.width * cloud.height;
      }
      // Copy it to the cloud
      for (size_t i = 0; i < cloud.width * cloud.height; ++i)
      {
        for (size_t j = 0; j < pters.size (); ++j)
        {
          memcpy (&cloud.data[i * fsize + fields[j].offset], pters[j], fields_sizes[j]);
          // Increment the pointer
          pters[j] += fields_sizes[j];
        }
      }
      //memcpy (&cloud.data[0], &buf[0], uncompressed_size);

      free (buf);
    }
    else
      // Copy the data
      memcpy (&cloud.data[0], &map[0] + data_idx, cloud.data.size ());

    // Unmap the pages of memory
#ifdef _WIN32
    UnmapViewOfFile (map);
    CloseHandle (fm);
#else
    if (munmap (map, data_size) == -1)
    {
      pcl_close (fd);
      PCL_ERROR ("[pcl::PCDReader::read] Munmap failure\n");
      return (-1);
    }
#endif
    pcl_close (fd);
  }

  if ((idx != nr_points) && (data_type == 0))
  {
    PCL_ERROR ("[pcl::PCDReader::read] Number of points read (%d) is different than expected (%d)\n", idx, nr_points);
    return (-1);
  }

  // No need to do any extra checks if the data type is ASCII
  if (data_type != 0)
  {
    int point_size = static_cast<int> (cloud.data.size () / (cloud.height * cloud.width));
    // Once copied, we need to go over each field and check if it has NaN/Inf values and assign cloud.is_dense to true or false
    for (uint32_t i = 0; i < cloud.width * cloud.height; ++i)
    {
      for (unsigned int d = 0; d < static_cast<unsigned int> (cloud.fields.size ()); ++d)
      {
        for (uint32_t c = 0; c < cloud.fields[d].count; ++c)
        {
          switch (cloud.fields[d].datatype)
          {
            case pcl::PCLPointField::INT8:
            {
              if (!isValueFinite<pcl::traits::asType<pcl::PCLPointField::INT8>::type>(cloud, i, point_size, d, c))
                cloud.is_dense = false;
              break;
            }
            case pcl::PCLPointField::UINT8:
            {
              if (!isValueFinite<pcl::traits::asType<pcl::PCLPointField::UINT8>::type>(cloud, i, point_size, d, c))
                cloud.is_dense = false;
              break;
            }
            case pcl::PCLPointField::INT16:
            {
              if (!isValueFinite<pcl::traits::asType<pcl::PCLPointField::INT16>::type>(cloud, i, point_size, d, c))
                cloud.is_dense = false;
              break;
            }
            case pcl::PCLPointField::UINT16:
            {
              if (!isValueFinite<pcl::traits::asType<pcl::PCLPointField::UINT16>::type>(cloud, i, point_size, d, c))
                cloud.is_dense = false;
              break;
            }
            case pcl::PCLPointField::INT32:
            {
              if (!isValueFinite<pcl::traits::asType<pcl::PCLPointField::INT32>::type>(cloud, i, point_size, d, c))
                cloud.is_dense = false;
              break;
            }
            case pcl::PCLPointField::UINT32:
            {
              if (!isValueFinite<pcl::traits::asType<pcl::PCLPointField::UINT32>::type>(cloud, i, point_size, d, c))
                cloud.is_dense = false;
              break;
            }
            case pcl::PCLPointField::FLOAT32:
            {
              if (!isValueFinite<pcl::traits::asType<pcl::PCLPointField::FLOAT32>::type>(cloud, i, point_size, d, c))
                cloud.is_dense = false;
              break;
            }
            case pcl::PCLPointField::FLOAT64:
            {
              if (!isValueFinite<pcl::traits::asType<pcl::PCLPointField::FLOAT64>::type>(cloud, i, point_size, d, c))
                cloud.is_dense = false;
              break;
            }
          }
        }
      }
    }
  }
  double total_time = tt.toc ();
  PCL_DEBUG ("[pcl::PCDReader::read] Loaded %s as a %s cloud in %g ms with %d points. Available dimensions: %s.\n", 
             file_name.c_str (), cloud.is_dense ? "dense" : "non-dense", total_time, 
             cloud.width * cloud.height, pcl::getFieldsList (cloud).c_str ());
  return (0);
}