main.cpp 13.56 KiB
#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); }