ORROctree::Node* pcl::recognition::ORROctree::getRandomFullLeafOnSphere (const float* p, float radius) const { vector<int> tmp_ids; tmp_ids.reserve (8); pcl::common::UniformGenerator<int> randgen (0, 1, static_cast<uint32_t> (time (NULL))); list<ORROctree::Node*> nodes; nodes.push_back (root_); while ( !nodes.empty () ) { // Get the last element in the list ORROctree::Node *node = nodes.back (); // Remove the last element from the list nodes.pop_back (); // Check if the sphere intersects the current node if ( fabs (radius - aux::distance3<float> (p, node->getCenter ())) <= node->getRadius () ) { // We have an intersection -> push back the children of the current node if ( node->hasChildren () ) { // Prepare the tmp id vector for ( int i = 0 ; i < 8 ; ++i ) tmp_ids.push_back (i); // Push back the children in random order for ( int i = 0 ; i < 8 ; ++i ) { randgen.setParameters (0, static_cast<int> (tmp_ids.size ()) - 1); int rand_pos = randgen.run (); nodes.push_back (node->getChild (tmp_ids[rand_pos])); // Remove the randomly selected id tmp_ids.erase (tmp_ids.begin () + rand_pos); } } else if ( node->hasData () ) return node; } } return NULL; }
void show_octree (ORROctree* octree, PCLVisualizer& viz, bool show_full_leaves_only) { vtkSmartPointer<vtkPolyData> vtk_octree = vtkSmartPointer<vtkPolyData>::New (); vtkSmartPointer<vtkAppendPolyData> append = vtkSmartPointer<vtkAppendPolyData>::New (); cout << "There are " << octree->getFullLeaves ().size () << " full leaves.\n"; if ( show_full_leaves_only ) { std::vector<ORROctree::Node*>& full_leaves = octree->getFullLeaves (); for ( std::vector<ORROctree::Node*>::iterator it = full_leaves.begin () ; it != full_leaves.end () ; ++it ) // Add it to the other cubes node_to_cube (*it, append); } else { ORROctree::Node* node; std::list<ORROctree::Node*> nodes; nodes.push_back (octree->getRoot ()); while ( !nodes.empty () ) { node = nodes.front (); nodes.pop_front (); // Visualize the node if it has children if ( node->getChildren () ) { // Add it to the other cubes node_to_cube (node, append); // Add all the children to the working list for ( int i = 0 ; i < 8 ; ++i ) nodes.push_back (node->getChild (i)); } // If we arrived at a leaf -> check if it's full and visualize it else if ( node->getData () ) node_to_cube (node, append); } } // Just print the leaf size std::vector<ORROctree::Node*>::iterator first_leaf = octree->getFullLeaves ().begin (); if ( first_leaf != octree->getFullLeaves ().end () ) printf("leaf size = %f\n", (*first_leaf)->getBounds ()[1] - (*first_leaf)->getBounds ()[0]); // Save the result append->Update(); vtk_octree->DeepCopy (append->GetOutput ()); // Add to the visualizer vtkRenderer *renderer = viz.getRenderWindow ()->GetRenderers ()->GetFirstRenderer (); vtkSmartPointer<vtkActor> octree_actor = vtkSmartPointer<vtkActor>::New(); vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New (); mapper->SetInput(vtk_octree); octree_actor->SetMapper(mapper); // Set the appearance & add to the renderer octree_actor->GetProperty ()->SetColor (1.0, 1.0, 1.0); octree_actor->GetProperty ()->SetLineWidth (1); octree_actor->GetProperty ()->SetRepresentationToWireframe (); renderer->AddActor(octree_actor); }