void Segmentation::doSegmentation(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud)
{
//MainPlane groundMainPlane;
// Variables
isComputed = false ;
struct timeval tbegin,tend;
double texec=0.0;
// Start timer
gettimeofday(&tbegin,NULL);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudFiltered(new pcl::PointCloud<pcl::PointXYZRGBA>);
//pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudFilteredCopy(new pcl::PointCloud<pcl::PointXYZRGBA>);
_mainCloud.reset(new pcl::PointCloud<pcl::PointXYZRGBA>);
(void) passthroughFilter(-5.0, 5.0, -5.0, 5.0, -5.0, 5.0, cloud, cloudFiltered); // 3.0, 3.0, -0.2, 2.0, -4.0, 4.0
pcl::copyPointCloud(*cloudFiltered, *_mainCloud);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr ground (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr Hullground (new pcl::PointCloud<pcl::PointXYZRGBA>);
std::vector<pcl::PointIndices> vectorInliers(0);
std::vector <pcl::ModelCoefficients> vectorCoeff(0);
std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > vectorCloudinliers(0);
std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > vectorHull(0);
std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > vectorCluster(0);
pcl::ModelCoefficients::Ptr coefficientsGround (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliersGround (new pcl::PointIndices);
// pcl::VoxelGrid< pcl::PointXYZRGBA > sor;
// sor.setInputCloud (_mainCloud);
// sor.setLeafSize (0.01f, 0.01f, 0.01f);
// sor.filter (*_mainCloud);
//(void) findGround(_mainCloud,_ground);
//isComputed = true;
if(findGround(_mainCloud,_ground))
{
Eigen::Vector3f gravityVector(_ground.getCoefficients()->values[0], _ground.getCoefficients()->values[1], _ground.getCoefficients()->values[2]);
//(void) findOtherPlanesRansac(_mainCloud,gravityVector,vectorCoeff , vectorCloudinliers,vectorHull);
//(void) regionGrowing(vectorCloudinliers, vectorCluster);
euclidianClustering(_mainCloud,vectorCloudinliers);
ransac(vectorCloudinliers,gravityVector,vectorCluster );
createPlane(vectorCluster);
// End timer
}
gettimeofday(&tend,NULL);
// Compute execution time
texec = ((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000;
std::cout << "time : " << texec << std::endl;
}
void fi::VPDetectionCloud::ComputeVPDetection()
{
//Collect all params first!
unsigned int nValidationRounds = m_ModelParams->fNumberOfValidationRounds;
//Check if Down Sampling required using a Voxel grid filter
// Create the filtering object: down sample the dataset using a leaf size of 1cm
float aLeaf = m_ModelParams->fVoxGridSize(0);
float bLeaf = m_ModelParams->fVoxGridSize(1);
float cLeaf = m_ModelParams->fVoxGridSize(2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFiltered (new pcl::PointCloud<pcl::PointXYZ>);
if (!((aLeaf == 0.0f) && (bLeaf == 0.0f) &&(cLeaf == 0.0f)) )
{
pcl::VoxelGrid<pcl::PointXYZ> vg;
vg.setInputCloud (m_InputCloud);
vg.setLeafSize (aLeaf, bLeaf, cLeaf); //fachwerkhaus
//vg.setLeafSize (0.04f, 0.04f, 0.04f); //Schloss Etlingen
//vg.setLeafSize (0.1f, 0.1f, 0.1f); //unichurch
vg.filter (*cloudFiltered);
std::cout << "PointCloud before filtering has: " << m_InputCloud->points.size () << " data points." << std::endl; //*
std::cout << "PointCloud after filtering has: " << cloudFiltered->points.size () << " data points." << std::endl; //*
}
else
{
//copying is not necessary here since the size of the cloud would not be reducing!
cloudFiltered = m_InputCloud;
}
for (unsigned int i = 0; i < nValidationRounds; i++)
{
Eigen::VectorXf fRobustEstimatedVP;
if (m_ModelParams->fRadiusSearched != 0.0f)
{
//ClusterRDirectionNeighbourhoods(cloudFiltered, fRobustEstimatedVP);
}
else
{
ClusterKDirectionPatches(cloudFiltered, fRobustEstimatedVP);
m_EstimatedVPs.push_back(fRobustEstimatedVP);
}
}
m_vgFilteredCloud = cloudFiltered;
}
int main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudFiltered (new pcl::PointCloud<pcl::PointXYZRGBA>);
int k=atoi(argv[1]);
double min_mah_dist=atof(argv[2]);
int max_gauss=atoi(argv[3]);
std::vector <Gauss> gaussians_learned;
pcl::PCDReader reader;
reader.read (argv[4], *cloud);
pcl::console::TicToc timer;
timer.tic();
// Filtering
Filtering filtering;
cloudFiltered=filtering.filter(cloud);
// Segmentation
Segmentation segmentation;
std::vector <Surface> surfaces=segmentation.segment (cloudFiltered);
// Colour extraction from point cloud
ExtractColour extractcolour;
cv::Mat samples=extractcolour.pcd2colourRGB(surfaces[0].segmented_cloud);
// Point cloud analysis
ColourAnalysis colouranalysis;
std::vector <Gauss> gaussians = colouranalysis.analyzeColourCluster(k, surfaces[0].segmented_cloud, surfaces[0].b, surfaces[0].d, samples);
// Learning
Learning learning;
gaussians_learned=learning.learnGaussians(gaussians, gaussians_learned, max_gauss);
// ROI
ROI roi;
std::vector<Line> lines=roi.roi(surfaces);
std::vector<int> index=roi.extractIndex(lines);
// Colour extraction from image
std::vector<Matrix_double> colours=extractcolour.indexPCD2VectorRGB(*cloud, index);
// Image analysis
std::vector <int> pixels_labeled=colouranalysis.labelIndex(colours, k, gaussians_learned, min_mah_dist);
// Map reconstruction
Map map;
std::vector<Matrix_double> distancesLabeled=map.mapDistances(index, surfaces[0].a ,surfaces[0].b, surfaces[0].c, surfaces[0].d);
std::cout << "Part 1 finished" << std::endl;
timer.toc_print();
timer.tic();
// Generate image
cv::Mat environment=extractcolour.pcd2image(*cloud);
// Calcule begin and end points for a line
std::vector <LinePoint> linePoints=roi.calculeLinePoints(lines);
// Draw lines
Visualize visualize;
environment=visualize.drawLines (environment, linePoints, "green");
// Print floor
std::vector<int> pixels=colouranalysis.labelPixel(pixels_labeled, index, colours);
environment=visualize.drawFloor (pixels, environment, "cyan");
std::vector <int> kinectIndex=roi.extractFloorIdx(surfaces[0].a, surfaces[0].b, surfaces[0].c, surfaces[0].d, cloud);
environment=visualize.drawFloor (kinectIndex, environment, "blue");
// Reconstruct kinect floor
std::vector <int> kinect (kinectIndex.size(), 1);
std::vector<Matrix_double> distancesKinect=map.mapDistances(kinectIndex, surfaces[0].a ,surfaces[0].b, surfaces[0].c, surfaces[0].d);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudEnhanced=map.mapEnhance(distancesLabeled, index, cloud, pixels_labeled, surfaces[0].a ,surfaces[0].b, surfaces[0].c, surfaces[0].d);
// Generate map reconstruction image
cv::Mat map_reconstruction(cv::Size(400,650),CV_8UC3);
map_reconstruction=cv::Scalar(0,0,0);
map_reconstruction=visualize.mapGeneration(map_reconstruction, pixels_labeled, distancesLabeled, "blue", "white");
map_reconstruction=visualize.mapGeneration(map_reconstruction, kinect, distancesKinect, "red", "yellow");
// Show images on a window
std::string name_window="Floor window";
visualize.visualizeImage (environment, name_window);
name_window="Map window";
visualize.visualizeImage (map_reconstruction, name_window);
visualize.visualizeCloud(cloudEnhanced, "Cloud");
// Save images
Save save;
char name[50]="map.png";
save.saveImage(map_reconstruction, name);
char name2[50]="environment.png";
save.saveImage(environment, name2);
// Save cloud
std::string names= "cloud.pcd";
save.saveCloud(surfaces[0].segmented_cloud, names);
std::cout << "Part 2 finished" << std::endl;
timer.toc_print();
cv::waitKey();
return 0;
}
void PointCloudProcessor::_doEuclideanClusterExtraction(const QList<pcl::PointCloud<PointType>::Ptr> & clouds)
{
QList<pcl::PointCloud<PointType>::Ptr> clusteredClouds;
foreach (pcl::PointCloud<PointType>::Ptr cloud, clouds)
{
// Create the filtering object: downsample the dataset using a leaf size of 10cm
pcl::PointCloud<PointType>::Ptr cloudFiltered (new pcl::PointCloud<PointType> ());
pcl::VoxelGrid<PointType> vg;
vg.setInputCloud (cloud);
vg.setLeafSize (0.1f, 0.1f, 0.1f);
vg.filter (*cloudFiltered);
int nr_points = static_cast<int>(cloudFiltered->points.size());
addStatus(QStringLiteral("PointCloud after filtering has: ") + QString::number(nr_points) + QStringLiteral(" data points."));
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<PointType> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<PointType>::Ptr cloud_plane (new pcl::PointCloud<PointType> ());
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
pcl::PointCloud<PointType>::Ptr cloud_f (new pcl::PointCloud<PointType>);
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)
{
addStatus(QStringLiteral("Could not estimate a planar model for the given dataset."));
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<PointType> extract;
extract.setInputCloud (cloudFiltered);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
addStatus(QStringLiteral("PointCloud representing the planar component: ") + QString::number(cloud_plane->points.size()) + QStringLiteral(" data points."));
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloudFiltered = *cloud_f;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<PointType>::Ptr tree (new pcl::search::KdTree<PointType>);
tree->setInputCloud (cloudFiltered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PointType> ec;
ec.setClusterTolerance (0.2); // 20cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (250000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloudFiltered);
ec.extract (cluster_indices);
for (auto it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<PointType>::Ptr cloud_cluster (new pcl::PointCloud<PointType>);
for (auto pit = it->indices.begin (); pit != it->indices.end (); pit++)
{
cloud_cluster->points.push_back (cloudFiltered->points[*pit]); //*
}
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
clusteredClouds.push_back(cloud_cluster);
}
}
