//-------------------------------------------------------------- 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()); }