initial commit

src/hull2d.hpp

+#include <pcl/ModelCoefficients.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/project_inliers.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/surface/concave_hull.h>
+#include <pcl/surface/convex_hull.h>
+#include <Eigen/Core>
+
+#include <fstream>
+#include <iostream>
+#include <numeric>
+
+
+
+
+typedef pcl::PointXYZI PointT;
+
+
+float
+getAveragePointsInNeighborhood(pcl::PointCloud<PointT>::Ptr cloud, float searchRadius )
+{
+    // create a kdtree object
+    pcl::KdTreeFLANN<PointT> kdtreeObj;
+    // set the input cloud for the kdtree. this should start creating the tree.
+    kdtreeObj.setInputCloud (cloud);
+
+    std::vector<int> pointIdxRadiusSearch;
+    std::vector<float> pointRadiusSquaredDistance;
+
+    // determine size of PC before entering for, as the size changes by one during the loop.
+    size_t nIters = cloud->points.size();
+
+    std::vector<float> nPtsVec;
+    int nPts; // number of points found within searchRadius. This will also be returned by function.
+
+
+    #pragma omp parallel for // use parallel processing to speed up the process slightly. More cores would be better i guess...
+    for (size_t p = 0; p < nIters; p++){
+
+        //clear the vectors before each search.
+        pointIdxRadiusSearch.clear();
+        pointRadiusSquaredDistance.clear();
+
+        const PointT searchPoint = cloud->points[p];
+
+        // if no neighbours were found add point to result point cloud;.
+        nPts = kdtreeObj.radiusSearch (searchPoint, searchRadius, pointIdxRadiusSearch, pointRadiusSquaredDistance);
+        if ( nPts > 1 )
+        {
+/*
+                    if (p % 10000 == 0)
+                        std::cout << "Performed " << p+1 << " kdtree searches out of "
+                              << nIters << ". " << std::setprecision(4) << "Found " << nPts
+                              << " points within searchRadius of " << searchRadius << ". "
+                              << float((p+1.))/float(nIters)*100.0 << " % completed." << std::endl;
+*/
+            nPtsVec.push_back((float)nPts);
+
+        }
+        else
+        {
+            nPts = -1;
+        }
+
+    }
+
+
+    return std::accumulate(nPtsVec.begin(), nPtsVec.end(), 0.0f) / (float) nPtsVec.size();
+}
+
+int
+applyHull (pcl::PointCloud<PointT>::Ptr cloud_filtered, pcl::PointCloud<PointT>::Ptr hull, pcl::PointCloud<PointT>::Ptr convexhull )
+{
+
+    pcl::console::TicToc tt;
+    tt.tic();
+
+      pcl::PointCloud<PointT>::Ptr cloud_projected (new pcl::PointCloud<PointT>);
+      // read in parameter file
+      std::vector<float> params;
+      readParamsFromFile("../cfg/hull2d.cfg", params, true);
+
+
+
+      // Create a Concave Hull representation of the projected inliers
+      pcl::PointCloud<PointT>::Ptr cloud_hull (new pcl::PointCloud<PointT>);
+      pcl::ConcaveHull<PointT> chull;
+      chull.setInputCloud (cloud_filtered);
+      chull.setAlpha (params[0]);
+
+      int nPts = (int)getAveragePointsInNeighborhood(cloud_hull, 0.25 ); // param2 = searchDistance in meters
+      std::cout << "number of average points within searchRadius found: " <<  nPts << "  alpha= " << chull.getAlpha() << std::endl;
+
+      if (nPts <= 8)
+          std::cout << "average number of neighboring points < 8. Continuing with processing... " << std::endl;
+      else
+      {
+        std::cout << "average number of neighboring points > 8, increasing Alpha by 0.15 => Alpha: " << chull.getAlpha() + 0.15 << " and reapplying hullDetection... " << std::endl;
+        chull.setAlpha (chull.getAlpha() + 0.1);
+        chull.reconstruct (*cloud_hull);
+      }
+
+      pcl::copyPointCloud(*cloud_hull, *hull);
+
+      std::cerr << "Concave hull has: " << cloud_hull->points.size ()
+                << " data points." << std::endl;
+
+      pcl::PCDWriter writer;
+      writer.write ("ConcavHull.pcd", *cloud_hull, false);
+
+
+      pcl::ConvexHull<PointT> convhull;
+      convhull.setInputCloud (cloud_projected);
+      convhull.setComputeAreaVolume( false );
+      convhull.reconstruct (*cloud_hull);
+      pcl::copyPointCloud(*cloud_hull, *convexhull);
+
+
+      std::cerr << "Convex hull has: " << cloud_hull->points.size ()
+                << " data points." << " Processing took: " <<
+                   tt.toc()/1000.0 << " seconds." << std::endl;
+
+      //writer.write ("Convexhull.pcd", *cloud_hull, false);
+
+      return (0);
+
+}
+
+
+int
+applyHull (pcl::PointCloud<PointT>::Ptr cloud_filtered, pcl::PointCloud<PointT>::Ptr hull)
+{
+
+    pcl::console::TicToc tt;
+    tt.tic();
+
+//  pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
+  pcl::PointCloud<PointT>::Ptr cloud_projected (new pcl::PointCloud<PointT>);
+
+  // read in parameter file
+  std::vector<float> params;
+  readParamsFromFile("../cfg/hull2d.cfg", params, true);
+
+  // Create a Concave Hull representation of the projected inliers
+  pcl::PointCloud<PointT>::Ptr cloud_hull (new pcl::PointCloud<PointT>);
+  pcl::ConcaveHull<PointT> chull;
+  chull.setInputCloud (cloud_filtered);
+  chull.setAlpha (params[0]);
+
+  chull.reconstruct (*cloud_hull);
+
+  int nPts = (int)getAveragePointsInNeighborhood(cloud_hull, 0.25 ); // param2 = searchDistance in meters
+  std::cout << "number of average points within searchRadius found: " <<  nPts << "  alpha= " << chull.getAlpha() << std::endl;
+
+  if (nPts <= 8)
+      std::cout << "average number of neighboring points < 8. Continuing with processing... " << std::endl;
+  else
+  {
+    std::cerr << "average number of neighboring points > 8, increasing Alpha by 0.05 => Alpha: " << chull.getAlpha() + 0.05 << " and reapplying hullDetection... " << std::endl;
+    chull.setAlpha (chull.getAlpha() + 0.05);
+    chull.reconstruct (*cloud_hull);
+  }
+
+
+
+
+
+  //viewer<PointT>(cloud_hull);
+  pcl::copyPointCloud(*cloud_hull, *hull);
+
+  std::cerr << "Concave hull has: " << cloud_hull->points.size ()
+            << " data points." << " Processing took: " <<
+               tt.toc()/1000.0 << " seconds." << std::endl;
+
+  pcl::PCDWriter writer;
+  if (cloud_hull->size() > 0)
+    writer.write ("ConcavHull.pcd", *cloud_hull, false);
+
+  return (0);
+}
+

src/miscFunctions.hpp

+#ifndef PI
+#define PI atan(1.)*4.0L
+#endif
+
+// standard c++
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include <string>
+#include <numeric>
+
+// pcl
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/ModelCoefficients.h>
+
+#include <pcl/filters/project_inliers.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/segmentation/sac_segmentation.h>
+
+#include <pcl/common/distances.h>
+
+
+
+// openCV
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/opencv.hpp>
+
+#include <opencv2/viz/vizcore.hpp>
+
+template <typename PointT>
+void addMinValues(const typename pcl::PointCloud<PointT>::Ptr &cloud,
+                  std::vector< std::vector<double> > &pointVector,
+                  std::vector< unsigned int > &intensityVec,
+                  const std::vector<double> minvals)
+{
+
+    double x,y,z;
+    std::setprecision(11);
+
+    // check if input cloud has an intensity field. If so use provided data, else create random values
+    bool hasIntensity = false;
+    std::string pcFields = pcl::getFieldsList(*cloud);
+    if (pcFields.find("intensity") < std::string::npos-1)
+        hasIntensity = true;
+
+
+    for(int i=0; i< cloud->size(); i++){
+        x =  (minvals[0] + static_cast<double> (cloud->points[i].x) ) ;
+        y =  (minvals[1] + static_cast<double> (cloud->points[i].y) ) ;
+        z =  (minvals[2] + static_cast<double> (cloud->points[i].z) ) ;
+
+        if (hasIntensity)
+            intensityVec.push_back( static_cast< unsigned int> (cloud->points[i].intensity) );
+        else
+            intensityVec.push_back( static_cast<unsigned int>( (rand() % 255)) );
+
+        std::vector<double> point = {x,y,z};
+       // std::cerr << "DEBUG: point after adding values: \t " <<
+        //             point[0] << "\t" << point[1] << "\t" <<  point[2] << std::endl;
+        pointVector.push_back(point);
+    }
+
+}
+
+template <typename PointT>
+void addMinValuesToBBOX(const typename pcl::PointCloud<PointT>::Ptr &cloud,
+                  std::vector< std::vector<double> > &pointVector,
+                  std::vector< unsigned int > &intensityVec,
+                  const std::vector<double> minvals)
+{
+
+    double x,y,z;
+    std::setprecision(11);
+
+    // check if input cloud has an intensity field. If so use provided data, else create random values
+    bool hasIntensity = false;
+    std::string pcFields = pcl::getFieldsList(*cloud);
+    if (pcFields.find("intensity") < std::string::npos-1)
+        hasIntensity = true;
+
+
+    for(int i=0; i< cloud->size(); i++){
+        x =  (minvals[0] + static_cast<double> (cloud->points[i].x) ) ;
+        y =  (minvals[1] + static_cast<double> (cloud->points[i].y) ) ;
+        z =  (minvals[2] + static_cast<double> (cloud->points[i].z) ) ;
+
+        if (hasIntensity)
+            intensityVec.push_back( static_cast< unsigned int> (cloud->points[i].intensity) );
+        else
+            intensityVec.push_back( static_cast<unsigned int>( (rand() % 255)) );
+
+        std::vector<double> point = {x,y,z};
+
+        pointVector.push_back(point);
+
+        point.clear();
+
+        point.push_back( x-0.1); point.push_back(y); point.push_back( z); pointVector.push_back(point); point.clear();
+        intensityVec.push_back( static_cast< unsigned int> (cloud->points[i].intensity) );
+        point.push_back( x+0.1); point.push_back(y); point.push_back( z); pointVector.push_back(point); point.clear();
+        intensityVec.push_back( static_cast< unsigned int> (cloud->points[i].intensity) );
+        point.push_back( x); point.push_back(y-.1); point.push_back( z); pointVector.push_back(point); point.clear();
+        intensityVec.push_back( static_cast< unsigned int> (cloud->points[i].intensity) );
+        point.push_back( x); point.push_back(y+0.1); point.push_back( z); pointVector.push_back(point); point.clear();
+        intensityVec.push_back( static_cast< unsigned int> (cloud->points[i].intensity) );
+        point.push_back( x); point.push_back(y); point.push_back( z-0.1); pointVector.push_back(point); point.clear();
+        intensityVec.push_back( static_cast< unsigned int> (cloud->points[i].intensity) );
+        point.push_back( x); point.push_back(y); point.push_back( z+0.1); pointVector.push_back(point); point.clear();
+        intensityVec.push_back( static_cast< unsigned int> (cloud->points[i].intensity) );
+        point.push_back( x+0.1); point.push_back(y+0.1); point.push_back( z+0.1); pointVector.push_back(point); point.clear();
+        intensityVec.push_back( static_cast< unsigned int> (cloud->points[i].intensity) );
+        point.push_back( x-0.1); point.push_back(y-0.1); point.push_back( z-0.1); pointVector.push_back(point); point.clear();
+        intensityVec.push_back( static_cast< unsigned int> (cloud->points[i].intensity) );
+
+
+    }
+
+}
+
+
+int toXYZ( std::string file2Write,
+          const std::vector< std::vector<double> > &pointVector,
+          const std::vector< unsigned int > &intensityVec )
+{
+
+   // std::cout << "debug: writing the point cloud to text file. This will take some time....\n";
+
+    std::string fileToWrite = file2Write + ".xyz";
+    std::ofstream txtfile (fileToWrite);
+
+    if (!txtfile.is_open() ) {
+        PCL_ERROR ("Couldn't open file for writing ");
+        return -1;
+    }
+
+    txtfile.precision(11);
+
+    unsigned int intensity;
+
+    for(int i=0; i< pointVector.size(); i++){
+
+        intensity = intensityVec[i];
+        std::vector< double > pt = pointVector.at(i);
+        txtfile << pt[0] << " " << pt[1] << " " << pt[2] << " " << intensity << " " << intensity << " " << intensity << std::endl;
+    }
+
+
+    txtfile.close();
+    std::cerr << "Saved " << pointVector.size() << " Points to " << fileToWrite << " \n" << std::endl;
+
+
+}
+
+
+typedef pcl::PointXYZI PointT;
+
+///////////////////////////////////////////////////////////////////////////////////////
+template <typename PointInT>
+void
+viewer (typename pcl::PointCloud<PointInT>::Ptr &cloud)
+{
+
+
+  pcl::visualization::CloudViewer viewer ("View Point cloud");
+  viewer.showCloud (cloud);
+  while (!viewer.wasStopped ())
+  {
+  }
+}
+
+
+
+
+//////////////////////////////////////////////////////////////////////
+/// \brief calcAreaFromBbox
+/// \param bBox in
+/// \return area of the bounding box
+///
+float calcAreaFromBbox(std::vector<Eigen::Vector3f> bBox, float &aspectRatio){
+
+    pcl::PointXYZ p0,p1,p2;
+
+    p0.x = bBox.at(0)[0];
+    p0.y = bBox.at(0)[1];
+    p0.z = bBox.at(0)[2];
+
+    p1.x = bBox.at(1)[0];
+    p1.y = bBox.at(1)[1];
+    p1.z = bBox.at(1)[2];
+
+    p2.x = bBox.at(2)[0];
+    p2.y = bBox.at(2)[1];
+    p2.z = bBox.at(2)[2];
+
+    float s0 = pcl::euclideanDistance(p0,p1);
+    float s1 = pcl::euclideanDistance(p1,p2);
+
+
+    /*
+    std::cout << s0 << std::endl;
+    std::cout << s1 << std::endl;
+
+*/
+
+    if (s0 < s1){
+        std::cout<< "aspect ratio s0/s1 = " << s0 / s1 << std::endl;
+        aspectRatio=s0/s1;
+    }
+    else{
+        std::cout<< "aspect ratio s1/s0 = " << s1 / s0 << std::endl;
+        aspectRatio=s1/s0;
+    }
+
+
+
+    float area = fabs(s0 * s1);
+    std::cout << "Window area: " << area << " sqm." << std::endl;
+    return area;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////
+
+void
+readParamsFromFile(std::string cfgFilename, std::vector<float> & params, bool printParameterNames){
+
+// read in a parameter file so no recompilation is necessary to test parameters.
+// create a file stream to the appropriate config file for the specfic method
+std::fstream parameterFile;
+parameterFile.open(cfgFilename);
+// initiate a variable vector and a string of the variable name in the text file (for debug only, not used here after)
+std::string discard;
+//std::vector<float> params;
+float value;
+
+// perform the actual file reading.
+int par=0;
+if (parameterFile.is_open()){
+
+    while (parameterFile >> discard >> value) {
+      params.push_back(value);
+
+      if (printParameterNames)
+        std::cout << "reading " << cfgFilename << ": paramter-value # " << par+1 << " is: '" << discard << "' its value is: " << value << std::endl;
+
+      par++;
+    }
+    parameterFile.close();
+}
+else
+    std::cerr << "ERROR: could not open specified file: " << cfgFilename << ". Check path and spelling and make sure the file exists\n";
+
+
+}
+
+
+void
+findBoundingBox(pcl::PointCloud<PointT>::Ptr &cloud, std::vector<Eigen::Vector3f> & table_top_bbx)
+{
+  //std::vector<Eigen::Vector3f> table_top_bbx;
+
+  // Project points onto the table plane
+
+    pcl::SACSegmentation<PointT> seg;
+  pcl::ModelCoefficients::Ptr planeCoefficients (new pcl::ModelCoefficients ());
+  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
+/*
+  //debug:
+  for (int i=0; i< cloud->points.size(); i++)
+      std::cout << "x: " << cloud->points[i].x << "\t y: " <<
+              cloud->points[i].y << "\t z: " << cloud->points[i].z << std::endl;
+
+*/
+  seg.setOptimizeCoefficients (true);
+  seg.setModelType (pcl::SACMODEL_PLANE);
+  seg.setMethodType (pcl::SAC_RANSAC);
+  seg.setMaxIterations (100);
+  seg.setDistanceThreshold (0.25);
+  seg.setInputCloud (cloud);
+  seg.segment (*inliers, *planeCoefficients);
+
+  pcl::ProjectInliers<PointT> proj;
+  proj.setModelType(pcl::SACMODEL_PLANE);
+  pcl::PointCloud<PointT> projected_cloud;
+  proj.setInputCloud(cloud);
+  proj.setModelCoefficients(planeCoefficients);
+  proj.filter(projected_cloud);
+
+  pcl::PointCloud<PointT>::Ptr temp (new pcl::PointCloud<PointT> ());
+  pcl::copyPointCloud(projected_cloud,*temp);
+  //pcl::copyPointCloud(*cloud,*temp);
+  //viewer<PointT> (temp);
+
+  // store the table top plane parameters
+  Eigen::Vector3f plane_normal;
+  plane_normal.x() = planeCoefficients->values[0];
+  plane_normal.y() = planeCoefficients->values[1];
+  plane_normal.z() = planeCoefficients->values[2];
+  // compute an orthogonal normal to the plane normal
+  Eigen::Vector3f v = plane_normal.unitOrthogonal();
+  // take the cross product of the two normals to get
+  // a thirds normal, on the plane
+  Eigen::Vector3f u = plane_normal.cross(v);
+
+  // project the 3D point onto a 2D plane
+  std::vector<cv::Point2f> points;
+  // choose a point on the plane
+  Eigen::Vector3f p0(projected_cloud.points[0].x,
+                      projected_cloud.points[0].y,
+                      projected_cloud.points[0].z);
+  for(unsigned int ii=0; ii<projected_cloud.points.size(); ii++)
+  {
+    Eigen::Vector3f p3d;
+    if ( std::isnan( projected_cloud.points[ii].x) ||
+         std::isnan( projected_cloud.points[ii].y) ||
+         std::isnan( projected_cloud.points[ii].z) )
+        continue;
+    else{
+          p3d[0] = projected_cloud.points[ii].x;
+          p3d[1] = projected_cloud.points[ii].y;
+          p3d[2] = projected_cloud.points[ii].z;
+    }
+
+      // subtract all 3D points with a point in the plane
+    // this will move the origin of the 3D coordinate system
+    // onto the plane
+     p3d = p3d - p0;
+
+    cv::Point2f p2d;
+    p2d.x = p3d.dot(u);
+    p2d.y = p3d.dot(v);
+    points.push_back(p2d);
+  }
+
+
+  cv::Mat points_mat(points);
+  cv::RotatedRect rrect = cv::minAreaRect(points_mat);
+  cv::Point2f rrPts[4];
+  rrect.points(rrPts);
+
+
+ // double arclength = cv::arcLength(points, true);
+  // double contArea = cv::contourArea( points);
+
+ // double compactness = (4.*PI)* contArea / (4.*arclength*arclength);
+
+  //std::cout << "arclength: " << arclength << "  contArea: " <<
+   //            contArea << "  compactness: " << compactness << std::endl;
+
+
+
+  //store the table top bounding points in a vector
+  for(unsigned int ii=0; ii<4; ii++)
+  {
+    Eigen::Vector3f pbbx(rrPts[ii].x*u + rrPts[ii].y*v + p0);
+    table_top_bbx.push_back(pbbx);
+  }
+  Eigen::Vector3f center(rrect.center.x*u + rrect.center.y*v + p0);
+  table_top_bbx.push_back(center);
+
+}

src/pcl_las_helper.hpp

+#include <iostream>
+#include <string>
+#include <fstream>  // std::ifstream
+#include <vector>
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/filters/uniform_sampling.h>
+
+
+#include <liblas/liblas.hpp>
+
+
+template <typename PointT>
+int
+readLAS2PCD(std::string fileToRead,
+            typename pcl::PointCloud<PointT>::Ptr &cloud,
+            std::vector<double> &minXYZValues,
+            float gridLeafSize,
+            bool subtractMinVals)
+{
+
+    // 1) create a file stream object to access the file
+    std::ifstream ifs;
+    ifs.open(fileToRead, std::ios::in | std::ios::binary);
+    // if the LAS file could not be opend. throw an error (using the PCL_ERROR functionality).
+    if (!ifs.is_open())
+    {
+        PCL_ERROR ("Couldn't read file ");
+        return -1;
+    }
+
+    // set up ReaderFactory of the LibLAS library for reading in the data.
+    std::cout << "Reading in LAS input file: " << fileToRead << std::endl;
+
+    liblas::ReaderFactory f;
+    liblas::Reader reader = f.CreateWithStream(ifs);
+
+    liblas::Header const& header = reader.GetHeader();
+
+    long int nPts = header.GetPointRecordsCount();
+    std::cout << "Compressed:  " << (header.Compressed() == true) ? "true\n":"false\n";
+    std::cout << "\nSignature: " << header.GetFileSignature() << '\n';
+    std::cout << "Points count: " << nPts << '\n';
+
+
+
+    // Fill in the PCD cloud data
+    cloud->width    = nPts;
+    cloud->height   = 1;
+    cloud->is_dense = true;
+    cloud->points.resize (cloud->width * cloud->height);
+    int i = 0;
+
+    std::vector<double> PCPointsX;
+    std::vector<double> PCPointsY;
+    std::vector<double> PCPointsZ;
+
+
+    double minX = 9999999999, minY= 9999999999, minZ = 9999999999;
+
+
+    while (reader.ReadNextPoint()){
+        liblas::Point const& p = reader.GetPoint();
+
+
+        if (p.GetX() < minX)
+            minX = p.GetX();
+
+        if (p.GetY() < minY)
+            minY = p.GetY();
+
+        if (p.GetZ() < minZ)
+            minZ = p.GetZ();
+
+        // push in the original double values to subtract the minXYZ values from those
+        // befor!! casting them to float.
+        PCPointsX.push_back(p.GetX());
+        PCPointsY.push_back(p.GetY());
+        PCPointsZ.push_back(p.GetZ());
+
+        // in case the user has not specified the -setZero flag. Take the values as they
+        // are, without subtracting the min values and just cast them to float.
+        cloud->points[i].x = (float) p.GetX();
+        cloud->points[i].y = (float) p.GetY();
+        cloud->points[i].z = (float) p.GetZ();
+        cloud->points[i].intensity = p.GetIntensity();
+
+        if (i % 500000 == 0)
+            std::cout << i  << "  x: " << p.GetX() << "  y: " << p.GetY() << "  z: " << p.GetZ() << "\n";
+
+
+        i++;
+    }
+
+    // store the minValues so they can be added in the final processing step to get the true coordinates
+    minXYZValues.push_back(minX); minXYZValues.push_back(minY); minXYZValues.push_back(minZ);
+
+    // if true, subtract the min values now. This will normally be 'true'
+    if (subtractMinVals){
+      pcl::console::print_highlight("\n\nRemoving min x,y,z coordinate values.\n\n");
+      pcl::console::print_highlight("minX = %f  , minY = %f , minZ = %f \n\n", minX, minY, minZ);
+      for (int i=0; i< cloud->points.size(); ++i){
+          cloud->points[i].x = PCPointsX[i] - minX;
+          cloud->points[i].y = PCPointsY[i] - minY;
+          cloud->points[i].z = PCPointsZ[i] - minZ;
+
+          if (i % 500000 == 0){
+            std::cout << i  << "  x: " << cloud->points[i].x << "  y: " << cloud->points[i].y <<
+                         "  z: " << cloud->points[i].z  << "    intensity:  " << cloud->points[i].intensity << std::endl;
+          }
+      }
+    }
+
+
+    pcl::console::TicToc time;
+    time.tic ();
+
+    typename pcl::PointCloud<PointT>::Ptr cloudFiltered =  boost::make_shared<pcl::PointCloud<PointT> >();
+
+
+    if (gridLeafSize > 0.029 && gridLeafSize < 1){
+      std::cout << "\nApplying Uniform downsampling with leafSize " << gridLeafSize << ". Processing...";
+
+      pcl::UniformSampling<PointT> uniform_sampling;
+      uniform_sampling.setInputCloud (cloud);
+      uniform_sampling.setRadiusSearch (gridLeafSize); //the 3D grid leaf size
+      uniform_sampling.filter(*cloudFiltered);
+      std::cout << "Downsampled in " << time.toc() / 1000. << " s" << std::endl;
+      pcl::copyPointCloud(*cloudFiltered, *cloud);  // cloud is given by reference so the downsampled cloud has to be copied in there
+
+    }
+    else  // else copy original cloud in cloud Filtered and save file...
+        pcl::copyPointCloud(*cloud,*cloudFiltered);
+
+    std::string fileToWrite = fileToRead + ".pcd";
+    std::cout << "Writing PCD output file: " << fileToWrite << std::endl;
+    pcl::io::savePCDFile (fileToWrite, *cloudFiltered,true);
+    std::cerr << "Saved " << cloudFiltered->points.size () << " Points to " << fileToWrite << std::endl;
+
+
+}
+
+/*
+void
+viewer (pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud)
+{
+
+
+  pcl::visualization::CloudViewer viewer ("View Point cloud");
+  viewer.showCloud (cloud);
+  while (!viewer.wasStopped ())
+  {
+  }
+}
+*/
+
+
+void readLAS2PCLsubMinVals(std::string filename, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud, std::vector<double> &minValues,  int downSampleFactor ){
+
+    // reading data from LAS file:
+    // 1) create a file stream object to access the file
+
+    std::ifstream ifs;
+    ifs.open(filename.c_str(), std::ios::in | std::ios::binary);
+
+    liblas::ReaderFactory f;
+    liblas::Reader reader = f.CreateWithStream(ifs);
+
+    liblas::Header const& header = reader.GetHeader();
+
+    long int nPts = header.GetPointRecordsCount();
+    std::cout << "Compressed:  " << (header.Compressed() == true) ? "true\n":"false\n";
+    std::cout << "\nSignature: " << header.GetFileSignature() << '\n';
+    std::cout << "Points count: " << nPts << '\n';
+
+
+    minValues.push_back(header.GetMinX());
+    minValues.push_back(header.GetMinY());
+    minValues.push_back(header.GetMinZ());
+
+
+    // Fill in the PCD cloud data
+    if (downSampleFactor == 0){
+        cloud->width    = nPts;
+        downSampleFactor = 1;
+        std::cerr << "downSampleFactor may not be zero. Applying no downsampling. " << std::endl;
+    }
+    else if (downSampleFactor == 1){
+        cloud->width    = nPts;
+    }
+    else{
+        cloud->width    = int (nPts/downSampleFactor)+1;
+    }
+
+    cloud->height   = 1;
+    cloud->is_dense = true;
+    cloud->points.resize (cloud->width * cloud->height);
+
+    int i = 0;
+    int k = 0;
+
+    while (reader.ReadNextPoint()){
+          liblas::Point const& p = reader.GetPoint();
+
+          if (i % downSampleFactor == 0){
+              cloud->points[k].x = p.GetX() - minValues[0];
+              cloud->points[k].y = p.GetY() - minValues[1];
+              cloud->points[k].z = p.GetZ() - minValues[2];
+              cloud->points[k].intensity = p.GetIntensity();
+              ++k;
+          }
+
+          i++;
+      }
+
+}