node * findBestFit(node * root, int M, node *fit) {
if (root == NULL)
return fit;
if (root->data == M)
return root;
if (root->data < M)
return findBestFit(root->right, M, fit);
else{
if (fit == NULL || ((fit->data > root->data) && (root->data > M)))
fit = root;
return findBestFit(root->left, M, fit);
}
}
void main() {
node *root = NULL, *fit=NULL;
int val;
printf("\nValue = ");
scanf("%d",&val);
while(val >= 0) {
root = insert(root, val);
printf("Value = ");
scanf("%d",&val);
}
printf("\n");
inorder(root);
zigzagorder(root);
printf("\n");
printf("\nM :: ");
scanf("%d",&val);
fit = findBestFit(root, val, NULL);
if (fit != NULL)
printf("Best Fit :: %d\n",fit->data);
else
printf("No Fit\n");
}
vector<PCPolygonPtr> PCPolyhedron::mergePolygons(vector<PCPolygonPtr>& toMerge) {
vector<PCPolygonPtr> aAgregar;
vector<PCPolygonPtr> aProcesar;
int remainingIters = 10;
do {
for (int i = 0; i < toMerge.size(); i++) {
PCPolygonPtr removed;
bool wasRemoved = false;
bool fitted = findBestFit(toMerge[i], removed, wasRemoved);
if (wasRemoved) {
aProcesar.push_back(removed);
}
if (!fitted) {
aAgregar.push_back(toMerge[i]);
}
}
toMerge = aProcesar;
aProcesar.clear();
remainingIters--;
} while (toMerge.size() > 0 && remainingIters > 0);
vector<PCPolygonPtr> keep;
vector<PCPolygonPtr> adjust;
for (vector<PCPolygonPtr>::iterator iter = pcpolygons.begin(); iter != pcpolygons.end(); iter++) {
if ((*iter)->hasMatching())
keep.push_back(*iter);
else
adjust.push_back(*iter);
}
vector<PCPolygonPtr> missing = updatePolygons();
vector<PCPolygonPtr> polygonsInBox(discardPolygonsOutOfBox(aAgregar, keep));
// Setea el nombre de los poligonos nuevos, con el poligono más cercano
for (int i = 0; i < polygonsInBox.size(); i++) {
if (indexOf(pcpolygons, polygonsInBox[i]) < 0) {
PCPolygonPtr closer = findCloserPolygon(polygonsInBox[i],missing);
polygonsInBox[i]->getPolygonModelObject()->setName(closer->getPolygonModelObject()->getName());
polygonsInBox[i]->setEstimated(false);
keep.push_back(polygonsInBox[i]);
}
}
polygonsInBox.clear();
return keep;
}
//--------------------------------------------------------------
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());
}
