An error occurred while loading the file. Please try again.
extractfacade.h 9.01 KiB
#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