Пример #1
0
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;
}
Пример #2
0
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);
}
Пример #3
0
void
pcl::recognition::ORROctree::build (const PointCloudIn& points, float voxel_size, const PointCloudN* normals, float enlarge_bounds)
{
  if ( voxel_size <= 0.0f )
    return;

  // Get the bounds of the input point set
  PointXYZ min, max;
  getMinMax3D(points, min, max);

  // Enlarge the bounds a bit to avoid points lying exact on the octree boundaries
  float eps = enlarge_bounds*std::max (std::max (max.x-min.x, max.y-min.y), max.z-min.z);
  float b[6] = {min.x-eps, max.x+eps, min.y-eps, max.y+eps, min.z-eps, max.z+eps};

  // Build an empty octree with the right boundaries and the right number of levels
  this->build (b, voxel_size);

#ifdef PCL_REC_ORR_OCTREE_VERBOSE
  printf("ORROctree::%s(): start\n", __func__);
  printf("point set bounds =\n"
         "[%f, %f]\n"
         "[%f, %f]\n"
         "[%f, %f]\n", min.x, max.x, min.y, max.y, min.z, max.z);
#endif

  size_t num_points = points.size ();

  // Fill the leaves with the points
  for (size_t i = 0 ; i < num_points ; ++i )
  {
    // Create a leaf which contains the i-th point.
    ORROctree::Node* node = this->createLeaf (points[i].x, points[i].y, points[i].z);

    // Make sure that the point is within some leaf
    if ( !node )
    {
      fprintf (stderr, "WARNING in 'ORROctree::%s()': the point (%f, %f, %f) should be within the octree bounds!\n",
        __func__, points[i].x, points[i].y, points[i].z);
      continue;
    }

    // Now, that we have the right leaf -> fill it
    node->getData ()->addToPoint (points[i].x, points[i].y, points[i].z);
    if ( normals )
      node->getData ()->addToNormal (normals->at(i).normal_x, normals->at(i).normal_y, normals->at(i).normal_z);
  }

  // Compute the normals and average points for each full octree node
  if ( normals )
  {
    for ( auto it = full_leaves_.begin() ; it != full_leaves_.end() ; )
    {
      // Compute the average point in the current octree leaf
      (*it)->getData ()->computeAveragePoint ();

      // Compute the length of the average normal
      float normal_length = aux::length3 ((*it)->getData ()->getNormal ());

      // We are suppose to use normals. However, it could be that all normals in this leaf are "illegal", because,
      // e.g., they were not available in the data set. In this case, remove the leaf from the octree.
      if ( normal_length <= numeric_limits<float>::epsilon () )
      {
        this->deleteBranch (*it);
        it = full_leaves_.erase (it);
      }
      else
      {
        aux::mult3 ((*it)->getData ()->getNormal (), 1.0f/normal_length);
        ++it;
      }
    }
  }
  else
  {
    // Iterate over all full leaves and average points
    for (const auto &full_leaf : full_leaves_)
      full_leaf->getData ()->computeAveragePoint ();
  }

#ifdef PCL_REC_ORR_OCTREE_VERBOSE
  printf("ORROctree::%s(): end\n", __func__);
#endif
}