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; }
void NestedSampler::run(LivePointsReducer &livePointsReducer, const int NinitialIterationsWithoutClustering, const int NiterationsWithSameClustering, const int maxNdrawAttempts, const double maxRatioOfRemainderToCurrentEvidence, string pathPrefix) { int startTime = time(0); double logMeanLiveEvidence; terminationFactor = maxRatioOfRemainderToCurrentEvidence; outputPathPrefix = pathPrefix; if (printOnTheScreen) { cerr << "------------------------------------------------" << endl; cerr << " Bayesian Inference problem has " << Ndimensions << " dimensions." << endl; cerr << "------------------------------------------------" << endl; cerr << endl; } // Save configuring parameters to an output ASCII file string fileName = "configuringParameters.txt"; string fullPath = outputPathPrefix + fileName; File::openOutputFile(outputFile, fullPath); outputFile << "# List of configuring parameters used for the NSMC." << endl; outputFile << "# Row #1: Ndimensions" << endl; outputFile << "# Row #2: Initial(Maximum) NlivePoints" << endl; outputFile << "# Row #3: Minimum NlivePoints" << endl; outputFile << "# Row #4: NinitialIterationsWithoutClustering" << endl; outputFile << "# Row #5: NiterationsWithSameClustering" << endl; outputFile << "# Row #6: maxNdrawAttempts" << endl; outputFile << "# Row #7: terminationFactor" << endl; outputFile << "# Row #8: Niterations" << endl; outputFile << "# Row #9: Optimal Niterations" << endl; outputFile << "# Row #10: Final Nclusters" << endl; outputFile << "# Row #11: Final NlivePoints" << endl; outputFile << "# Row #12: Computational Time (seconds)" << endl; outputFile << Ndimensions << endl; outputFile << initialNlivePoints << endl; outputFile << minNlivePoints << endl; outputFile << NinitialIterationsWithoutClustering << endl; outputFile << NiterationsWithSameClustering << endl; outputFile << maxNdrawAttempts << endl; outputFile << terminationFactor << endl; // Set up the random number generator. It generates integer random numbers // between 0 and NlivePoints-1, inclusive. uniform_int_distribution<int> discreteUniform(0, NlivePoints-1); // Draw the initial sample from the prior PDF. Different coordinates of a point // can have different priors, so these have to be sampled individually. if (printOnTheScreen) { cerr << "------------------------------------------------" << endl; cerr << " Doing initial sampling of parameter space..." << endl; cerr << "------------------------------------------------" << endl; cerr << endl; } nestedSample.resize(Ndimensions, NlivePoints); int beginIndex = 0; int NdimensionsOfCurrentPrior; ArrayXXd priorSample; for (int i = 0; i < ptrPriors.size(); i++) { // Some priors cover one particalar coordinate, others may cover two or more coordinates // Find out how many dimensions the current prior covers. NdimensionsOfCurrentPrior = ptrPriors[i]->getNdimensions(); // Draw the subset of coordinates randomly from the current prior priorSample.resize(NdimensionsOfCurrentPrior, NlivePoints); ptrPriors[i]->draw(priorSample); // Insert this random subset of coordinates into the total sample of coordinates of points nestedSample.block(beginIndex, 0, NdimensionsOfCurrentPrior, NlivePoints) = priorSample; // Move index to the beginning of the coordinate set of the next prior beginIndex += NdimensionsOfCurrentPrior; } // Compute the log(Likelihood) for each of our points in the live sample logLikelihood.resize(NlivePoints); for (int i = 0; i < NlivePoints; ++i) { logLikelihood(i) = likelihood.logValue(nestedSample.col(i)); } // Initialize the prior mass interval and cumulate it double logWidthInPriorMass = log(1.0 - exp(-1.0/NlivePoints)); // X_0 - X_1 First width in prior mass logCumulatedPriorMass = Functions::logExpSum(logCumulatedPriorMass, logWidthInPriorMass); // 1 - X_1 logRemainingPriorMass = Functions::logExpDifference(logRemainingPriorMass, logWidthInPriorMass); // X_1 // Initialize first part of width in prior mass for trapezoidal rule // X_0 = (2 - X_1), right-side boundary condition for trapezoidal rule double logRemainingPriorMassRightBound = Functions::logExpDifference(log(2), logRemainingPriorMass); double logWidthInPriorMassRight = Functions::logExpDifference(logRemainingPriorMassRightBound,logRemainingPriorMass); // Find maximum log(Likelihood) value in the initial sample of live points. // This information can be useful when reducing the number of live points adopted within the nesting process. logMaxLikelihoodOfLivePoints = logLikelihood.maxCoeff(); // The nested sampling will involve finding clusters in the sample. // This will require the containers clusterIndices and clusterSizes. unsigned int Nclusters = 0; vector<int> clusterIndices(NlivePoints); // clusterIndices must have the same number of elements as the number of live points vector<int> clusterSizes; // The number of live points counted in each cluster is updated everytime one live point // is removed from the sample. // Start the nested sampling loop. Each iteration, we'll replace the point with the worst likelihood. // New points are drawn from the prior, but with the constraint that they should have a likelihood // that is better than the currently worst one. if (printOnTheScreen) { cerr << "-------------------------------" << endl; cerr << " Starting nested sampling... " << endl; cerr << "-------------------------------" << endl; cerr << endl; } bool nestedSamplingShouldContinue = true; bool livePointsShouldBeReduced = (initialNlivePoints > minNlivePoints); // Update live points only if required Niterations = 0; do { // Resize the arrays to make room for an additional point. // Do so without destroying the original contents. posteriorSample.conservativeResize(Ndimensions, Niterations + 1); logLikelihoodOfPosteriorSample.conservativeResize(Niterations + 1); logWeightOfPosteriorSample.conservativeResize(Niterations + 1); // Find the point with the worst likelihood. This likelihood value will set a constraint // when drawing new points later on. int indexOfLivePointWithWorstLikelihood; worstLiveLogLikelihood = logLikelihood.minCoeff(&indexOfLivePointWithWorstLikelihood); // Although we will replace the point with the worst likelihood in the live sample, we will save // it in our collection of posterior sample. Also save its likelihood value. The weight is // computed and collected at the end of each iteration. posteriorSample.col(Niterations) = nestedSample.col(indexOfLivePointWithWorstLikelihood); logLikelihoodOfPosteriorSample(Niterations) = worstLiveLogLikelihood; // Compute the (logarithm of) the mean likelihood of the set of live points. // Note that we are not computing mean(log(likelihood)) but log(mean(likelhood)). // Since we are only storing the log(likelihood) values, this results in a peculiar // way of computing the mean. This will be used for computing the mean live evidence // at the end of the iteration. logMeanLikelihoodOfLivePoints = logLikelihood(0); for (int m = 1; m < NlivePoints; m++) { logMeanLikelihoodOfLivePoints = Functions::logExpSum(logMeanLikelihoodOfLivePoints, logLikelihood(m)); } logMeanLikelihoodOfLivePoints -= log(NlivePoints); // Find clusters in our live sample of points. Don't do this every iteration but only // every x iterations, where x is given by 'NiterationsWithSameClustering'. if ((Niterations % NiterationsWithSameClustering) == 0) { // Don't do clustering the first N iterations, where N is user-specified. That is, // the first N iterations we assume that there is only 1 cluster containing all the points. // This is often useful because initially the points may be sampled from a uniform prior, // and we therefore don't expect any clustering _before_ the algorithm is able to tune in on // the island(s) of high likelihood. Clusters found in the first N initial iterations are // therefore likely purely noise. if (Niterations < NinitialIterationsWithoutClustering) { // There is only 1 cluster, containing all objects. All points have the same cluster // index, namely 0. Nclusters = 1; clusterSizes.resize(1); clusterSizes[0] = NlivePoints; fill(clusterIndices.begin(), clusterIndices.end(), 0); } else { // After the first N initial iterations, we do a proper clustering. Nclusters = clusterer.cluster(nestedSample, clusterIndices, clusterSizes); } } // Draw a new point, which should replace the point with the worst likelihood. // This new point should be drawn from the prior, but with a likelihood greater // than the current worst likelihood. The drawing algorithm may need a starting point, // for which we will take a randomly chosen point of the live sample (excluding the // worst point). int indexOfRandomlyChosenLivePoint = 0; if (NlivePoints > 1) { // Select randomly an index of a sample point, but not the one of the worst point do { // 0 <= indexOfRandomlyChosenLivePoint < NlivePoints indexOfRandomlyChosenLivePoint = discreteUniform(engine); } while (indexOfRandomlyChosenLivePoint == indexOfLivePointWithWorstLikelihood); } // drawnPoint will be a starting point as input, and will contain the newly drawn point as output ArrayXd drawnPoint = nestedSample.col(indexOfRandomlyChosenLivePoint); double logLikelihoodOfDrawnPoint = 0.0; bool newPointIsFound = drawWithConstraint(nestedSample, Nclusters, clusterIndices, clusterSizes, drawnPoint, logLikelihoodOfDrawnPoint, maxNdrawAttempts); // If the adopted sampler produces an error (e.g. in the case of the ellipsoidal sampler a failure // in the ellipsoid matrix decomposition), then we can stop right here. nestedSamplingShouldContinue = verifySamplerStatus(); if (!nestedSamplingShouldContinue) break; // If we didn't find a point with a better likelihood, then we can stop right here. if (!newPointIsFound) { nestedSamplingShouldContinue = false; cerr << "Can't find point with a better Likelihood." << endl; cerr << "Stopping the nested sampling loop prematurely." << endl; break; } // Replace the point having the worst likelihood with our newly drawn one. nestedSample.col(indexOfLivePointWithWorstLikelihood) = drawnPoint; logLikelihood(indexOfLivePointWithWorstLikelihood) = logLikelihoodOfDrawnPoint; // If we got till here this is not the last iteration possible, hence // update all the information for the next iteration. // Check if the number of live points has not reached the minimum allowed, // and update it for the next iteration. if (livePointsShouldBeReduced) { // Update the number of live points for the current iteration based on the previous number. // If the number of live points reaches the minimum allowed // then do not update the number anymore. updatedNlivePoints = livePointsReducer.updateNlivePoints(); if (updatedNlivePoints > NlivePoints) { // Terminate program if new number of live points is greater than previous one cerr << "Something went wrong in the reduction of the live points." << endl; cerr << "The new number of live points is greater than the previous one." << endl; cerr << "Quitting program. " << endl; break; } // If the lower bound for the number of live points has not been reached yet, // the process should be repeated at the next iteration. // Otherwise the minimun number allowed is reached right now. In this case // stop the reduction process starting from the next iteration. livePointsShouldBeReduced = (updatedNlivePoints > minNlivePoints); if (updatedNlivePoints != NlivePoints) { // Resize all eigen arrays and vectors of dimensions NlivePoints according to // new number of live points evaluated. In case previos and new number // of live points coincide, no resizing is done. vector<int> indicesOfLivePointsToRemove = livePointsReducer.findIndicesOfLivePointsToRemove(engine); // At least one live point has to be removed, hence update the sample removeLivePointsFromSample(indicesOfLivePointsToRemove, clusterIndices, clusterSizes); // Since everything is fine update discreteUniform with the corresponding new upper bound uniform_int_distribution<int> discreteUniform2(0, updatedNlivePoints-1); discreteUniform = discreteUniform2; } } // Store the new number of live points in the vector containing this information. // This is done even if the new number is the same as the previous one. NlivePointsPerIteration.push_back(NlivePoints); // Compute the mean live evidence given the previous set of live points (see Keeton 2011, MNRAS) logMeanLiveEvidence = logMeanLikelihoodOfLivePoints + Niterations * (log(NlivePoints) - log(NlivePoints + 1)); // Compute the ratio of the evidence of the live sample to the current Skilling's evidence. // Only when we gathered enough evidence, this ratio will be sufficiently small so that we can stop the iterations. ratioOfRemainderToCurrentEvidence = exp(logMeanLiveEvidence - logEvidence); // Re-evaluate the stopping criterion, using the condition suggested by Keeton (2011) nestedSamplingShouldContinue = (ratioOfRemainderToCurrentEvidence > maxRatioOfRemainderToCurrentEvidence); // Shrink prior mass interval according to proper number of live points // (see documentation by Enrico Corsaro October 2013). When reducing the number of live points // the equation is a generalized version of that used by Skilling 2004. The equation // reduces to the standard case when the new number of live points is the same // as the previous one. // ---- Use the line below for simple rectangular rule ---- // double logWeight = logWidthInPriorMass; // -------------------------------------------------------- double logStretchingFactor = Niterations*((1.0/NlivePoints) - (1.0/updatedNlivePoints)); logWidthInPriorMass = logRemainingPriorMass + Functions::logExpDifference(0.0, logStretchingFactor - 1.0/updatedNlivePoints); // X_i - X_(i+1) // Compute the logWeight according to the trapezoidal rule 0.5*(X_(i-1) - X_(i+1)) // and new contribution of evidence to be cumulated to the total evidence. // This is done in logarithmic scale by summing the right (X_(i-1) - X_i) and left part (X_i - X_(i+1)) // of the total width in prior mass required for the trapezoidal rule. We do this computation at the end // of the nested iteration because we need to know the new remaining prior mass of the next iteration. double logWidthInPriorMassLeft = logWidthInPriorMass; // ---- Use the line below for trapezoidal rule ---- double logWeight = log(0.5) + Functions::logExpSum(logWidthInPriorMassLeft, logWidthInPriorMassRight); double logEvidenceContributionNew = logWeight + worstLiveLogLikelihood; // Save log(Weight) of the current iteration logWeightOfPosteriorSample(Niterations) = logWeight; // Update the right part of the width in prior mass interval by replacing it with the left part logWidthInPriorMassRight = logWidthInPriorMass; // Update the evidence and the information Gain double logEvidenceNew = Functions::logExpSum(logEvidence, logEvidenceContributionNew); informationGain = exp(logEvidenceContributionNew - logEvidenceNew) * worstLiveLogLikelihood + exp(logEvidence - logEvidenceNew) * (informationGain + logEvidence) - logEvidenceNew; logEvidence = logEvidenceNew; // Print current information on the screen, if required if (printOnTheScreen) { if ((Niterations % 50) == 0) { cerr << "Nit: " << Niterations << " Ncl: " << Nclusters << " Nlive: " << NlivePoints << " CPM: " << exp(logCumulatedPriorMass) << " Ratio: " << ratioOfRemainderToCurrentEvidence << " log(E): " << logEvidence << " IG: " << informationGain << endl; } } // Update total width in prior mass and remaining width in prior mass from beginning to current iteration // and use this information for the next iteration (if any) logCumulatedPriorMass = Functions::logExpSum(logCumulatedPriorMass, logWidthInPriorMass); logRemainingPriorMass = logStretchingFactor + logRemainingPriorMass - 1.0/updatedNlivePoints; // Update new number of live points in NestedSampler class NlivePoints = updatedNlivePoints; // Increase nested loop counter Niterations++; } while (nestedSamplingShouldContinue); // Add the remaining live sample of points to our collection of posterior points // (i.e parameter coordinates, likelihood values and weights) unsigned int oldNpointsInPosterior = posteriorSample.cols(); posteriorSample.conservativeResize(Ndimensions, oldNpointsInPosterior + NlivePoints); // First make enough room posteriorSample.block(0, oldNpointsInPosterior, Ndimensions, NlivePoints) = nestedSample; // Then copy the live sample to the posterior array logWeightOfPosteriorSample.conservativeResize(oldNpointsInPosterior + NlivePoints); logWeightOfPosteriorSample.segment(oldNpointsInPosterior, NlivePoints).fill(logRemainingPriorMass - log(NlivePoints)); // Check if the best condition to impose logLikelihoodOfPosteriorSample.conservativeResize(oldNpointsInPosterior + NlivePoints); logLikelihoodOfPosteriorSample.segment(oldNpointsInPosterior, NlivePoints) = logLikelihood; // Compute Skilling's error on the log(Evidence) logEvidenceError = sqrt(fabs(informationGain)/NlivePoints); // Add Mean Live Evidence of the remaining live sample of points to the total log(Evidence) collected logEvidence = Functions::logExpSum(logMeanLiveEvidence, logEvidence); if (printOnTheScreen) { cerr << "------------------------------------------------" << endl; cerr << " Final log(E): " << logEvidence << " +/- " << logEvidenceError << endl; cerr << "------------------------------------------------" << endl; } // Print total computational time printComputationalTime(startTime); // Append information to existing output file and close stream afterwards outputFile << Niterations << endl; outputFile << static_cast<int>((NlivePoints*informationGain) + (NlivePoints*sqrt(Ndimensions*1.0))) << endl; outputFile << Nclusters << endl; outputFile << NlivePoints << endl; outputFile << computationalTime << endl; }
int KmeansClusterer::cluster(RefArrayXXd sample, vector<int> &optimalClusterIndices, vector<int> &optimalClusterSizes) { bool convergedSuccessfully; unsigned int Npoints = sample.cols(); unsigned int Ndimensions = sample.rows(); unsigned int optimalNclusters; double bestBICvalue = numeric_limits<double>::max(); double BICvalue; double sumOfDistancesToClosestCenter; double bestSumOfDistancesToClosestCenter = numeric_limits<double>::max(); vector<int> clusterIndices(Npoints); // For each point the index of the cluster to ... vector<int> bestClusterIndices(Npoints); // ... which it belongs ArrayXd clusterSizes; // Not vector<int> because will be used in Eigen array expressions ArrayXd bestClusterSizes; ArrayXXd centers; ArrayXXd bestCenters; // As we don't know a prior the optimal number of clusters, loop over a // user-specified range of clusters, and determine which number gives the // optimal clustering for (unsigned int Nclusters = minNclusters; Nclusters <= maxNclusters; ++Nclusters) { centers = ArrayXXd::Zero(Ndimensions, Nclusters); // coordinates of each of the old cluster centers bestCenters = ArrayXXd::Zero(Ndimensions, Nclusters); // coordinates of the best centers (over all trials) clusterSizes = ArrayXd::Zero(Nclusters); // # of points belonging to each cluster... bestClusterSizes = ArrayXd::Zero(Nclusters); // ... 'double', to avoid casting problems. // The k-means algorithm is sensitive to the choice of the initial centers. // We therefore run the algorithm 'Ntrial' times, and take the best clustering. bestSumOfDistancesToClosestCenter = numeric_limits<double>::max(); for (int m = 0; m < Ntrials; ++m) { chooseInitialClusterCenters(sample, centers, Nclusters); convergedSuccessfully = updateClusterCentersUntilConverged(sample, centers, clusterSizes, clusterIndices, sumOfDistancesToClosestCenter, relTolerance); // If the convergence was not successfull (e.g. because some clusters contain 0 or 1 points), // we likely had an unfortunate set of initial cluster centers. In this case, simply continue // with the next 'trial'. if (!convergedSuccessfully) continue; // If we did obtain a successful convergence, compare it with the previous clusterings // (all of them with the same number of clusters), and keep the best one. if (sumOfDistancesToClosestCenter < bestSumOfDistancesToClosestCenter) { bestSumOfDistancesToClosestCenter = sumOfDistancesToClosestCenter; bestCenters = centers; bestClusterIndices = clusterIndices; bestClusterSizes = clusterSizes; } } // end loop over Ntrials to determine the best clustering trying different initial centers // Evaluate the current number of clusters, using the BIC value. Note that this is only necessary // if the user selected more than one particular number of clusters. if (maxNclusters - minNclusters > 1) { BICvalue = evaluateBICvalue(sample, bestCenters, bestClusterSizes, bestClusterIndices); if (BICvalue < bestBICvalue) { // We found a cluster combination that is better than anything found before. Save it. // In what follows 'best' refers to the best cluster configuration given a specific // number of clusters. 'optimal' refers to the optimal configuration over all possible // values for the number of clusters. bestBICvalue = BICvalue; optimalNclusters = Nclusters; optimalClusterIndices = bestClusterIndices; optimalClusterSizes.resize(Nclusters); for (int n = 0; n < Nclusters; ++n) { optimalClusterSizes[n] = bestClusterSizes(n); } } } else { // User allowed only 1 particular number of clusters. Computation of BIC // to compare is no longer required. optimalNclusters = Nclusters; optimalClusterIndices = bestClusterIndices; optimalClusterSizes.resize(Nclusters); for (int n = 0; n < Nclusters; ++n) { optimalClusterSizes[n] = bestClusterSizes(n); } } } // end loop over Nclusters // That's it! return optimalNclusters; }