#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));
}

template<typename PointT> intread (const std::string &file_name, pcl::PointCloud<PointT> &cloud, const int offset = 0){    pcl::PCLPointCloud2 blob;    int pcd_version;    int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_, pcd_version, offset);    // If no error, convert the data    if (res == 0)    pcl::fromPCLPointCloud2 (blob, cloud);    return (res);
}

intpcl::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);}