initial commit

.gitignore

+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app

CMakeLists.txt

+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+set (CMAKE_CXX_STANDARD 11)
+
+project(detectWindows)
+
+find_package(PCL 1.8 REQUIRED)
+find_package(libLAS REQUIRED)
+find_package(vtk)
+find_package(OpenCV REQUIRED)
+
+include_directories(${PCL_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+add_executable (detectWindows ./main.cpp ./src/pcl_las_helper.hpp ./src/miscFunctions.hpp ./src/CondEuclClustering.hpp ./src/hull2d.hpp
+    ./h/extractfacade.h ./h/cloudsubtractor.h ./h/inrange.h ./h/areafromconcavehull.h ./h/removeconcavehullframe.h)
+target_link_libraries (detectWindows ${PCL_LIBRARIES} ${libLAS_LIBRARIES} ${OpenCV_LIBS})

README.md

+# DetectWindows
+## General information
+DetectWindows from point clouds of buildings from mobile mapping drives. HighRes point clouds are required, e.g. higher than what a Velodyne64 laser can achieve. However, the issue with the Velodyne scanners might not just be the lower resolution but the organized structure of the data that could lead to singularities (not confirmed, just an assumption).
+
+Detailed explaination of the steps of the method can be found in the following paper (in German):
+
+*Schneider, S., Coors, V. (2018). Automatische Extraktion von Fenstern in 3D Punktwolken mittels einer hierarchischen Methode. In T. P. Kersten, E. Gülch, J. Schiewe, T. H. Kolbe, & U. Stilla (Hrsg.), Wissenschaftlich-Technische Jahrestagung der DGPF und PFGK18 Tagung (pp. 559–572). Munich, Germany: Publikationen der DGPF.*
+https://www.dgpf.de/src/tagung/jt2018/proceedings/proceedings/papers/44_PFGK18_P03_Schneider_Coors.pdf
+
+**If you use this code for your work, please cite the work above.**
+
+## Requirements
+The code was developed on a Ubunutu 16.04 system and has only be build and tested on this system. Theoretically, the code should be able to be build and run on Windows, if all the dependencies are fulfilled - this was not tested.
+
+The following libraries and versions were used:
+
+* PCL 1.8 (and dependencies, such as Boost 1.5.8, Eigen3 3.2.92, Flann 1.8.4, etc.) http://www.pointclouds.org/downloads or https://larrylisky.com/2016/11/03/point-cloud-library-on-ubuntu-16-04-lts/
+* libLAS 1.8.1 (and dependencies) https://liblas.org/download.html
+* vtk 8.1 
+* OpenCV 3.2.0
+
+Note 1: You could remove the support for las files from the code in order to reduce the dependency on liblas because PCD and TXT files are also supported.
+
+Note 2: VTK is used for visualization of the intermediate steps. For batch processing this would not be required and VTK might be removed as well. However, note that PCL might need to be rebuild without VTK support in this case.
+
+Note 3: Only few routines from the OpenCV library were used in order to project the detected window corner points onto a 2d plain and estimate a bounding box. You might write your own routine to do this to reduce dependencies. 
+
+## Software description
+
+**Input**: is a single point cloud file as *.PCD, *.las or *.txt/*.xyz (tested only with unstructured / unorganized point clouds but others might work as well).
+Data needs to be stored in columns of X Y Z [intensity][R G B] for each point. Intensity and RGB is not used in this method.
+You can easily create your own PCD files from simple TXT / XYZ files be manually adding the header info. For this please see the documentation of the PCD file format at: http://pointclouds.org/documentation/tutorials/pcd_file_format.php
+Note that PCD files do not store information about the spatial reference system (SRS), you could however add a comment into the header and prcesses this by writing your own routines.
+**Units in the input file are assumed to be in meters, not degrees.**
+
+**Output**: Along the processing chain, certain point clouds are written to disc, such as the (concave) hull, the reduced input point cloud in PCD format, and so on (this can all be commented out to reduce clutter).
+Main outputs are: cloudWithWindows.pcd which contains all points of the contour found of the window found where each point beloing to the same window will have the same ID encoded as RGB values: e.g.
+```
+     X           Y           Z       R G B
+358951.02199 5701437.075 132.3170307 8 8 8
+358951.01914 5701437.074 132.6170413 8 8 8
+....
+```
+Also there is `STEP_05_bBox.pcd` which contains only the corner points of each window. Each corner point of a particular window will have the same ID. In this file, there are always 4 points associated with each window, thus 4 points having the same ID. 
+
+```
+    X         Y         Z     I
+358951.02199 5701437.075 132.3170307 8
+358952.01914 5701436.074 132.6170413 8 
+358953.02199 5701436.075 132.3170307 8 
+358951.01914 5701437.074 132.6170413 8
+```
+Note that the **IDs are incremented by a value of two starting from ID 4**. This has historical reasons and can be changed. I did not get aorund it yet.
+Also note that the **PCL only supports float data types** for point clouds (**not double**). This has some implications if data is stored in geographic SRS, as the values are very large and **precision will be lost. This leads to artifacts in your the data**. To solve this issue, data needs to be centered using the `-setZero` option (see Usage below).
+
+## Usage:
+run from shell as: .detectWindows /home/of/your/DATAset.las [-setZero] [-downsample 0.4]
+- setZero will center your point cloud to Xmin, Ymin, Zmin to avoid artifacts for data with large coordinates 
+- downsample FLOAT-VALUE : will reduce the voxel size by a given amount. Valid values are between [0.02,1]
+
+[argument] are optional. If point clouds are very large, e.g. > 10M points, it is a good idea to downsample the data so the program runs more stable and much faster. For large number of input points and large facades, you can quickly run out of memorey!
+
+
+## License
+The MIT License
+Copyright (c) 2018 Sven Schneider, Hochschule fuer Technik Stuttgart
+
+
+
+
+

cfg/CondEuclidClustering.cfg

+normalSearchRadius 0.075
+ClusterTolerance 0.3
+MinClusterSize 20
+MaxClusterSize 180

cfg/GreedyArea.cfg

+PlaneDetectionThreshold 0.15
+NormalEstimationSearchRadius 0.5
+Greedy_Mu 8.2
+Greedy_SearchRadiusLongestEdge 0.4
+Greedy_MaxNN 200
+Greedy_MaxSurfAngle 0.785375
+Greedy_MaxAngleInTriangle 2.356125
+Greedy_MinAngle 0.17452778
+Greedy_NormalConstistency 0

cfg/backup/CondEuclidClustering.cfg

+normalSearchRadius 0.075
+ClusterTolerance 0.3
+MinClusterSize 35
+MaxClusterSize 180

cfg/backup/customRegionGrowing.cfg

+squared_distance 10000
+threshold1 8
+threshold2 0.06
+threshold3 3

cfg/backup/hull2d.cfg

+Alpha 0.1

cfg/backup/issDetector.cfg

+NonMaxSuppresionRadius 0.5
+SalientRadius 0.75
+Gamma21 0.6
+Gamma32 1.
\ No newline at end of file

cfg/cloudSubtractor.cfg

+pointIncrement 0.3
+planeDistThreshold 0.05
+kdRadius 0.25
+	
\ No newline at end of file

cfg/customRegionGrowing.cfg

+squared_distance 10000
+threshold1 8
+threshold2 0.06
+threshold3 3

cfg/hull2d.cfg

+Alpha 0.1

cfg/issDetector.cfg

+NonMaxSuppresionRadius 0.5
+SalientRadius 0.75
+Gamma21 0.6
+Gamma32 1.
\ No newline at end of file

h/areafromconcavehull.h

+#ifndef AREAFROMCONCAVEHULL_H
+//PCL
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/sample_consensus/sac_model_line.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/common/pca.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/surface/mls.h>
+#include <pcl/surface/poisson.h>
+
+#include <pcl/surface/gp3.h>
+#include <pcl/features/normal_3d.h>
+
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/io/vtk_io.h>
+
+
+
+// STL
+#include <tgmath.h>
+
+// own
+#include "removeconcavehullframe.h"
+#include "../h/inrange.h"
+
+
+#define AREAFROMCONCAVEHULL_H
+
+
+template <typename PointInT>
+class AreaFromConcaveHull: public  pcl::PCLBase<PointInT>
+{
+
+protected:
+    typedef typename pcl::PointCloud<PointInT>::Ptr PointCloudPtr;
+    typedef typename pcl::PointCloud<PointInT>::ConstPtr PointCloudConstPtr;
+
+
+public:
+
+    typedef boost::shared_ptr< AreaFromConcaveHull<PointInT> > Ptr;
+    typedef boost::shared_ptr< const AreaFromConcaveHull<PointInT> > ConstPtr;
+
+
+    AreaFromConcaveHull () :
+          lineDistThreshold_ (0.1),
+          pointIncrement_ (0.1),
+          area_ (0.0),
+          kdRadius_ (1),
+          coefficients_ (new pcl::ModelCoefficients),
+          gridLine_ (new pcl::PointCloud<PointInT>),
+          extractedLine_ (new pcl::PointCloud<PointInT>)
+          { };
+
+  //  inline void setPointIncrement(float inc) {pointIncrement_ = inc;}
+    inline void setKdTreeRadius(float radius) {kdRadius_ = radius;}
+
+
+    inline void setLineDistThreshold(float threshold) {lineDistThreshold_ = threshold;}
+
+
+    void greedyArea(float &area);
+    void detectLines(typename pcl::PointCloud<PointInT>::Ptr cloud_out);
+    void calculateAreaFromHull(typename pcl::PointCloud<PointInT>::Ptr cloudOut);
+    void makeGridLine();
+
+    virtual ~AreaFromConcaveHull ()
+    {
+
+    }
+
+protected:
+
+    float kdRadius_;
+    float area_;
+    /** \brief The grid spacing in x,yz, direction. Smaller values lead to to larger and denser point clouds (default: 0.25). */
+    const float pointIncrement_;
+
+    float lineDistThreshold_;
+    /** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */
+    using pcl::PCLBase<PointInT>::input_;
+    /** \brief The subtracted (inverted) point cloud. */
+    typename pcl::PointCloud<PointInT>::Ptr extractedLine_;
+    typename pcl::PointCloud<PointInT>::Ptr gridLine_;
+
+    pcl::ModelCoefficients::Ptr coefficients_;
+
+
+};
+
+template <typename PointInT>
+void AreaFromConcaveHull<PointInT>::greedyArea(float &area){
+
+    // read in parameter file
+    std::vector<float> params;
+    readParamsFromFile("../cfg/GreedyArea.cfg", params, true);
+
+        typename pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals (new pcl::PointCloud<pcl::PointNormal>);
+    typename pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormalsMLS (new pcl::PointCloud<pcl::PointNormal>);
+        typename pcl::PointCloud<pcl::PointXYZ>::Ptr inputXYZ (new pcl::PointCloud<pcl::PointXYZ>);
+        typename pcl::PointCloud<pcl::PointXYZ>::Ptr filteredXYZ (new pcl::PointCloud<pcl::PointXYZ>);
+
+        // manually copy all points over to pointXYZ data type
+        pcl::PointXYZ p;
+        for (int i = 0; i < input_->points.size(); i++){
+            p.x= input_->points[i].x;
+            p.y= input_->points[i].y;
+            p.z= input_->points[i].z;
+
+            inputXYZ->points.push_back(p);
+        }
+        inputXYZ->width = inputXYZ->points.size();
+        inputXYZ->height = 1;
+
+        // Create the filtering object
+          pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
+          sor.setInputCloud (inputXYZ);
+          sor.setMeanK (50);
+          sor.setStddevMulThresh (0.75);
+          sor.filter (*inputXYZ);
+
+
+        pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
+        // Create the segmentation object
+        pcl::SACSegmentation<pcl::PointXYZ> seg;
+        // Optional
+        seg.setOptimizeCoefficients (true);
+        // Mandatory
+        seg.setModelType (pcl::SACMODEL_PLANE);
+        seg.setMethodType (pcl::SAC_RANSAC);
+        seg.setDistanceThreshold (params[0]);
+        seg.setInputCloud (inputXYZ);
+        seg.segment (*inliers, *coefficients_);
+
+        pcl::ExtractIndices<pcl::PointXYZ> extract;
+        extract.setInputCloud (inputXYZ);
+            extract.setIndices (inliers);
+            extract.setNegative (false);
+//            extract.filter (*filteredXYZ);
+
+           extract.filter (*inputXYZ);
+
+
+           // estimate normals
+           typename  pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne;
+           ne.setInputCloud (inputXYZ);
+           typename pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
+           ne.setSearchMethod (tree);
+           // Use all neighbors in a sphere of radius 3cm
+            ne.setRadiusSearch (params[1]);
+            ne.compute (*cloudWithNormals);
+
+            // concaternate the normals and the input xyz data
+            pcl::concatenateFields(*inputXYZ, *cloudWithNormals, *cloudWithNormals);
+
+/*
+            typename pcl::search::KdTree<pcl::PointXYZ>::Ptr treemls (new pcl::search::KdTree<pcl::PointXYZ> ());
+
+                        // Init object (second point type is for the normals, even if unused)
+
+
+           //viewer<pcl::PointXYZ> (inputXYZ);
+            pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls;
+             mls.setInputCloud (inputXYZ);
+             mls.setComputeNormals(true);
+             mls.setSearchRadius (0.1);
+             mls.setSearchMethod(treemls);
+             mls.setPolynomialFit (false);
+             //mls.setPolynomialOrder (2);
+             //mls.setUpsamplingMethod (pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointXYZ>::RANDOM_UNIFORM_DENSITY);
+             //mls.setUpsamplingMethod (pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointXYZ>::VOXEL_GRID_DILATION);
+             //mls.setDilationVoxelSize (0.1);
+             //mls.setDilationIterations(5);
+             //mls.setUpsamplingRadius (0.5);
+             //mls.setUpsamplingStepSize (0.3);
+              // Reconstruct
+              std::cout << "starting MovingLeastSqr Normal Estimation and Smoothing...";
+              mls.process (*cloudWithNormalsMLS);
+              std::cout << "Process completed." << std::endl;
+             // viewer<pcl::PointXYZ> (*cloudWithNormalsMLS);
+*/
+
+
+
+
+
+
+         
+           typename pcl::search::KdTree<pcl::PointNormal>::Ptr tree1 (new pcl::search::KdTree<pcl::PointNormal> ());
+        pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp;
+        pcl::PolygonMesh mesh;
+
+
+        // GreedyTriangulation
+        gp.setInputCloud(cloudWithNormals);
+        gp.setSearchMethod(tree1);
+        /*gp.setMu(2.20);
+        gp.setSearchRadius(10.40);
+        gp.setMaximumNearestNeighbors(500);
+        gp.setMaximumSurfaceAngle(M_PI/4);
+        gp.setMaximumAngle(3*M_PI/4);
+        gp.setMinimumAngle(M_PI/18);
+        */
+        gp.setMu(params[2]);
+        gp.setSearchRadius(params[3]);
+        gp.setMaximumNearestNeighbors(params[4]);
+        gp.setMaximumSurfaceAngle(params[5]);
+        gp.setMaximumAngle(params[6]);
+        gp.setMinimumAngle(params[7]);
+        gp.setNormalConsistency(bool(params[8]));
+        std::cout << "\nstarting reconstructing surface mesh..." << std::endl;
+        std::cout << " \n";
+        gp.reconstruct(mesh);
+        std::cout << "Process completed." << std::endl;
+        /**/
+
+/*
+         pcl::Poisson<pcl::PointNormal> poisson;
+               poisson.setDepth(9);
+
+               poisson.setInputCloud(cloudWithNormals);
+               poisson.reconstruct(mesh);
+*/
+        cout<<"The size of polygons is "<<mesh.polygons.size()<<endl;
+        pcl::io::saveVTKFile("potting-reconstruction.vtk",mesh);
+        //pcl::io::savePLYFile("potting-reconstruction.ply",mesh);
+
+        //calculate area;
+        pcl::PointCloud<pcl::PointXYZ> cloud1;
+        cloud1=*inputXYZ;
+
+        int index_p1,index_p2,index_p3;
+        float x1,x2,x3,y1,y2,y3,z1,z2,z3,a,b,c,q;
+
+
+        for(int i=0;i<mesh.polygons.size();++i)
+        {
+              index_p1=mesh.polygons[i].vertices[0];
+              index_p2=mesh.polygons[i].vertices[1];
+              index_p3=mesh.polygons[i].vertices[2];
+
+              x1=cloud1.points[index_p1].x;
+              y1=cloud1.points[index_p1].y;
+              z1=cloud1.points[index_p1].z;
+
+              x2=cloud1.points[index_p2].x;
+              y2=cloud1.points[index_p2].y;
+              z2=cloud1.points[index_p2].z;
+
+              x3=cloud1.points[index_p3].x;
+              y3=cloud1.points[index_p3].y;
+              z3=cloud1.points[index_p3].z;
+
+                      //Heron's formula;
+              a=sqrt(pow((x1-x2),2)+pow((y1-y2),2)+pow((z1-z2),2));
+              b=sqrt(pow((x1-x3),2)+pow((y1-y3),2)+pow((z1-z3),2));
+              c=sqrt(pow((x3-x2),2)+pow((y3-y2),2)+pow((z3-z2),2));
+              q=(a+b+c)/2;
+
+              float radizierer = q*(q-a)*(q-b)*(q-c);
+              if (radizierer < 0)
+                  std::cout << "negativ of sqrt not allowed" << std::endl; //area=area+sqrt(fabs(radizierer));
+              else
+                  area=area+sqrt(radizierer);
+
+        }
+        std::cout << area << std::endl;
+        area_ = area;
+
+
+        boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer Greedy Mesh Reconstruction"));
+        viewer->setBackgroundColor (0, 0, 0);
+        viewer->addPolygonMesh(mesh,"meshes",0);
+        viewer->addCoordinateSystem (1.0);
+        viewer->initCameraParameters ();
+        while (!viewer->wasStopped ()){
+            viewer->spinOnce (100);
+            boost::this_thread::sleep (boost::posix_time::microseconds (100000));
+        }
+
+
+}
+
+template <typename PointInT>
+void AreaFromConcaveHull<PointInT>::makeGridLine(){
+
+
+    PointInT startPt, endPt, minPt, maxPt;
+    pcl::getMinMax3D(*extractedLine_, minPt, maxPt);
+
+
+    size_t nPts = extractedLine_->points.size();
+    startPt = extractedLine_->points[0];
+    endPt = extractedLine_->points[nPts-1];
+
+    ////////// detect plane
+    ////////////////////////////////
+    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
+    // Create the segmentation object
+    pcl::SACSegmentation<PointInT> seg;
+    // Optional
+    seg.setOptimizeCoefficients (true);
+    // Mandatory
+    seg.setModelType (pcl::SACMODEL_PLANE);
+    seg.setMethodType (pcl::SAC_RANSAC);
+    seg.setDistanceThreshold (lineDistThreshold_);
+    seg.setInputCloud (input_);
+    seg.segment (*inliers, *coefficients_);
+
+    std::cout << "Normalized coeffs of Pt-Normal Eq. (Ax+By+Cz=D): " << coefficients_->values[0] << " " << coefficients_->values[1] << " " << coefficients_->values[2] << " " << coefficients_->values[3] << std::endl;
+
+    float z = (startPt.z + endPt.z) / 2.0;
+    PointInT point;
+    // construct new points on the plane by rearranging the point normal equation for one of the coordinates.
+       for (float x=minPt.x; x<=maxPt.x; x+=pointIncrement_)
+        {
+          for (float y=minPt.y; y<= maxPt.y; y+=pointIncrement_)
+          {
+
+                point.x = x;
+
+                if (coefficients_->values[1] != 0) // to avoid division by zero
+                    point.y = -1.0 * (1.0 * coefficients_->values[3] + coefficients_->values[0] * x + coefficients_->values[2] * z  ) / coefficients_->values[1] ;
+                else  // divisor is set to 0.0000001 if the coefficient is zero
+                    point.y = -1.0 * (1.0 * coefficients_->values[3] + coefficients_->values[0] * x + coefficients_->values[2] * z  ) / 0.000001 ;
+
+                point.z = z;
+                gridLine_->points.push_back (point);
+          }
+        }
+
+    std::cerr << "GridLine_ has: " << gridLine_->points.size () << " points. " <<  std::endl;
+    viewer<PointInT> (gridLine_);
+
+
+
+
+}
+
+template <typename PointInT>
+void AreaFromConcaveHull<PointInT>::detectLines(typename pcl::PointCloud<PointInT>::Ptr cloudOut){
+
+    typename pcl::PointCloud<PointInT>::Ptr cloudFiltered (new pcl::PointCloud<PointInT>);
+
+    pcl::copyPointCloud(*input_, *cloudFiltered);
+    pcl::ExtractIndices<PointInT> extract;
+
+      pcl::PointIndices::Ptr inliers (new pcl::PointIndices);      
+      // Create the segmentation object
+      pcl::SACSegmentation<PointInT> seg;
+      // Optional
+      seg.setOptimizeCoefficients (true);
+      // Mandatory
+      //seg.setModelType (pcl::SACMODEL_LINE);
+      seg.setModelType(pcl::SACMODEL_LINE);
+      seg.setMethodType (pcl::SAC_RANSAC);
+      seg.setDistanceThreshold (lineDistThreshold_);
+
+
+      seg.setInputCloud (cloudFiltered);
+      seg.segment (*inliers, *coefficients_);
+      std::cerr << "PointCloud after LINE segmentation has: " << inliers->indices.size () << " inliers." << std::endl;
+
+
+      // Extract the inliers
+         extract.setInputCloud (cloudFiltered);
+         extract.setIndices (inliers);
+         extract.setNegative (false);
+         extract.filter (*extractedLine_);
+         viewer<PointInT> (extractedLine_);
+
+         Eigen::VectorXf coeffs_;
+
+          std::vector<int> indx;
+          std::vector<double> distToModel;
+          indx.push_back(*inliers->indices.begin());
+          indx.push_back(*inliers->indices.end());
+          pcl::SampleConsensusModelLine<PointInT> segLine(input_, false);
+          segLine.setInputCloud(input_);
+          segLine.computeModelCoefficients(indx,coeffs_);
+
+          segLine.getDistancesToModel(coeffs_, distToModel);
+          std::cout << coeffs_ << std::endl;
+
+
+
+         makeGridLine();
+
+         pcl::copyPointCloud(*gridLine_, *cloudOut);
+
+
+
+}
+
+
+#endif // AREAFROMCONCAVEHULL_H

h/cloudsubtractor.h

+#ifndef CLOUDSUBTRACTOR_H
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/extract_indices.h>
+
+#include <pcl/visualization/pcl_visualizer.h>
+
+
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/common/pca.h>
+#include "../h/inrange.h"
+//#include "../src/miscFunctions.hpp"
+#define CLOUDSUBTRACTOR_H
+
+// by Sven Schneider 07/2017
+
+template <typename PointInT>
+class CloudSubtractor: public pcl::PCLBase<PointInT>
+{
+
+protected:    
+    typedef typename pcl::PointCloud<PointInT>::Ptr PointCloudPtr;
+    typedef typename pcl::PointCloud<PointInT>::ConstPtr PointCloudConstPtr;
+
+
+public:
+
+    typedef boost::shared_ptr< CloudSubtractor<PointInT> > Ptr;
+    typedef boost::shared_ptr< const CloudSubtractor<PointInT> > ConstPtr;
+
+
+    CloudSubtractor () :
+          pointIncrement_ (0.25),
+          planeDistThreshold_ (0.1),
+          kdRadius_ (1),
+          kdRadiusPCA_ (1),
+          coefficients_ (new pcl::ModelCoefficients),
+          gridPC_ (new pcl::PointCloud<PointInT>),
+          invertedPC_ (new pcl::PointCloud<PointInT>)
+          { };
+
+    CloudSubtractor (float pointInc, float planeDist, float kdR) :
+          pointIncrement_ (pointInc),
+          planeDistThreshold_ (planeDist),
+          kdRadius_ (kdR),
+          kdRadiusPCA_ (1),
+          coefficients_ (new pcl::ModelCoefficients),
+          gridPC_ (new pcl::PointCloud<PointInT>),
+          invertedPC_ (new pcl::PointCloud<PointInT>)
+          { };
+
+    inline void setPointIncrement(float inc) {pointIncrement_ = inc;}
+    inline void setKdTreeRadius(float radius) {kdRadius_ = radius;}
+    inline void setKdTreeRadiusPCA(float radius) {kdRadiusPCA_ = radius;}
+    inline void setPlaneDistThreshold(float threshold) {planeDistThreshold_ = threshold;}
+    inline float getPointIncrement() {return pointIncrement_;}    
+    inline float getPlaneDistThreshold() {return planeDistThreshold_;}
+    inline float getKdTreeRadius() {return kdRadius_;}
+
+    void dummySearch();
+    void applyCloudSubtraction(typename pcl::PointCloud<PointInT>::Ptr cloud_out);
+    void pointwisePCA(typename pcl::PointCloud<PointInT>::Ptr cloudOut);
+
+
+
+
+    virtual ~CloudSubtractor ()
+    {
+
+    }
+
+protected:
+
+    float kdRadius_;
+    float kdRadiusPCA_;
+    /** \brief The grid spacing in x,yz, direction. Smaller values lead to to larger and denser point clouds (default: 0.25). */
+    float pointIncrement_;
+
+    float planeDistThreshold_;
+    /** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */
+    using pcl::PCLBase<PointInT>::input_;
+    /** \brief The subtracted (inverted) point cloud. */
+    typename pcl::PointCloud<PointInT>::Ptr invertedPC_;
+    typename pcl::PointCloud<PointInT>::Ptr gridPC_;
+
+    pcl::ModelCoefficients::Ptr coefficients_;
+
+    std::vector<size_t> sort_indexes(const std::vector<float> &v);
+    void detectPlane();
+    void makePlaneFromCoefficients();
+    void gridSubtraction();
+
+
+};
+
+
+template <typename PointInT>
+std::vector<size_t> CloudSubtractor<PointInT>::sort_indexes(const std::vector<float> &v) {
+
+  // initialize original index locations
+  std::vector<size_t> idx(v.size());
+  std::iota(idx.begin(), idx.end(), 0);
+
+  // sort indexes based on comparing values in v
+  std::sort(idx.begin(), idx.end(),
+       [&v](size_t i1, size_t i2) {return v[i1] < v[i2];} );
+
+  return idx;
+}
+
+template <typename PointInT>
+void CloudSubtractor<PointInT>::dummySearch ()//, typename pcl::PointCloud<PointInT>::Ptr invertedPC_)
+{
+    //typename pcl::PointCloud<PointT>::Ptr cloud_out (new pcl::PointCloud<PointT>);
+    pcl::copyPointCloud(*input_,*invertedPC_);
+    for (int i=0; i< input_->size(); ++i){
+        invertedPC_->points[i].x = input_->points[i].x + 3;
+        invertedPC_->points[i].y = input_->points[i].y + 5;
+    }
+}
+
+template <typename PointInT>
+void CloudSubtractor<PointInT>::applyCloudSubtraction (typename pcl::PointCloud<PointInT>::Ptr cloud_out)
+{
+    detectPlane();
+    makePlaneFromCoefficients();
+    gridSubtraction();
+    //viewer<PointInT> (invertedPC_);
+    pcl::copyPointCloud(*invertedPC_, *cloud_out);
+
+}
+
+template <typename PointInT>
+void CloudSubtractor<PointInT>::detectPlane(){
+
+    typename pcl::PointCloud<PointInT>::Ptr cloudFiltered (new pcl::PointCloud<PointInT>);
+    typename pcl::PointCloud<PointInT>::Ptr tmpPlane (new pcl::PointCloud<PointInT>);
+
+
+        pcl::console::TicToc tt;
+/*      // Create the filtering object
+        pcl::VoxelGrid<PointInT> sor;
+        sor.setInputCloud (input_);
+        sor.setLeafSize (0.1f, 0.1f, 0.1f);
+        tt.tic();
+        sor.filter (*cloud_filtered);
+        std::cerr << "PointCloud after VoxelGrid Downsampling has: " << cloud_filtered->size() << " points. Took " << tt.toc() / 1000. << " s." << std::endl;
+*/
+    pcl::copyPointCloud(*input_,*cloudFiltered);
+
+
+
+      //pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
+    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
+    // Create the segmentation object
+    pcl::SACSegmentation<PointInT> seg;
+    // Optional
+    seg.setOptimizeCoefficients (true);
+      // Mandatory
+      seg.setModelType (pcl::SACMODEL_PLANE);
+      seg.setMethodType (pcl::SAC_RANSAC);
+      seg.setDistanceThreshold (planeDistThreshold_);
+
+      tt.tic ();
+      seg.setInputCloud (cloudFiltered);
+      seg.segment (*inliers, *coefficients_);
+      std::cerr << "PointCloud after plane segmentation has: " << inliers->indices.size () << " inliers. Took " << tt.toc() / 1000. << " s." << std::endl;
+
+      std::cout << "Normalized coeffs of Pt-Normal Eq. (Ax+By+Cz=D): " << coefficients_->values[0] << " " << coefficients_->values[1] << " " << coefficients_->values[2] << " " << coefficients_->values[3] << std::endl;
+
+      //TODO: Detect 1 more plane: ~30 cm +/- 10cm before (direction of the street) the facadePlane
+      // this is done to detect Erkers.
+      //TODO: Combine both facadePoint clouds
+
+      // For DEBUGING ONLY
+      // Extract the planar inliers from the input cloud
+      typename pcl::ExtractIndices<PointInT> extract;
+      extract.setInputCloud (cloudFiltered);
+      extract.setIndices (inliers);
+      extract.setNegative (false);
+      // Get the points associated with the planar surface
+      extract.filter (*tmpPlane);
+
+      std::stringstream ss;
+      pcl::PCDWriter writer;
+      ss << "STEP_03_moreRefinedPlane_.pcd";
+      //writer.write<PointInT> (ss.str (), *tmpPlane, false); //*
+
+
+
+
+}
+
+template <typename PointInT>
+void CloudSubtractor<PointInT>::makePlaneFromCoefficients()
+{
+    PointInT minPt;
+    PointInT maxPt;
+    pcl::getMinMax3D(*input_, minPt, maxPt);
+
+    //sort coefficients and get indices
+    std::vector<float> v = {0, 0 ,0 };
+    for (int i = 0; i < 3; i++)
+        v[i] = fabs(coefficients_->values[i]);
+
+    std::vector<size_t> idx = sort_indexes(v);
+
+    //point.x = -1.0 * (1.0 * coefficients_->values[3] + coefficients_->values[1] * y + coefficients_->values[2] * z  ) / coefficients_->values[idx[2]] ;
+
+
+    PointInT point;
+    // construct new points on the plane by rearranging the point normal equation for one of the coordinates.
+       for (float x=minPt.x; x<=maxPt.x; x+=pointIncrement_)
+        {
+          for (float y=minPt.y; y<= maxPt.y; y+=pointIncrement_)
+          {
+              for(float z=minPt.z; z <= maxPt.z ; z+=pointIncrement_)
+              {
+            // get the largest component of the plane normal to set the normal for the new grid plane and generate plane points in the plane perpendicular to the normal vector
+                    if ( idx[2] == 0 ){
+                        point.x = -1.0 * (1.0 * coefficients_->values[3] + coefficients_->values[1] * y + coefficients_->values[2] * z  ) / coefficients_->values[0] ;
+                        point.y = y;
+                        point.z = z;
+                    }
+                    if ( idx[2] == 1 ){
+                        point.x = x;
+                        point.y = -1.0 * (1.0 * coefficients_->values[3] + coefficients_->values[0] * x + coefficients_->values[2] * z  ) / coefficients_->values[1] ;
+                        point.z = z;
+                    }
+                    if ( idx[2] == 2 ){
+                        point.x = x;
+                        point.y = y;
+                        point.z = -1.0 * (1.0 * coefficients_->values[3] + coefficients_->values[0] * x + coefficients_->values[1] * y  ) / coefficients_->values[2] ;
+                    }
+
+                gridPC_->points.push_back (point);
+              }
+          }
+        }
+
+        gridPC_->width = (int) gridPC_->points.size ();  gridPC_->height = 1;
+        std::cout << "size of gridPlane: " << gridPC_->points.size() << std::endl;
+
+        /*
+        //////////////////////////////////////////////////////////////
+        // Visualization of keypoints along with the original cloud
+         pcl::visualization::PCLVisualizer viewer("PCL Viewer");
+         pcl::visualization::PointCloudColorHandlerCustom<PointT> keypoints_color_handler (gridPC_, 0, 255, 0);
+         viewer.setBackgroundColor( 0.0, 0.0, 0.0 );
+         viewer.addPointCloud(gridPC_, keypoints_color_handler, "keypoints");
+         viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "keypoints");
+
+        while(!viewer.wasStopped ())
+          viewer.spinOnce ();
+              */
+
+        std::stringstream ss;
+        pcl::PCDWriter writer;
+        ss << "STEP_04_PlaneFromCoeffs.pcd";
+       // writer.write<PointInT> (ss.str (), *gridPC_, false); //*
+
+
+}
+
+template <typename PointInT>
+void CloudSubtractor<PointInT>::gridSubtraction()
+{
+    pcl::console::TicToc tt;
+    typename pcl::PointCloud<PointInT>::Ptr cloud (new pcl::PointCloud<PointInT>);
+    pcl::copyPointCloud(*input_,*cloud);  // make a copy of original cloud
+
+    // create a kdtree object
+    pcl::KdTreeFLANN<PointInT> 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
+    size_t nIters = gridPC_->size();
+
+    //typename pcl::PointCloud<PointInT>::iterator pointIdxZERO;
+
+    tt.tic();
+    #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 PointInT searchPoint = gridPC_->points[p];
+
+        // if no neighbours were found add point to result point cloud;.
+        if ( !(kdtreeObj.radiusSearch (searchPoint, kdRadius_, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 1) )
+        {
+                    invertedPC_->points.push_back(searchPoint);
+                    if (p % 100000 == 0)
+                        std::cout << "Performed " << p+1 << " kdtree searches out of "
+                              << nIters << ". " << std::setprecision(2)
+                              << float((p+1.))/float(nIters)*100.0 << " % completed..." << std::endl;
+
+        }
+
+    }
+
+    std::cout << "gridSearch took: " << tt.toc() / 1000. << " seconds." << std::endl;
+}
+
+
+template <typename PointInT>
+void CloudSubtractor<PointInT>::pointwisePCA(typename pcl::PointCloud<PointInT>::Ptr cloudOut)
+{
+
+    pcl::console::TicToc tt;
+    typename pcl::PointCloud<pcl::PointNormal>::Ptr visCloud (new pcl::PointCloud<pcl::PointNormal>);
+
+    // create a kdtree object
+    pcl::KdTreeFLANN<PointInT> kdtreeObj;
+    // set the input cloud for the kdtree. this should start creating the tree.
+    kdtreeObj.setInputCloud (input_);
+    pcl::IndicesPtr indices (new std::vector <int>);
+    std::vector<float> pointRadiusSquaredDistance;
+
+    // create PCA object
+    pcl::PCA<PointInT> pcaObj;
+    pcaObj.setInputCloud(input_);
+
+    // determine size of PC before entering for, as the size changes by one during the loop.
+    size_t cloudSize = input_->size();
+    Eigen::Vector3f EigenVals;
+
+    tt.tic();
+    #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 < cloudSize; p++){
+
+        PointInT searchPoint = input_->points[p];
+        pcl::PointNormal visPoint;  // this allows to store Lambda0,1,2 in the normal_x,_y,_z values
+
+        // if neighbours were found, calculate PCA for this neighbourhood
+        if ( (kdtreeObj.radiusSearch (searchPoint, kdRadiusPCA_, *indices, pointRadiusSquaredDistance) > 3) )
+        {
+                pcaObj.setIndices(indices);
+                EigenVals = pcaObj.getEigenValues();
+
+               const float alpha = EigenVals(0) +EigenVals(1) + EigenVals(2);
+
+
+               // store planarity value in intensity field of point cloud for easy visualisation                
+               searchPoint.intensity = EigenVals(0)/alpha;
+
+                visPoint.x = searchPoint.x;
+                visPoint.y = searchPoint.y;
+                visPoint.z = searchPoint.z;                
+                visPoint.normal_x = EigenVals(0)/alpha;
+                visPoint.normal_y = EigenVals(1)/alpha;
+                visPoint.normal_z = EigenVals(2)/alpha;
+
+
+                // curvature is used to save to classification results based on thresholds
+                if ( inrange(0.62, 0.7).contains(visPoint.normal_x) && inrange(0.17, 0.3).contains(visPoint.normal_y) && inrange(0.0, 0.1).contains(visPoint.normal_z)  )
+                { // basically test if L0~2/3 and L1~1/3 and L2~0,  where L=Lambda (Eigenvalues)
+                    visPoint.curvature = 0.25; // its a border point, labeled as 0.25
+                    /*if (p % 1000 == 0)
+                        std::cout << "Border" << std::endl;*/
+                }
+
+                else if ( inrange(0.20, 0.3).contains(visPoint.normal_x) && inrange(0.20, 0.3).contains(visPoint.normal_y) && inrange(0.1, 0.3).contains(visPoint.normal_z)  )
+                { // basically test if L0 ~ L1 ~ L2 ~= 1/3
+                    visPoint.curvature = 0.5; //its a corner point labeled as 0.5
+                    /*if (p % 1000 == 0)
+                        std::cout << "Corner" << std::endl;*/
+                }
+                else if ( inrange(0.8, 1.1).contains(visPoint.normal_x) && inrange(0.0, 0.1).contains(visPoint.normal_y) && inrange(0.0, 0.1).contains(visPoint.normal_z)  )
+                {    // basically test if L0 ~ 1 and L2 L1 = 0
+                    visPoint.curvature = 0.75; //its a line point labeld as 0.75
+                    /*if (p % 1000 == 0)
+                        std::cout << "Line" << std::endl;*/
+                }
+                else{
+                    visPoint.curvature = 0.0; //its an inlier labeld as 0
+                   /* if (p % 1000 == 0)
+                       std::cout << "Inlier" << std::endl;  */
+                }
+
+                invertedPC_->points.push_back(searchPoint);  // push_back the point with planarity info into result PC
+                visCloud->points.push_back(visPoint);  // cloud vor visualization, take generated point and add it to cloudvector
+
+                // user info - output
+                if (p % 100000 == 0)  // give processing info to the user...
+                    std::cout << "PCA Performed. " << p+1 << " kdtree searches out of "
+                          << cloudSize << ". " << std::setprecision(3)
+                          << float((p+1.))/float(cloudSize)*100.0 << " % completed..." << std::endl;
+
+
+
+        }
+        indices->clear();  // clear indices for new neighbour search
+
+    }
+    // set basic cloud information.
+    invertedPC_->width = invertedPC_->points.size();
+    invertedPC_->height = 1;
+
+    visCloud->width = visCloud->points.size();
+    visCloud->height = 1;
+
+
+    // write the data to a file. This will be removed in a final version... or?
+    std::cout << "pointwisePCA took: " << std::setprecision(5) << tt.toc() / 1000. << " seconds." << std::endl;
+    if (invertedPC_->points.size() > 0){
+      pcl::PCDWriter writer;
+      writer.write<PointInT> ("pca_cloud.pcd", *invertedPC_, false);
+    }
+    // view result. Will be removed in final version??
+    viewer<PointInT>(invertedPC_);
+
+  /////////////////////////////////////////////////
+  // creating point cloud for visualization
+  ///////////////////////////////////////////////
+    if (visCloud->points.size() > 0){
+      pcl::PCDWriter writer;
+      writer.write<pcl::PointNormal> ("pca_visCloud.pcd", *visCloud, false);
+    }
+
+    for (size_t i=0; i < visCloud->points.size(); i++)
+    {
+
+        if (visCloud->points[i].curvature == 0.25)
+        {
+            pcl::PointXYZI borderPoint;
+            borderPoint.x = visCloud->points[i].x;
+            borderPoint.y = visCloud->points[i].y;
+            borderPoint.z = visCloud->points[i].z;
+            borderPoint.intensity = visCloud->points[i].normal_y;
+            cloudOut->points.push_back(borderPoint);
+        }
+    }
+    cloudOut->width = cloudOut->points.size();
+    cloudOut->height = 1;
+
+}
+
+#endif // CLOUDSUBTRACTOR_H

h/extractfacade.h

+#ifndef EXTRACTFACADE_H
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/ModelCoefficients.h>
+
+#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+
+
+
+#define EXTRACTFACADE_H
+
+
+template<typename T>
+class extractFacade : protected pcl::PCLBase<T>
+{
+
+public:
+
+    extractFacade(typename pcl::PointCloud<T>::Ptr In, std::vector< typename pcl::PointCloud<T> > Out, float distanceThresh) :
+        input_(In),
+        facades_(Out),
+        distThresh_(distanceThresh),
+        grndOffset_ (1.)
+        {};
+
+    // public functions
+    inline void setGrndOffset(float offset) {grndOffset_ = offset;}
+    bool applyFilter( std::vector< typename pcl::PointCloud<T> > & Out) { detectFacades(); Out = facades_; }  //return true if within range else return false.
+private:
+
+    // member variables
+     typename pcl::PointCloud<T>::Ptr input_;
+     std::vector< pcl::PointCloud<T> > facades_;
+     float distThresh_;
+     float grndOffset_;
+
+     // member functions
+     void detectFacades();
+
+
+};
+
+
+template<typename T>
+extractFacade<T> getFacades(typename pcl::PointCloud<T>::Ptr In,  std::vector< typename pcl::PointCloud<T> > Out, float distanceThresh)
+{
+    return extractFacade<T>(In, Out, distanceThresh);
+}
+
+template<typename T>
+void extractFacade<T>::detectFacades()
+{
+
+        std::cout << "starting to detect planes in point cloud..." << std::endl;
+        pcl::console::TicToc tt;
+        tt.tic();
+
+
+      typename pcl::PointCloud<T>::Ptr cloud_f (new pcl::PointCloud<T>);
+
+
+      typename pcl::PointCloud<T>::Ptr cloudFiltered (new pcl::PointCloud<T>);
+/*
+      pcl::VoxelGrid<T> vg;
+      vg.setInputCloud (input_);
+      vg.setLeafSize (0.1f, 0.1f, 0.1f);
+      vg.filter (*cloudFiltered);
+      std::cout << "PointCloud after filtering has:  " << cloudFiltered->points.size ()  << " data points." << std::endl; //*
+*/
+
+        pcl::copyPointCloud(*input_,*cloudFiltered);
+        /*
+      // Create the filtering object
+        typename pcl::StatisticalOutlierRemoval<T> sor;
+        sor.setInputCloud (cloudFiltered);
+        sor.setMeanK (25);
+        sor.setStddevMulThresh (0.5);
+        sor.filter (*cloudFiltered);
+*/
+
+      // Create the segmentation object for the planar model and set all the parameters
+      typename pcl::SACSegmentation<T> seg;
+      pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
+      pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
+      typename pcl::PointCloud<T>::Ptr cloudPlane (new pcl::PointCloud<T> ());
+
+      seg.setOptimizeCoefficients (true);
+      seg.setModelType (pcl::SACMODEL_PLANE);
+      seg.setMethodType (pcl::SAC_RANSAC);
+      seg.setMaxIterations (500);
+      seg.setDistanceThreshold (distThresh_);
+
+      int i=0, nr_points = (int) cloudFiltered->points.size ();
+      while (cloudFiltered->points.size () > 0.25 * nr_points)
+      {
+        // Segment the largest planar component from the remaining cloud
+        seg.setInputCloud (cloudFiltered);
+        seg.segment (*inliers, *coefficients);
+        if (inliers->indices.size () == 0)
+        {
+          std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
+          break;
+        }
+
+        // Extract the planar inliers from the input cloud
+        typename pcl::ExtractIndices<T> extract;
+        extract.setInputCloud (cloudFiltered);
+        extract.setIndices (inliers);
+        extract.setNegative (false);
+
+        // Get the points associated with the planar surface
+        extract.filter (*cloudPlane);
+
+        // see if the average z value of the detected plane is within +/- grndOffset_ (5m default) meters of the minPt of the entire pointcloud;
+        pcl::CentroidPoint<T> centroid;
+        T minPt;
+        T maxPt;
+        pcl::getMinMax3D(*cloudPlane, minPt, maxPt);
+        // get coefficients of plane to get the normal vector to see if it is point upward (road) or sideways (vertical plane)
+        const Eigen::Vector3f normVec (coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2));
+        const Eigen::Vector3f w(0,0,1); // road normal vector. assuming road is parallel to x-y plane
+        for (size_t c=0; c < cloudPlane->points.size(); c++)
+            centroid.add(cloudPlane->points[c]);
+
+        T centPt;
+        centroid.get(centPt);
+        //if ( (centPt.z < minPt.z + grndOffset_ &&  centPt.z > minPt.z - grndOffset_) && normVec.dot(w) > 0.9  )
+        if ( normVec.dot(w) > 0.9  )
+            std::cout << "road or non-facade detected. Not included into facade vector\n";
+        else
+            facades_.push_back(*cloudPlane);  // it is not a road surface
+
+
+        std::cout << "PointCloud representing the planar component: " << cloudPlane->points.size () << " data points." << std::endl;
+
+        // Remove the planar inliers, extract the rest
+        extract.setNegative (true);
+        extract.filter (*cloud_f);
+
+        std::stringstream ss;
+        pcl::PCDWriter writer;
+        ss << "STEP_01_SurfaceExtracted_" << i << ".pcd";
+        //writer.write<T> (ss.str (), *cloudPlane, false); //*
+
+        //viewer<T> (cloudPlane);
+
+
+        *cloudFiltered = *cloud_f;
+
+       // viewer<T> (cloud_f);
+     ++i;
+      }
+
+      /////////////////////////////////////////////////////////////////////////////////
+      /// refine the found planes by reducing the distancethreshold to 0.1m. this will stay hard-coded.
+      /// 1) copy all cloud clusters in a new cloud
+      /// 2) perform the same plane estimation as above
+      ///
+
+      typename pcl::PointCloud<T>::Ptr combClusters (new pcl::PointCloud<T>);
+
+      for (int c = 0; c< facades_.size() ; ++c ){
+          typename pcl::PointCloud<T> tmpCloud;
+          pcl::copyPointCloud(facades_.at(c),tmpCloud);
+          for (size_t p=0; p < tmpCloud.points.size(); p++)
+            combClusters->points.push_back( tmpCloud.points[p] );
+      }
+      combClusters->width = combClusters->points.size();
+      combClusters->height = 1;
+
+      pcl::copyPointCloud(*combClusters, *cloudFiltered);
+      i=0;
+      nr_points = cloudFiltered->points.size ();
+      facades_.clear();
+      seg.setDistanceThreshold (0.2);
+
+      std::cout << "  \n";
+
+      while (cloudFiltered->points.size () > 0.3 * nr_points)
+      {
+        // Segment the largest planar component from the remaining cloud
+        seg.setInputCloud (cloudFiltered);
+        seg.segment (*inliers, *coefficients);
+        if (inliers->indices.size () == 0)
+        {
+          std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
+          break;
+        }
+
+        // Extract the planar inliers from the input cloud
+        typename pcl::ExtractIndices<T> extract;
+        extract.setInputCloud (cloudFiltered);
+        extract.setIndices (inliers);
+        extract.setNegative (false);
+
+        // Get the points associated with the planar surface
+        extract.filter (*cloudPlane);
+
+        // see if the average z value of the detected plane is within +/- grndOffset_ (5m default) meters of the minPt of the entire pointcloud;
+        pcl::CentroidPoint<T> centroid;
+        T minPt;
+        T maxPt;
+        pcl::getMinMax3D(*cloudPlane, minPt, maxPt);
+        // get coefficients of plane to get the normal vector to see if it is point upward (road) or sideways (vertical plane)
+        const Eigen::Vector3f normVec (coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2));
+        const Eigen::Vector3f w(0,0,1); // road normal vector. assuming road is parallel to x-y plane
+        for (size_t c=0; c < cloudPlane->points.size(); c++)
+            centroid.add(cloudPlane->points[c]);
+
+        T centPt;
+        centroid.get(centPt);
+        if ( (centPt.z < minPt.z + grndOffset_ &&  centPt.z > minPt.z - grndOffset_) || normVec.dot(w) > 0.9 || cloudPlane->points.size() < 0.05 * nr_points )
+            std::cout << "road or non-facade detected. Not included into facade vector\n";
+        else
+            facades_.push_back(*cloudPlane);  // it is not a road surface
+
+
+        std::cout << "PointCloud representing the planar component: " << cloudPlane->points.size () << " data points." << std::endl;
+
+        // Remove the planar inliers, extract the rest
+        extract.setNegative (true);
+        extract.filter (*cloud_f);
+
+        *cloudFiltered = *cloud_f;
+      }
+
+
+
+      for(int j = 0; j < facades_.size(); j++)
+      {       
+        std::stringstream ss;
+        pcl::PCDWriter writer;
+        ss << "STEP_02_cloud_cluster_" << j << ".pcd";
+        writer.write<T> (ss.str (), facades_[j], false); //*
+        typename pcl::PointCloud<T>::Ptr tmpFacade (new pcl::PointCloud<T>);
+        pcl::copyPointCloud(facades_.at(j), *tmpFacade);
+       // viewer<T> (tmpFacade);
+
+      }
+      std::cout << "detecting planes took : " << tt.toc()/1000 << " seconds. " << std::endl;
+
+}
+
+
+#endif // EXTRACTFACADES_H

h/inrange.h

+#ifndef INRANGE_H
+#define INRANGE_H
+
+
+template<typename T>
+class inRange
+{
+public:
+    inRange(T lo, T hi) : low(lo), high(hi) {}
+    bool contains(T value) const { return low <= value && value < high; }  //return true if within range else return false.
+private:
+    T low;
+    T high;
+};
+template<typename T>
+inRange<T> inrange(T lo, T hi) { return inRange<T>(lo, hi); }
+
+#endif // INRANGE_H

h/removeconcavehullframe.h

+#ifndef REMOVECONCAVEHULLFRAME_H
+#define REMOVECONCAVEHULLFRAME_H
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/common/pca.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/passthrough.h>
+
+
+
+template <typename PointInT>
+class RemoveConcaveHullFrame: public pcl::PCLBase<PointInT>
+{
+
+protected:
+    typedef typename pcl::PointCloud<PointInT>::Ptr PointCloudPtr;
+    typedef typename pcl::PointCloud<PointInT>::ConstPtr PointCloudConstPtr;
+
+
+public:
+
+    typedef boost::shared_ptr< RemoveConcaveHullFrame<PointInT> > Ptr;
+    typedef boost::shared_ptr< const RemoveConcaveHullFrame<PointInT> > ConstPtr;
+
+
+    RemoveConcaveHullFrame () :
+          tollerance_ (0.0),
+          hullFrame_ (new pcl::PointCloud<PointInT>),
+          cloudWithoutFrame_ (new pcl::PointCloud<PointInT>)
+          { };
+
+
+
+    inline void setTollerance(float toll) {tollerance_ = toll;}
+
+    void apply(typename pcl::PointCloud<PointInT>::Ptr cloudOut);
+
+
+    virtual ~RemoveConcaveHullFrame ()
+    {
+
+    }
+
+protected:
+
+
+    float tollerance_;
+    /** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */
+    using pcl::PCLBase<PointInT>::input_;
+    /** \brief The subtracted (inverted) point cloud. */
+    typename pcl::PointCloud<PointInT>::Ptr hullFrame_;
+    typename pcl::PointCloud<PointInT>::Ptr cloudWithoutFrame_;
+
+
+};
+
+template <typename PointInT>
+void RemoveConcaveHullFrame<PointInT>::apply(typename pcl::PointCloud<PointInT>::Ptr cloudOut){
+
+    typename pcl::PointCloud<PointInT>::Ptr cloudFiltered (new pcl::PointCloud<PointInT>);
+    typename pcl::PointCloud<PointInT>::Ptr cloudFiltered2 (new pcl::PointCloud<PointInT>);
+
+    //get extension of point cloud
+    PointInT minPt;
+    PointInT maxPt;
+    pcl::getMinMax3D(*input_, minPt, maxPt);
+
+    // add and subtract a small tolerance to the determined bounds to generate a wider range
+    // to be sure to filter all the outside borders
+
+    if (tollerance_ != 0){
+        minPt.x = minPt.x + tollerance_;
+        minPt.y = minPt.y + tollerance_;
+        minPt.z = minPt.z + tollerance_;
+
+        maxPt.x = maxPt.x - tollerance_;
+        maxPt.y = maxPt.y - tollerance_;
+        maxPt.z = maxPt.z - tollerance_;
+    }
+
+    // Create the filtering object
+      pcl::PassThrough<PointInT> pass;
+      pass.setInputCloud (input_);
+      pass.setFilterFieldName ("x");        // filter in x
+      pass.setFilterLimits (minPt.x, maxPt.x);
+      //pass.setFilterLimitsNegative (true);
+      pass.filter (*cloudFiltered);
+
+      // filter in y
+      pass.setInputCloud (cloudFiltered);
+      pass.setFilterFieldName ("y");
+      pass.setFilterLimits (minPt.y, maxPt.y);
+      //pass.setFilterLimitsNegative (true);
+      pass.filter (*cloudFiltered2);
+
+
+      // filter in z
+      pass.setInputCloud (cloudFiltered2);
+      pass.setFilterFieldName ("z");
+      pass.setFilterLimits (minPt.z, maxPt.z);
+      //pass.setFilterLimitsNegative (true);
+      pass.filter (*cloudOut);
+      //viewer<PointInT> (cloudOut);
+
+
+}
+
+#endif // REMOVECONCAVEHULLFRAME_H

main.cpp

+#include <iostream>
+#include <iomanip>
+#include <vector>
+#include <fstream>  // std::ifstream
+#include <string>
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/console/parse.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/console/time.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/filters/passthrough.h>
+
+// Boost
+#include <boost/filesystem.hpp>
+
+// libLAS library
+#include <liblas/liblas.hpp>
+
+// OpenCV
+//#include <opencv2/core/core.hpp>
+//#include <opencv2/viz/vizcore.hpp>
+
+
+//include own libs and classes here
+#include "./src/miscFunctions.hpp"
+#include "./src/hull2d.hpp"
+#include "./src/CondEuclClustering.hpp"
+#include "./h/cloudsubtractor.h"
+#include "./src/pcl_las_helper.hpp"
+#include "./h/extractfacade.h"
+#include "./h/areafromconcavehull.h"
+#include "./h/removeconcavehullframe.h"
+
+#define PI atan(1.)*4.0L
+
+typedef pcl::PointXYZI PointT;
+
+using namespace std;
+
+
+void processFacades(std::vector< pcl::PointCloud<PointT> > facadeVec,                    
+                    const std::vector<double> &minXYZValues)
+{
+    float facadeArea = 0;
+    float windowArea = 0;
+
+
+
+    std::vector<float> facadeAreaVec, windowAreaVec;
+
+    for (int f=0 ; f < facadeVec.size(); f++)
+    {
+        pcl::PointCloud<PointT>::Ptr tmpFacade (new pcl::PointCloud<PointT>);
+        pcl::PointCloud<PointT>::Ptr invertedCloud (new pcl::PointCloud<PointT>);
+        pcl::PointCloud<PointT>::Ptr concaveCloud (new pcl::PointCloud<PointT>);
+        pcl::PointCloud<PointT>::Ptr lines (new pcl::PointCloud<PointT>);
+        pcl::PointCloud<PointT>::Ptr cloudWithWindows (new pcl::PointCloud<PointT>);
+
+        *tmpFacade = facadeVec.at(f);
+        facadeArea = 0;
+        windowArea = 0;
+        viewer<PointT> (tmpFacade);
+
+
+        //AreaFromConcaveHull<PointT> AObj;
+       // AObj.setInputCloud(tmpFacade);
+        //AObj.greedyArea(facadeArea);
+
+        facadeAreaVec.push_back(facadeArea);
+
+        // read CLoudSubtractor values from config file
+        std::vector<float> params;
+        readParamsFromFile("../cfg/cloudSubtractor.cfg", params, true);
+
+
+        // create cloudSubtractor object and call the necessary input functions
+        CloudSubtractor<PointT> cs(params[0], params[1], params[2]);
+        /*
+        CloudSubtractor<PointT> cs;
+        cs.setPointIncrement(params[0]);  // grid spacing between points for the grid / plane creation
+        cs.setKdTreeRadius(params[2]);      // kdtree search radius for point search in original point cloud for point cloud subtraction.
+        cs.setKdTreeRadiusPCA(0.05);
+        cs.setPlaneDistThreshold(params[1]);*/
+        cs.setInputCloud(tmpFacade);
+        cs.applyCloudSubtraction(invertedCloud);
+
+        // run again for inverted cloud.
+        applyHull(invertedCloud, concaveCloud);
+        //viewer<PointT> (concaveCloud);
+
+
+        RemoveConcaveHullFrame<PointT> rmFrame;
+        rmFrame.setInputCloud(concaveCloud);
+        rmFrame.setTollerance(0.1);
+        rmFrame.apply(concaveCloud);
+        viewer<PointT> (concaveCloud);
+
+        // write output to xyz for 3d tiles convertsion
+        std::vector< std::vector< double > > pointVec;
+        std::vector< unsigned int > intensityVec;
+        addMinValues<PointT>(concaveCloud,pointVec,intensityVec, minXYZValues);
+        toXYZ("hull", pointVec, intensityVec);
+
+/*
+        AreaFromConcaveHull<PointT> AreaObj;
+        AreaObj.setInputCloud(tmpFacade);
+        AreaObj.greedyArea();
+
+        AreaObj.setInputCloud(concaveCloud);
+        AreaObj.setLineDistThreshold(0.2);
+        AreaObj.detectLines(lines);
+
+        viewer<PointT> (lines);
+*/
+
+         CondEuclClustering(concaveCloud,cloudWithWindows);
+         pointVec.clear();
+         intensityVec.clear();
+         addMinValues<PointT>(cloudWithWindows,pointVec,intensityVec, minXYZValues);
+         toXYZ("cloudWithWindows", pointVec, intensityVec);
+
+
+        int maxLabel = 3; // assuming -2 and 2 for too small and too large clusters, respectively
+        int nObj = 0;
+        for (int p=0; p<concaveCloud->points.size(); p++)
+            if (maxLabel < concaveCloud->points[p].intensity ){
+                maxLabel = concaveCloud->points[p].intensity;
+                nObj++;
+            }
+
+
+        int label = 4; //assuming first true label == 4, see CondEuclClustering
+        pcl::PointCloud<PointT>::Ptr tmpWindow (new pcl::PointCloud<PointT>);
+        pcl::PointCloud<PointT>::Ptr boxCloud (new pcl::PointCloud<PointT>);   //store the four corners of the bounding box in here
+        tmpFacade.reset();
+
+        for (int obj=0; obj < nObj; obj++, label+=2 ) { //assuming min label == 4 and a label difference of 2
+
+            // Create the filtering object
+             pcl::PassThrough<PointT> pass;
+             pass.setInputCloud (concaveCloud);
+             pass.setFilterFieldName ("intensity");
+             pass.setFilterLimits (label, label);  // get only one window with that specific label
+             //pass.setFilterLimitsNegative (true);
+             pass.filter (*tmpWindow);  // this variable stores the potential window points
+
+            tmpWindow->width = tmpWindow->points.size();
+            tmpWindow->height = 1;
+
+            std::vector<Eigen::Vector3f> bBox;
+            float aspectRatio, tmpA;
+            findBoundingBox(tmpWindow, bBox);  // apply fit bounding box in openCV
+
+            //windowArea += calcAreaFromBbox(bBox,aspectRatio);
+            tmpA = calcAreaFromBbox(bBox,aspectRatio);
+            if (aspectRatio > 0.33){
+                windowArea += tmpA;
+                std::cout << "accumulated window area: " << std::setprecision(4) << windowArea << " sqm" << std::endl;
+            }
+            else
+                continue;
+
+
+            //for (int ptNr=0; ptNr < bBox.size(); ptNr++){  // for all 4 corners store one point in the the boxCloud variable
+            for (int ptNr=0; ptNr < 4; ptNr++){
+                PointT pt;
+                Eigen::Vector3f tmp(bBox.at(ptNr) );
+                pt.x = tmp(0);
+                pt.y = tmp(1);
+                pt.z = tmp(2);
+                pt.intensity = label;
+                boxCloud->points.push_back( pt );  // store point
+                //tmpFacadel->points.push_back( pt );
+            }
+
+            boxCloud->width = boxCloud->points.size();
+            boxCloud->height = 1;
+
+        }
+        windowAreaVec.push_back(windowArea);
+
+      /* std::cout<< "Total window Area / Facade area = " << windowArea << " / " <<
+                    facadeArea << " = " << windowArea / facadeArea <<
+                    " = window-to-wall ratio." <<
+                    "But the facadeArea was calculated without windows." << std::endl;
+        std::cout << "corrected ratio is: windowArea / (windowArea+FacadeArea)= " <<
+                     windowArea / (windowArea+facadeArea) << std::endl;
+                     */
+
+       // viewer<PointT>(boxCloud);
+        //viewer<PointT>(tmpWindow);
+
+        //std::vector< std::vector< double > > pointVec;
+        //std::vector< unsigned int > intensityVec;
+        pointVec.clear();
+        intensityVec.clear();
+        addMinValues<PointT>(boxCloud,pointVec,intensityVec, minXYZValues);
+        toXYZ("STEP_05_bBox", pointVec, intensityVec);
+
+
+        pointVec.clear();
+        intensityVec.clear();
+        addMinValuesToBBOX<PointT>(boxCloud,pointVec,intensityVec, minXYZValues);
+        toXYZ("STEP_05_bBoxForVIS", pointVec, intensityVec);
+
+
+        std::stringstream ss;
+        pcl::PCDWriter writer;
+        ss << "STEP_05_bBox.pcd";
+        writer.write<PointT> (ss.str (), *boxCloud, false); //*
+
+
+
+
+    }
+}
+
+int  main (int argc, char** argv){
+
+
+   if (argc == 1){
+        PCL_ERROR ("No file has been specified... aborting.");
+        return -1;
+    }
+
+   //check if pcd or las file was given
+   std::string extension = boost::filesystem::extension(argv[1]);
+   bool isPCD = false;
+   if ( (extension.compare(1,3,".pcd",1,3) == 0 ) ||  ( (extension.compare(1,3,".PCD",1,3)) == 0 ) )
+      isPCD = true;
+   else if ( ( (extension.compare(1,3,".las",1,3)) == 0 ) ||  ( (extension.compare(1,3,".LAS",1,3)) == 0 ) )
+       isPCD = false;
+   else{
+       PCL_ERROR ("Wrong file extension. Aborting.\n .pcd or .las is required");
+       return -1;
+   }
+
+
+ //  pcl::PointCloud<PointT>::Ptr cloudIn = boost::make_shared<pcl::PointCloud<PointT> >();
+  pcl::PointCloud<PointT>::Ptr cloudIn (new pcl::PointCloud<PointT>);
+  pcl::PointCloud<PointT>::Ptr cloudFiltered (new pcl::PointCloud<PointT>);
+  pcl::PointCloud<PointT>::Ptr invertedCloud (new pcl::PointCloud<PointT>);
+
+
+
+// Checking parameters and input arguments
+  if (argc < 2){
+      pcl::console::print_error ("Syntax is: %%s <LAS-INPUT>  \t are mandatory.\nOtional are: \n "
+                                         "-setZero: setMinXYZ to zero  no value required. If flag is set it is applied. \n "
+                                         "-downsample: float [0.02,1] \n", argv[0]);
+      return -1;
+   }
+
+    // check optional parameters for subtracting the min Values of the coordinates from all x,y,z coordinates respectively
+    // this is done to work around problems caused by the loss of precision using float data types. -> poor PCD point clouds.
+    bool subtractMinVals = true;
+    subtractMinVals = pcl::console::parse (argc, argv, "-setZero", subtractMinVals);
+
+    // check if the user wants to downsample the point cloud using the voxelgrid function. Specfiy standard values for the leafSize.
+    float gridLeafSize = 0.25;
+    bool LeafSize_specified = pcl::console::find_switch (argc, argv, "-downsample");
+    if (LeafSize_specified){
+        pcl::console::parse (argc, argv, "-downsample", gridLeafSize);
+        if (gridLeafSize > 1){
+            pcl::console::print_highlight("Warning: LeafSize is out of bounds [0, 1]. Setting it to default to proceed (0.25).\n");
+            gridLeafSize = 0.25;
+        }
+    }
+
+
+    ///// end checking parameters.
+
+
+    // store minvalues to restore the original coordinates later after processing is completed
+    std::vector<double> minXYZValues;
+
+    if (isPCD){  // if pcd no values will be subtracted because pcd cannot store this precision anyway
+        // load pcd file
+        if(pcl::io::loadPCDFile<PointT> (argv[1], *cloudIn) == -1) // load the file
+         {
+           PCL_ERROR ("Couldn't read file ");
+           return -1;
+         }
+
+        if (gridLeafSize > 0.029 && gridLeafSize < 1 && LeafSize_specified){
+          std::cout << "\nApplying Uniform downsampling with leafSize " << gridLeafSize << ". Processing...";
+
+          pcl::UniformSampling<PointT> uniform_sampling;
+          uniform_sampling.setInputCloud (cloudIn);
+          uniform_sampling.setRadiusSearch (gridLeafSize); //the 3D grid leaf size
+          uniform_sampling.filter(*cloudFiltered);
+          pcl::copyPointCloud(*cloudFiltered, *cloudIn);  // cloud is given by reference so the downsampled cloud has to be copied in there
+
+          /*
+          std::string fileToWrite = string(argv[1]) + "_downsampled.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;*/
+
+        }
+
+
+    }
+    else{
+
+
+        // reading in LAS file and returning a PCD cloud XYZI data.
+        readLAS2PCD<PointT>(argv[1], cloudIn, minXYZValues, gridLeafSize, subtractMinVals);
+    }
+
+    viewer<PointT> (cloudIn);
+
+    //extract planes from point cloud
+    std::vector< pcl::PointCloud<PointT> > facadeVec;  //construct a vector containing the all vertical planes, i.e. hopfully all facades
+
+    getFacades<PointT>(cloudIn, facadeVec, 0.4).applyFilter(facadeVec);  //template class  / function to obtain all vertical planes / facades
+
+    processFacades(facadeVec, minXYZValues);
+
+
+    if (invertedCloud->points.size() > 0){
+      pcl::PCDWriter writer;
+      writer.write<PointT> ("inverted_subtracted_Cloud.pcd", *invertedCloud, true);
+    }
+
+
+  // tt.tic();
+   //std::cout << "starting to compute something...." << std::endl;
+
+   //applyHull(invertedCloud, concaveHull, convexHull);
+   //applyHull(invertedCloud, concaveHull);
+
+  // viewer<PointT> (concaveHull);
+ //  std::cout << "Number of points after processing " << concaveHull->points.size () << " .\nAll in all it took " << tt.toc() / 1000. << " seconds." << std::endl;
+ //  viewer<PointT> (convexHull);
+
+ //  tt.tic();
+  // std::cout << " starting conditional Euclidean clustering..." << std::endl;
+ //  CondEuclClustering(invertedCloud,cloudFiltered);
+  // std::cout << "Conditional Euclidean  clustering applied to " << invertedCloud->points.size () << " points took: " << tt.toc() / 1000. << " seconds." << std::endl;
+
+
+/*
+
+    //////////////////////////////////////////////////////////////
+    // Visualization of keypoints along with the original cloud
+     pcl::visualization::PCLVisualizer viewer("PCL Viewer");
+     pcl::visualization::PointCloudColorHandlerCustom<PointT> keypoints_color_handler (concaveHull, 0, 255, 0);
+     pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_color_handler (cloudIn, 255, 0, 0);
+     pcl::visualization::PointCloudColorHandlerCustom<PointT> outputfinal_color_handler (convexHull, 0, 0, 255);
+     viewer.setBackgroundColor( 0.0, 0.0, 0.0 );
+     viewer.addPointCloud(cloudIn, cloud_color_handler, "cloud");
+
+     viewer.addPointCloud(concaveHull, keypoints_color_handler, "keypoints");
+     viewer.addPointCloud(convexHull, outputfinal_color_handler, "outputfinal");
+     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "keypoints");
+     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "outputfinal");
+
+     while(!viewer.wasStopped ())
+     {
+     viewer.spinOnce ();
+     }
+*/
+    return (0);
+}
+

src/CondEuclClustering.hpp

+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/console/time.h>
+
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/segmentation/conditional_euclidean_clustering.h>
+
+
+typedef pcl::PointXYZI PointT;
+typedef pcl::PointXYZINormal PointTNormal;
+
+
+bool
+enforceIntensitySimilarity (const PointTNormal& point_a, const PointTNormal& point_b, float squared_distance)
+{
+  if (fabs (point_a.intensity - point_b.intensity) < 5.0f)
+    return (true);
+  else
+    return (false);
+}
+
+bool
+enforceCurvatureOrIntensitySimilarity (const PointTNormal& point_a, const PointTNormal& point_b, float squared_distance)
+{
+  Eigen::Map<const Eigen::Vector3f> point_a_normal(point_a.normal);
+  Eigen::Map<const Eigen::Vector3f> point_b_normal(point_b.normal);
+  if (fabs (point_a.intensity - point_b.intensity) < 5.0f)
+    return (true);
+  if (fabs (point_a_normal.dot (point_b_normal)) < 0.05)
+    return (true);
+  return (false);
+}
+
+bool
+customRegionGrowing (const PointTNormal& point_a, const PointTNormal& point_b, float squared_distance)
+{
+
+    // read in parameter file
+    std::vector<float> params;
+    readParamsFromFile("../cfg/customRegionGrowing.cfg", params, false);
+
+    Eigen::Map<const Eigen::Vector3f> point_a_normal(point_a.normal);
+    Eigen::Map<const Eigen::Vector3f> point_b_normal(point_b.normal);
+
+
+
+    if (squared_distance < params[0] )
+  {
+    if (fabs (point_a.intensity - point_b.intensity) < params[1])
+      return (true);
+    if (fabs (point_a_normal.dot (point_b_normal)) < params[2])
+      return (true);
+  }
+  else
+  {
+    if (fabs (point_a.intensity - point_b.intensity) < params[3])
+      return (true);
+  }
+  return (false);
+
+}
+
+int
+CondEuclClustering (pcl::PointCloud<PointT>::Ptr cloudXyz, pcl::PointCloud<PointT>::Ptr cloudOut)
+{
+  // Data containers used
+
+  pcl::PointCloud<PointTNormal>::Ptr cloud_with_normals (new pcl::PointCloud<PointTNormal>);
+  pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters), small_clusters (new pcl::IndicesClusters), large_clusters (new pcl::IndicesClusters);
+  pcl::search::KdTree<PointT>::Ptr search_tree (new pcl::search::KdTree<PointT>);
+  pcl::console::TicToc tt;
+
+  // read in parameter file
+  std::vector<float> params;
+  readParamsFromFile("../cfg/CondEuclidClustering.cfg", params, true);
+  // the way it is used is prone to access violations as it will never checked if the params[value] really exists...
+
+
+
+  // Set up a Normal Estimation class and merge data in cloud_with_normals
+  std::cerr << "Computing normals...\n", tt.tic ();
+  pcl::copyPointCloud (*cloudXyz, *cloud_with_normals);
+  pcl::NormalEstimation<PointT, PointTNormal> ne;
+  ne.setInputCloud (cloudXyz);
+  ne.setSearchMethod (search_tree);
+  ne.setRadiusSearch ( params[0] );
+  ne.compute (*cloud_with_normals);
+  std::cerr << ">> Done: " << tt.toc () / 1000.0  << " s\n";
+
+  // Set up a Conditional Euclidean Clustering class
+  std::cerr << "Segmenting to clusters...\n", tt.tic ();
+  pcl::ConditionalEuclideanClustering<PointTNormal> cec (true);
+  cec.setInputCloud (cloud_with_normals);
+  cec.setConditionFunction (&customRegionGrowing);
+  //cec.setConditionFunction(&enforceCurvatureOrIntensitySimilarity);
+  cec.setClusterTolerance ( params[1] );
+  cec.setMinClusterSize ( params[2] );
+  cec.setMaxClusterSize ( params[3] );
+  cec.segment (*clusters);
+  cec.getRemovedClusters (small_clusters, large_clusters);
+  std::cerr << ">> Done: " << tt.toc () / 1000.0 << " s\n";
+
+  std::cerr << "Clusters within defined cluster size: " << clusters->size() << std::endl;
+  std::cerr << "Clusters which are too LARGE: " << large_clusters->size() << std::endl;
+  std::cerr << "Clusters which are too SMALL: " << small_clusters->size() << std::endl;
+
+  // Using the intensity channel for lazy visualization of the output
+  for (int i = 0; i < small_clusters->size (); ++i)
+    for (int j = 0; j < (*small_clusters)[i].indices.size (); ++j)
+      cloudXyz->points[(*small_clusters)[i].indices[j]].intensity = -2.0;
+  for (int i = 0; i < large_clusters->size (); ++i)
+    for (int j = 0; j < (*large_clusters)[i].indices.size (); ++j)
+      cloudXyz->points[(*large_clusters)[i].indices[j]].intensity = +2.0;
+
+  int label = 4;
+  long nPtsOfAllClusters = 0;
+  for (int i = 0; i < clusters->size (); ++i)
+  {
+    //int label = rand () % 8;
+    for (int j = 0; j < (*clusters)[i].indices.size (); ++j){
+      cloudXyz->points[(*clusters)[i].indices[j]].intensity = label;
+    }
+    nPtsOfAllClusters += (*clusters)[i].indices.size();
+    label += 2;
+  }
+
+  //viewer<PointT>(cloudXyz);
+
+  pcl::PointCloud<PointT>::Ptr cloud_withWindows (new pcl::PointCloud<PointT>);
+  cloud_withWindows->width = nPtsOfAllClusters;
+  cloud_withWindows->height = 1;
+  cloud_withWindows->resize(cloud_withWindows->width );
+
+  long k = 0;
+  for (int i = 0; i < clusters->size (); ++i)
+  {
+    //int label = rand () % 8;
+    for (int j = 0; j < (*clusters)[i].indices.size (); ++j){
+      cloud_withWindows->points[k].intensity = cloudXyz->points[(*clusters)[i].indices[j]].intensity;
+      cloud_withWindows->points[k].x = cloudXyz->points[(*clusters)[i].indices[j]].x;
+      cloud_withWindows->points[k].y = cloudXyz->points[(*clusters)[i].indices[j]].y;
+      cloud_withWindows->points[k].z = cloudXyz->points[(*clusters)[i].indices[j]].z;
+      ++k;
+    }  
+  }
+
+  cloud_withWindows->width = cloud_withWindows->points.size();
+  cloud_withWindows->height = 1;
+
+
+  pcl::copyPointCloud(*cloud_withWindows,*cloudOut);
+  viewer<PointT>(cloud_withWindows);
+
+
+  // Save the output point cloud
+  std::cerr << "Saving...\n", tt.tic ();
+  if (cloudXyz->points.size() > 0)
+    pcl::io::savePCDFile ("EuclClusteringOutput.pcd", *cloudXyz);
+
+  if (cloud_withWindows->points.size() > 0)
+    pcl::io::savePCDFile ("OnlyWindows.pcd", *cloud_withWindows);
+
+  std::cerr << ">> EuclidCLustering done... "<< "\n";
+
+  return (0);
+}