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