C++ AlignedPointTVector类代码示例

 template<typename PointT> void
 OutofcoreOctreeRamContainer<PointT>::readRange (const boost::uint64_t start, const boost::uint64_t count,
                                          AlignedPointTVector& v)
 {
   v.resize (count);
   memcpy (v.data (), container_.data () + start, count * sizeof(PointT));
 }

template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> int
pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getApproxIntersectedVoxelCentersBySegment (
    const Eigen::Vector3f& origin,
    const Eigen::Vector3f& end,
    AlignedPointTVector &voxel_center_list,
    float precision)
{
  Eigen::Vector3f direction = end - origin;
  float norm = direction.norm ();
  direction.normalize ();

  const float step_size = static_cast<const float> (resolution_) * precision;
  // Ensure we get at least one step for the first voxel.
  const int nsteps = std::max (1, static_cast<int> (norm / step_size));

  OctreeKey prev_key;

  bool bkeyDefined = false;

  // Walk along the line segment with small steps.
  for (int i = 0; i < nsteps; ++i)
  {
    Eigen::Vector3f p = origin + (direction * step_size * static_cast<const float> (i));

    PointT octree_p;
    octree_p.x = p.x ();
    octree_p.y = p.y ();
    octree_p.z = p.z ();

    OctreeKey key;
    this->genOctreeKeyforPoint (octree_p, key);

    // Not a new key, still the same voxel.
    if ((key == prev_key) && (bkeyDefined) )
      continue;

    prev_key = key;
    bkeyDefined = true;

    PointT center;
    genLeafNodeCenterFromOctreeKey (key, center);
    voxel_center_list.push_back (center);
  }

  OctreeKey end_key;
  PointT end_p;
  end_p.x = end.x ();
  end_p.y = end.y ();
  end_p.z = end.z ();
  this->genOctreeKeyforPoint (end_p, end_key);
  if (!(end_key == prev_key))
  {
    PointT center;
    genLeafNodeCenterFromOctreeKey (end_key, center);
    voxel_center_list.push_back (center);
  }

  return (static_cast<int> (voxel_center_list.size ()));
}

 template<typename PointT> void
 OutofcoreOctreeRamContainer<PointT>::insertRange (const PointT* const * start, const boost::uint64_t count)
 {
   AlignedPointTVector temp;
   temp.resize (count);
   for (boost::uint64_t i = 0; i < count; i++)
   {
     temp[i] = *start[i];
   }
   container_.insert (container_.end (), temp.begin (), temp.end ());
 }

 template<typename ContainerT, typename PointT> void
 OutofcoreOctreeBase<ContainerT, PointT>::queryBBIncludes_subsample (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const size_t query_depth, const double percent, AlignedPointTVector& dst) const
 {
   boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
   dst.clear ();
   root_->queryBBIncludes_subsample (min, max, query_depth, percent, dst);
 }

void point_test(octree_disk& t)
{
  boost::mt19937 rng(rngseed);
  boost::uniform_real<float> dist(0,1);

  double query_box_min[3];
  double qboxmax[3];

  for(int i = 0; i < 10; i++)
  {
    //std::cout << "query test round " << i << std::endl;
    for(int j = 0; j < 3; j++)
    {
      query_box_min[j] = dist(rng);
      qboxmax[j] = dist(rng);

      if(qboxmax[j] < query_box_min[j])
      {
        std::swap(query_box_min[j], qboxmax[j]);
      }
    }

    //query the trees
    AlignedPointTVector p_ot;

    t.queryBBIncludes(query_box_min, qboxmax, t.getDepth(), p_ot);

    //query the list
    AlignedPointTVector pointsinregion;

    for(AlignedPointTVector::iterator pointit = points.begin (); pointit != points.end (); ++pointit)
    {
      if((query_box_min[0] <= pointit->x) && (pointit->x < qboxmax[0]) && (query_box_min[1] < pointit->y) && (pointit->y < qboxmax[1]) && (query_box_min[2] <= pointit->z) && (pointit->z < qboxmax[2]))
      {
        pointsinregion.push_back(*pointit);
      }
    }

    EXPECT_EQ (p_ot.size (), pointsinregion.size ());

    //very slow exhaustive comparison
    while( !p_ot.empty () )
    {
      AlignedPointTVector::iterator it;
      it = std::find_first_of(p_ot.begin(), p_ot.end(), pointsinregion.begin (), pointsinregion.end (), compPt);

      if(it != p_ot.end())
      {
        p_ot.erase(it);
      }
      else
      {
        FAIL () <<  "Dropped Point from tree1!" << std::endl;
        break;
      }
    }

    EXPECT_TRUE(p_ot.empty());
  }
}

template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> int
pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getOccupiedVoxelCenters (
    AlignedPointTVector &voxelCenterList_arg) const
{
  OctreeKey key;
  key.x = key.y = key.z = 0;

  voxelCenterList_arg.clear ();

  return getOccupiedVoxelCentersRecursive (this->rootNode_, key, voxelCenterList_arg);

}

template<typename Container, typename PointT> boost::uint64_t
octree_base<Container, PointT>::addDataToLeaf (const AlignedPointTVector& p)
{
    boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);

    const bool _FORCE_BB_CHECK = true;

    uint64_t pt_added = root_->addDataToLeaf (p, _FORCE_BB_CHECK);

    assert (p.size () == pt_added);

    return (pt_added);
}

template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> int
pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getOccupiedVoxelCentersRecursive (
    const BranchNode* node_arg,
    const OctreeKey& key_arg,
    AlignedPointTVector &voxelCenterList_arg) const
{
  // child iterator
  unsigned char childIdx;

  int voxelCount = 0;

  // iterate over all children
  for (childIdx = 0; childIdx < 8; childIdx++)
  {
    if (!this->branchHasChild (*node_arg, childIdx))
      continue;

    const OctreeNode * childNode;
    childNode = this->getBranchChildPtr (*node_arg, childIdx);

    // generate new key for current branch voxel
    OctreeKey newKey;
    newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2)));
    newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1)));
    newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0)));

    switch (childNode->getNodeType ())
    {
      case BRANCH_NODE:
      {
        // recursively proceed with indexed child branch
        voxelCount += getOccupiedVoxelCentersRecursive (static_cast<const BranchNode*> (childNode), newKey, voxelCenterList_arg);
        break;
      }
      case LEAF_NODE:
      {
        PointT newPoint;

        genLeafNodeCenterFromOctreeKey (newKey, newPoint);
        voxelCenterList_arg.push_back (newPoint);

        voxelCount++;
        break;
      }
      default:
        break;
    }
  }
  return (voxelCount);
}

template<typename PointT, typename LeafT, typename OctreeT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getIntersectedVoxelCenters (
    Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxelCenterList) const
{
  OctreeKey key;
  key.x = key.y = key.z = 0;

  voxelCenterList.clear ();
  voxelCenterList.reserve (this->leafCount_);

  // Voxel childIdx remapping
  unsigned char a = 0;

  double minX, minY, minZ, maxX, maxY, maxZ;

  initIntersectedVoxel (origin, direction, minX, minY, minZ, maxX, maxY, maxZ, a);

  if (max (max (minX, minY), minZ) < min (min (maxX, maxY), maxZ))
    return getIntersectedVoxelCentersRecursive (minX, minY, minZ, maxX, maxY, maxZ, a, this->rootNode_, key,
                                                voxelCenterList);

  return (0);
}

    template<typename PointT> void
    OutofcoreOctreeRamContainer<PointT>::readRangeSubSample (const boost::uint64_t start, 
                                                      const boost::uint64_t count,
                                                      const double percent, 
                                                      AlignedPointTVector& v)
    {
      /** \todo change the subsampling technique to use built in PCL sampling */
      boost::uint64_t samplesize = static_cast<boost::uint64_t> (percent * static_cast<double> (count));

      boost::mutex::scoped_lock lock (rng_mutex_);

      boost::uniform_int < boost::uint64_t > buffdist (start, start + count);
      boost::variate_generator<boost::mt19937&, boost::uniform_int<boost::uint64_t> > buffdie (rand_gen_, buffdist);

      for (boost::uint64_t i = 0; i < samplesize; i++)
      {
        boost::uint64_t buffstart = buffdie ();
        v.push_back (container_[buffstart]);
      }
    }

template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::getIntersectedVoxelCenters (
    Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxel_center_list,
    int max_voxel_count) const
{
  OctreeKey key;
  key.x = key.y = key.z = 0;

  voxel_center_list.clear ();

  // Voxel child_idx remapping
  unsigned char a = 0;

  double min_x, min_y, min_z, max_x, max_y, max_z;

  initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);

  if (std::max (std::max (min_x, min_y), min_z) < std::min (std::min (max_x, max_y), max_z))
    return getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
                                                voxel_center_list, max_voxel_count);

  return (0);
}

//loads chunks of up to 2e9 pts at a time; this is a completely arbitrary number
template<typename Container, typename PointT> void
octree_base<Container, PointT>::buildLOD (octree_base_node<Container, PointT>** current_branch, const int current_dims)
{
    //stop if this brach DNE
    if (!current_branch[current_dims - 1])
    {
        return;
    }

    if ((current_branch[current_dims - 1]->numchildren () == 0)
            && (!current_branch[current_dims - 1]->hasUnloadedChildren ()))//at leaf: subsample, remove, and copy to higher nodes
    {
        //this node's idx is (current_dims-1)
        octree_base_node<Container, PointT>* leaf = current_branch[current_dims - 1];

        boost::uint64_t leaf_start_size = leaf->payload->size ();
        if (leaf_start_size > 0)//skip empty
        {
            for (boost::uint64_t startp = 0; startp < leaf_start_size; startp += LOAD_COUNT_)
            {
                //there are (current_dims-1) nodes above this one, indexed 0 thru (current_dims-2)
                for (size_t level = (current_dims - 1); level >= 1; level--)
                {
                    //the target
                    octree_base_node<Container, PointT>* target_parent = current_branch[level - 1];

                    //the percent to copy
                    //each level up the chain gets sample_precent^l of the leaf's data
                    double percent = pow (double (octree_base_node<Container, PointT>::sample_precent), double (current_dims - level));

                    //read in percent of node
                    AlignedPointTVector v;
                    if ((startp + LOAD_COUNT_) < leaf_start_size)
                    {
                        leaf->payload->readRangeSubSample (startp, LOAD_COUNT_, percent, v);
                    }
                    else
                    {
                        leaf->payload->readRangeSubSample (startp, leaf_start_size - startp, percent, v);
                    }

                    //write to the target
                    if (!v.empty ())
                    {
                        target_parent->payload->insertRange ( v );
//                target_parent->payload->insertRange (&(v.front ()), v.size ());
                        this->incrementPointsInLOD (target_parent->depth, v.size ());
                    }

                }
            }
        }
    }
    else//not at leaf, keep going down
    {
        //clear this node, in case we are updating the LOD
        current_branch[current_dims - 1]->payload->clear ();

        const int next_dims = current_dims + 1;
        octree_base_node<Container, PointT>** next_branch = new octree_base_node<Container, PointT>*[next_dims];
        memcpy (next_branch, current_branch, current_dims * sizeof(octree_base_node<Container, PointT>**));

        size_t numchild = current_branch[current_dims - 1]->numchildren ();
        if ((numchild != 8) && (current_branch[current_dims - 1]->hasUnloadedChildren ()))
        {
            current_branch[current_dims - 1]->loadChildren (false);
            numchild = current_branch[current_dims - 1]->numchildren ();
        }

        for (size_t i = 0; i < numchild; i++)
        {
            next_branch[next_dims - 1] = next_branch[current_dims - 1]->children[i];
            buildLOD (next_branch, next_dims);
        }

        delete[] next_branch;
    }
}

TEST (PCL, Outofcore_Ram_Tree)
{
  Eigen::Vector3d min (0.0,0.0,0.0);
  Eigen::Vector3d max (1.0, 1.0, 1.0);

  const boost::filesystem::path filename_otreeA = "ram_tree/ram_tree.oct_idx";

  octree_ram t (min, max, .1, filename_otreeA, "ECEF");

  boost::mt19937 rng (rngseed);
  //boost::uniform_real<double> dist(0,1);//for testing sparse
  boost::normal_distribution<float> dist (0.5f, .1f);//for testing less sparse
  PointT p;

  points.resize (numPts);
  for (size_t i = 0; i < numPts; i++)
  {
    p.x = dist(rng);
    p.y = dist(rng);
    p.z = dist(rng);

    points[i] = p;
  }

  t.addDataToLeaf_and_genLOD (points);
  //t.addDataToLeaf(points);

  Eigen::Vector3d qboxmin;
  Eigen::Vector3d qboxmax;
  for (int i = 0; i < 10; i++)
  {
    //std::cout << "query test round " << i << std::endl;
    for (int j = 0; j < 3; j++)
    {
      qboxmin[j] = dist (rng);
      qboxmax[j] = dist (rng);

      if (qboxmax[j] < qboxmin[j])
      {
        std::swap (qboxmin[j], qboxmax[j]);
      }
    }

    //query the trees
    AlignedPointTVector p_ot1;
    t.queryBBIncludes (qboxmin, qboxmax, t.getDepth (), p_ot1);

    //query the list
    AlignedPointTVector pointsinregion;
    BOOST_FOREACH(const PointT& p, points)
    {
      if ((qboxmin[0] <= p.x) && (p.x <= qboxmax[0]) && (qboxmin[1] <= p.y) && (p.y <= qboxmax[1]) && (qboxmin[2] <= p.z) && (p.z <= qboxmax[2]))
      {
        pointsinregion.push_back (p);
      }
    }

    EXPECT_EQ (p_ot1.size (), pointsinregion.size ());

    //very slow exhaustive comparison
    while (!p_ot1.empty ())
    {
      AlignedPointTVector::iterator it;
      it = std::find_first_of (p_ot1.begin (), p_ot1.end (), pointsinregion.begin (), pointsinregion.end (), compPt);

      if (it != p_ot1.end ())
      {
        p_ot1.erase(it);
      }
      else
      {
        break;
        FAIL () <<  "Dropped Point from tree1!" << std::endl;
      }
    }

    EXPECT_TRUE (p_ot1.empty ());
  }
}

template<typename PointT, typename LeafT, typename BranchT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafT, BranchT>::getIntersectedVoxelCentersRecursive (
    double minX, double minY, double minZ, double maxX, double maxY, double maxZ, unsigned char a,
    const OctreeNode* node, const OctreeKey& key, AlignedPointTVector &voxelCenterList, int maxVoxelCount) const
{
  if (maxX < 0.0 || maxY < 0.0 || maxZ < 0.0)
    return (0);

  // If leaf node, get voxel center and increment intersection count
  if (node->getNodeType () == LEAF_NODE)
  {
    PointT newPoint;

    this->genLeafNodeCenterFromOctreeKey (key, newPoint);

    voxelCenterList.push_back (newPoint);

    return (1);
  }

  // Voxel intersection count for branches children
  int voxelCount = 0;

  // Voxel mid lines
  double midX = 0.5 * (minX + maxX);
  double midY = 0.5 * (minY + maxY);
  double midZ = 0.5 * (minZ + maxZ);

  // First voxel node ray will intersect
  int currNode = getFirstIntersectedNode (minX, minY, minZ, midX, midY, midZ);

  // Child index, node and key
  unsigned char childIdx;
  const OctreeNode *childNode;
  OctreeKey childKey;

  do
  {
    if (currNode != 0)
      childIdx = static_cast<unsigned char> (currNode ^ a);
    else
      childIdx = a;

    // childNode == 0 if childNode doesn't exist
    childNode = this->getBranchChildPtr (static_cast<const BranchNode&> (*node), childIdx);

    // Generate new key for current branch voxel
    childKey.x = (key.x << 1) | (!!(childIdx & (1 << 2)));
    childKey.y = (key.y << 1) | (!!(childIdx & (1 << 1)));
    childKey.z = (key.z << 1) | (!!(childIdx & (1 << 0)));

    // Recursively call each intersected child node, selecting the next
    //   node intersected by the ray.  Children that do not intersect will
    //   not be traversed.

    switch (currNode)
    {
      case 0:
        if (childNode)
          voxelCount += getIntersectedVoxelCentersRecursive (minX, minY, minZ, midX, midY, midZ, a, childNode,
                                                             childKey, voxelCenterList, maxVoxelCount);
        currNode = getNextIntersectedNode (midX, midY, midZ, 4, 2, 1);
        break;

      case 1:
        if (childNode)
          voxelCount += getIntersectedVoxelCentersRecursive (minX, minY, midZ, midX, midY, maxZ, a, childNode,
                                                             childKey, voxelCenterList, maxVoxelCount);
        currNode = getNextIntersectedNode (midX, midY, maxZ, 5, 3, 8);
        break;

      case 2:
        if (childNode)
          voxelCount += getIntersectedVoxelCentersRecursive (minX, midY, minZ, midX, maxY, midZ, a, childNode,
                                                             childKey, voxelCenterList, maxVoxelCount);
        currNode = getNextIntersectedNode (midX, maxY, midZ, 6, 8, 3);
        break;

      case 3:
        if (childNode)
          voxelCount += getIntersectedVoxelCentersRecursive (minX, midY, midZ, midX, maxY, maxZ, a, childNode,
                                                             childKey, voxelCenterList, maxVoxelCount);
        currNode = getNextIntersectedNode (midX, maxY, maxZ, 7, 8, 8);
        break;

      case 4:
        if (childNode)
          voxelCount += getIntersectedVoxelCentersRecursive (midX, minY, minZ, maxX, midY, midZ, a, childNode,
                                                             childKey, voxelCenterList, maxVoxelCount);
        currNode = getNextIntersectedNode (maxX, midY, midZ, 8, 6, 5);
        break;

      case 5:
        if (childNode)
          voxelCount += getIntersectedVoxelCentersRecursive (midX, minY, midZ, maxX, midY, maxZ, a, childNode,
                                                             childKey, voxelCenterList, maxVoxelCount);
        currNode = getNextIntersectedNode (maxX, midY, maxZ, 8, 7, 8);
        break;

      case 6:
//.........这里部分代码省略.........

TEST (PCL, Outofcore_Octree_Build)
{

  boost::filesystem::remove_all (filename_otreeA.parent_path ());
  boost::filesystem::remove_all (filename_otreeB.parent_path ());

  Eigen::Vector3d min (-32.0, -32.0, -32.0);
  Eigen::Vector3d max (32.0, 32.0, 32.0);
  
  // Build two trees using each constructor
  // depth of treeA will be same as B because 1/2^3 > .1 and 1/2^4 < .1
  // depth really affects performance
  octree_disk treeA (min, max, .1, filename_otreeA, "ECEF");
  octree_disk treeB (4, min, max, filename_otreeB, "ECEF");

  // Equidistributed uniform pseudo-random number generator
  boost::mt19937 rng(rngseed);

  // For testing sparse 
  //boost::uniform_real<double> dist(0,1);

  // For testing less sparse
  boost::normal_distribution<float> dist (0.5f, .1f);

  // Create a point
  PointT p;
  points.resize (numPts);

  //ignore these fields from the UR point for now
  // p.r = p.g = p.b = 0;
  // p.nx = p.ny = p.nz = 1;
  // p.cameraCount = 0;
  // p.error = 0;
  // p.triadID = 0;

  // Radomize it's position in space
  for (size_t i = 0; i < numPts; i++)
  {
    p.x = dist (rng);
    p.y = dist (rng);
    p.z = dist (rng);

    points[i] = p;
  }

  // Add to tree
  treeA.addDataToLeaf (points);

  // Add to tree
  treeB.addDataToLeaf (points);

}