void Process::step() { QTime time; time.start(); switch (mode) { case ProcessNone: break; case ProcessColor: findColor(); findClusters(hitImage, areas); transform2DAreas(areas); findSeqAreas(areas, seqAreas); filterSeqAreas(seqAreas, seqAreasBuffer); break; case ProcessMotion: findMotion(); findClusters(hitImage, areas); transform2DAreas(areas); findSeqAreas(areas, seqAreas); filterSeqAreas(seqAreas, seqAreasBuffer); cvCopy(image, prevImage); break; case ProcessHaar: findHaar(); findSeqAreas(areas, seqAreas); filterSeqAreas(seqAreas, seqAreasBuffer); break; case ProcessContour: findContours(); findClusters(hitImage, areas); transform2DAreas(areas); findSeqAreas(areas, seqAreas); filterSeqAreas(seqAreas, seqAreasBuffer); break; case ProcessHoughCircles: findHoughCircles(); findSeqAreas(areas, seqAreas); filterSeqAreas(seqAreas, seqAreasBuffer); break; } timeMean += time.elapsed(); timeNum++; if ( timeNum == 10 ) { //qDebug() << "Process time:" << timeMean/10; timeMean = 0; timeNum = 0; } }
template <typename PointT> void pcl::UnaryClassifier<PointT>::trainWithLabel ( std::vector<pcl::PointCloud<pcl::FPFHSignature33>, Eigen::aligned_allocator<pcl::PointCloud<pcl::FPFHSignature33> > > &output) { // find clusters std::vector<int> cluster_numbers; findClusters (input_cloud_, cluster_numbers); std::cout << "cluster numbers: "; for (size_t i = 0; i < cluster_numbers.size (); i++) std::cout << cluster_numbers[i] << " "; std::cout << std::endl; for (size_t i = 0; i < cluster_numbers.size (); i++) { // extract all points with the same label number pcl::PointCloud<pcl::PointXYZ>::Ptr label_cloud (new pcl::PointCloud<pcl::PointXYZ>); getCloudWithLabel (input_cloud_, label_cloud, cluster_numbers[i]); // compute FPFH feature histograms for all point of the input point cloud pcl::PointCloud<pcl::FPFHSignature33>::Ptr feature (new pcl::PointCloud<pcl::FPFHSignature33>); computeFPFH (label_cloud, feature, normal_radius_search_, fpfh_radius_search_); // use k-means to cluster the features pcl::PointCloud<pcl::FPFHSignature33>::Ptr kmeans_feature (new pcl::PointCloud<pcl::FPFHSignature33>); kmeansClustering (feature, kmeans_feature, cluster_size_); output.push_back (*kmeans_feature); } }
Transformer& KMeans::fit(const Eigen::MatrixXd& X) { OPENANN_CHECK_EQUALS(X.cols(), D); if(!initialized) initialize(X); OPENANN_CHECK_EQUALS(X.rows(), clusterIndices.size()); findClusters(X); updateCenters(X); return *this; }
vector<PCPolygonPtr> PCPolyhedron::detectPolygons(const PCPtr& cloud, float planeTolerance, float pointsTolerance, bool limitFaces) { float DOT_EPSILON = 0.15; saveCloud("1_toDetect.pcd", *cloud); PCPtr cloudTemp(cloud); float maxFaces = limitFaces ? MAX_FACES : 100; sensor_msgs::PointCloud2::Ptr cloud_blob (new sensor_msgs::PointCloud2()); sensor_msgs::PointCloud2::Ptr cloud_filtered_blob (new sensor_msgs::PointCloud2); pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; vector<PCPolygonPtr> nuevos; PCPtr cloudP (new PC()); pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::ExtractIndices<pcl::PointXYZ> extract; std::vector<ofVec3f> vCloudHull; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (50); seg.setDistanceThreshold (planeTolerance); //original: 0.01 // Create the filtering object int i = 0, nrPoints = cloudTemp->points.size (); // mientras 7% de la nube no se haya procesad+o int numFaces = 0; while (cloudTemp->points.size () > 0.07 * nrPoints && numFaces < maxFaces) { pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloudTemp); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cerr << "Could not estimate a planar model for the given dataset." << std::endl; break; } //FIX PCPtr cloudFilteredTempInliers (new PC()); PCPtr cloudFilteredTempOutliers (new PC()); if(inliers->indices.size() != cloudTemp->size()) { // Extract the inliers extract.setInputCloud (cloudTemp); extract.setIndices (inliers); extract.setNegative (false); extract.filter (*cloudFilteredTempInliers); cloudP = cloudFilteredTempInliers; } else cloudP = cloudTemp; // Create the filtering object extract.setInputCloud (cloudTemp); extract.setIndices (inliers); extract.setNegative (true); if(cloudP->size() != cloudTemp->size()) extract.filter (*cloudFilteredTempOutliers); cloudTemp = cloudFilteredTempOutliers; saveCloud("2_DetectedPol" + ofToString(i) + ".pcd", *cloudP); //Remove outliers by clustering vector<pcl::PointIndices> clusterIndices(findClusters(cloudP, pointsTolerance, 10, 10000)); int debuccount = 0; PCPtr cloudPFiltered (new PC()); if(clusterIndices.size() > 0) { cloudPFiltered = getCloudFromIndices(cloudP, clusterIndices.at(0)); } saveCloud("3_Postfilter_pol" + ofToString(i) + ".pcd",*cloudPFiltered); if (cloudPFiltered->size() < 4) break; //Controlo que las normales sean perpendiculares ofVec3f norm (coefficients->values[0],coefficients->values[1],coefficients->values[2]); norm.normalize(); bool normalCheck = true; for(int i = 0; i < nuevos.size() && normalCheck; i++) { float dot = abs(nuevos[i]->getNormal().dot(norm)); if( dot > DOT_EPSILON) { normalCheck = false; } } if(normalCheck) { //proyecto los puntos sobre el plano pcl::ProjectInliers<pcl::PointXYZ> proj; proj.setModelType(pcl::SACMODEL_PLANE); PCPtr projectedCloud (new PC()); proj.setInputCloud(cloudPFiltered); proj.setModelCoefficients(coefficients); proj.filter(*projectedCloud); saveCloud("4_Proy_Pol" + ofToString(i) + ".pcd",*projectedCloud); PCPolygonPtr pcp(new PCQuadrilateral(*coefficients, projectedCloud)); pcp->detectPolygon(); nuevos.push_back(pcp); numFaces++; } i++; } return nuevos; }
vector<pcl::PointIndices> findClusters(const PCPtr& cloud, float tolerance, int minClusterSize) { return findClusters(cloud, tolerance, minClusterSize, cloud->size()); }
OperationsList*OperationsList::findCluster(const QString& label) const { QList<OperationsList*> res = findClusters( label ); return res.isEmpty()?NULL:res.front(); }
//-------------------------------------------------------------- void ObjectsThread::processCloud() { PCPtr cloud; { inCloudMutex.lock(); cloud = PCPtr(new PC(*inCloud)); inCloud.reset(); inCloudMutex.unlock(); } // Updating temporal detections for (list<TrackedCloudPtr>::iterator iter = trackedClouds.begin(); iter != trackedClouds.end(); iter++) { if((*iter)->hasObject()) { PCPolyhedron* polyhedron = dynamic_cast<PCPolyhedron*>((*iter)->getTrackedObject().get()); if (polyhedron != NULL) { // Está dentro del frustum si todos sus vértices lo están (*iter)->setInViewField(Frustum::IsInFrustum(polyhedron->getVertexs())); } } if(!(*iter)->hasObject() || (*iter)->isInViewField()) { (*iter)->addCounter(-1); } } int size = trackedClouds.size(); trackedClouds.remove_if(countIsLessThanZero); int dif = size - trackedClouds.size(); if(cloud->empty()) { updateDetectedObjects(); return; } list<TrackedCloudPtr> newClouds; int debugCounter = 0; { setObjectsThreadStatus("Detecting clusters..."); saveCloud("rawClusters.pcd", *cloud); std::vector<pcl::PointIndices> clusterIndices = findClusters(cloud, Constants::OBJECT_CLUSTER_TOLERANCE(), Constants::OBJECT_CLUSTER_MIN_SIZE()); for (std::vector<pcl::PointIndices>::const_iterator it = clusterIndices.begin (); it != clusterIndices.end (); ++it) { PCPtr cloudCluster = getCloudFromIndices(cloud, *it); gModel->tableMutex.lock(); TablePtr table = gModel->getTable(); gModel->tableMutex.unlock(); saveCloud("objectsCluster" + ofToString(debugCounter) + ".pcd", *cloudCluster); if(table->isOnTable(cloudCluster)) newClouds.push_back(TrackedCloudPtr (new TrackedCloud(cloudCluster,false))); else newClouds.push_back(TrackedCloudPtr (new TrackedCloud(cloudCluster,true))); debugCounter++; } } // Look into the new clouds for the best fit list<TrackedCloudPtr> cloudsToMatch; list<TrackedCloudPtr> cloudsToAdd; debugCounter = 0; int maxIter = 10; setObjectsThreadStatus("Matching clusters with existing ones..."); do { for (list<TrackedCloudPtr>::iterator iter = newClouds.begin(); iter != newClouds.end(); iter++) { //saveCloudAsFile("clusterInTable" + ofToString(debugCounter) + ".pcd", *(*iter)->getTrackedCloud()); TrackedCloudPtr removedCloud; bool removed = false; bool fitted = findBestFit(*iter, removedCloud, removed); if (removed) cloudsToMatch.push_back(removedCloud); // Push back the old cloud to try again if (!fitted) cloudsToAdd.push_back(*iter); // No matching cloud, this will be a new object debugCounter++; } newClouds = cloudsToMatch; cloudsToMatch.clear(); maxIter--; } while (newClouds.size() > 0 && maxIter > 0); // Effectuate the update of the tracked cloud with the new ones setObjectsThreadStatus("Update existing and new data..."); updateDetectedObjects(); trackedClouds.insert(trackedClouds.end(), cloudsToAdd.begin(), cloudsToAdd.end()); }