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