Пример #1
0
TEST (PCL, Octree_Pointcloud_Test)
{

  size_t i;
  int test_runs = 100;
  int pointcount = 300;

  int test, point;

  float resolution = 0.01f;

  // instantiate OctreePointCloudSinglePoint and OctreePointCloudPointVector classes
  OctreePointCloudSinglePoint<PointXYZ> octreeA (resolution);
  OctreePointCloudSearch<PointXYZ> octreeB (resolution);
  OctreePointCloudPointVector<PointXYZ> octreeC (resolution);

  // create shared pointcloud instances
  PointCloud<PointXYZ>::Ptr cloudA (new PointCloud<PointXYZ> ());
  PointCloud<PointXYZ>::Ptr cloudB (new PointCloud<PointXYZ> ());

  // assign input point clouds to octree
  octreeA.setInputCloud (cloudA);
  octreeB.setInputCloud (cloudB);
  octreeC.setInputCloud (cloudB);

  for (test = 0; test < test_runs; ++test)
  {

    // clean up
    cloudA->points.clear ();
    octreeA.deleteTree ();

    cloudB->points.clear ();
    octreeB.deleteTree ();

    octreeC.deleteTree ();

    for (point = 0; point < pointcount; point++)
    {
      // gereate a random point
      PointXYZ newPoint (1024.0 * (float)rand () / (float)RAND_MAX, 1024.0 * (float)rand () / (float)RAND_MAX,
                         1024.0 * (float)rand () / (float)RAND_MAX);

      if (!octreeA.isVoxelOccupiedAtPoint (newPoint))
      {
        // add point only to OctreePointCloudSinglePoint if voxel at point location doesn't exist
        octreeA.addPointToCloud (newPoint, cloudA);
      }

      // OctreePointCloudPointVector can store all points..
      cloudB->points.push_back (newPoint);
    }

    ASSERT_EQ(octreeA.getLeafCount(), cloudA->points.size());

    // checks for getVoxelDataAtPoint() and isVoxelOccupiedAtPoint() functionality
    for (i = 0; i < cloudA->points.size (); i++)
    {
      ASSERT_EQ(octreeA.isVoxelOccupiedAtPoint(cloudA->points[i]), true);
      octreeA.deleteVoxelAtPoint (cloudA->points[i]);
      ASSERT_EQ(octreeA.isVoxelOccupiedAtPoint(cloudA->points[i]), false);
    }

    ASSERT_EQ(octreeA.getLeafCount(), (unsigned int)0);

    // check if all points from leaf data can be found in input pointcloud data sets
    octreeB.defineBoundingBox ();
    octreeB.addPointsFromInputCloud ();

    //  test iterator
    OctreePointCloudSearch<PointXYZ>::Iterator b_it (octreeB);
    unsigned int node_count = 0;
    unsigned int branch_count = 0;
    unsigned int leaf_count = 0;

    double minx, miny, minz, maxx, maxy, maxz;
    octreeB.getBoundingBox (minx, miny, minz, maxx, maxy, maxz);

    // iterate over tree
    while (*++b_it)
    {
      // depth should always be less than tree depth
      unsigned int depth = b_it.getCurrentOctreeDepth ();
      ASSERT_LE(depth, octreeB.getTreeDepth());

      Eigen::Vector3f voxel_min, voxel_max;
      octreeB.getVoxelBounds (b_it, voxel_min, voxel_max);

      ASSERT_GE(voxel_min.x (), minx - 1e-4);
      ASSERT_GE(voxel_min.y (), miny - 1e-4);
      ASSERT_GE(voxel_min.z (), minz - 1e-4);

      ASSERT_LE(voxel_max.x (), maxx + 1e-4);
      ASSERT_LE(voxel_max.y (), maxy + 1e-4);
      ASSERT_LE(voxel_max.z (), maxz + 1e-4);

      // store node, branch and leaf count
      const OctreeNode* node = b_it.getCurrentOctreeNode ();
      if (node->getNodeType () == BRANCH_NODE)
      {
        branch_count++;
      }
      else if (node->getNodeType () == LEAF_NODE)
      {
        leaf_count++;
      }
      node_count++;
    }

    // compare node, branch and leaf count against actual tree values
    ASSERT_EQ(node_count, branch_count + leaf_count);
    ASSERT_EQ(leaf_count, octreeB.getLeafCount ());

    for (i = 0; i < cloudB->points.size (); i++)
    {

      std::vector<int> pointIdxVec;
      octreeB.voxelSearch (cloudB->points[i], pointIdxVec);

      bool bIdxFound = false;
      std::vector<int>::const_iterator current = pointIdxVec.begin ();
      while (current != pointIdxVec.end ())
      {
        if (*current == (int)i)
        {
          bIdxFound = true;
          break;
        }
        ++current;
      }

      ASSERT_EQ(bIdxFound, true);

    }

    // test octree pointcloud serialization

    std::vector<char> treeBinaryB;
    std::vector<char> treeBinaryC;

    std::vector<int> leafVectorB;
    std::vector<int> leafVectorC;

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

    // serialize octreeB
    octreeB.serializeTree (treeBinaryB, leafVectorB);
    octreeB.getBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);

    ASSERT_EQ(cloudB->points.size(), leafVectorB.size());

    // deserialize into octreeC
    octreeC.deleteTree ();
    octreeC.defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);

    octreeC.deserializeTree (treeBinaryB, leafVectorB);

    // retrieve point data content of octreeC
    octreeC.serializeTree (treeBinaryC, leafVectorC);

    // check if data is consistent
    ASSERT_EQ(octreeB.getLeafCount(), octreeC.getLeafCount());
    ASSERT_EQ(treeBinaryB.size(), treeBinaryC.size());
    ASSERT_EQ(octreeB.getLeafCount(), cloudB->points.size());

    for (i = 0; i < leafVectorB.size (); ++i)
    {
      ASSERT_EQ(leafVectorB[i], leafVectorC[i]);
    }

    // check if binary octree output of both trees is equal
    for (i = 0; i < treeBinaryB.size (); ++i)
    {
      ASSERT_EQ(treeBinaryB[i], treeBinaryC[i]);
    }

  }

}
Пример #2
0
TEST (PCL, Octree_Pointcloud_Test)
{

  size_t i;
  int test_runs = 100;
  int pointcount = 300;

  int test, point;

  float resolution = 0.01f;

  // instantiate OctreePointCloudSinglePoint and OctreePointCloudPointVector classes
  OctreePointCloudSinglePoint<PointXYZ> octreeA (resolution);
  OctreePointCloudPointVector<PointXYZ> octreeB (resolution);
  OctreePointCloudPointVector<PointXYZ> octreeC (resolution);

  // create shared pointcloud instances
  PointCloud<PointXYZ>::Ptr cloudA (new PointCloud<PointXYZ> ());
  PointCloud<PointXYZ>::Ptr cloudB (new PointCloud<PointXYZ> ());

  // assign input point clouds to octree
  octreeA.setInputCloud (cloudA);
  octreeB.setInputCloud (cloudB);
  octreeC.setInputCloud (cloudB);

  for (test = 0; test < test_runs; ++test)
  {

    // clean up
    cloudA->points.clear ();
    octreeA.deleteTree ();

    cloudB->points.clear ();
    octreeB.deleteTree ();

    octreeC.deleteTree ();

    for (point = 0; point < pointcount; point++)
    {
      // gereate a random point
      PointXYZ newPoint (1024.0 * (float)rand () / (float)RAND_MAX, 1024.0 * (float)rand () / (float)RAND_MAX,
                         1024.0 * (float)rand () / (float)RAND_MAX);

      if (!octreeA.isVoxelOccupiedAtPoint (newPoint))
      {
        // add point only to OctreePointCloudSinglePoint if voxel at point location doesn't exist
        octreeA.addPointToCloud (newPoint, cloudA);
      }

      // OctreePointCloudPointVector can store all points..
      cloudB->points.push_back (newPoint);
    }

    ASSERT_EQ (octreeA.getLeafCount(), cloudA->points.size());

    // checks for getVoxelDataAtPoint() and isVoxelOccupiedAtPoint() functionality
    for (i = 0; i < cloudA->points.size (); i++)
    {
      ASSERT_EQ (octreeA.isVoxelOccupiedAtPoint(cloudA->points[i]), true);
      octreeA.deleteVoxelAtPoint(cloudA->points[i]);
      ASSERT_EQ (octreeA.isVoxelOccupiedAtPoint(cloudA->points[i]), false);
    }

    ASSERT_EQ (octreeA.getLeafCount(), 0);

    // check if all points from leaf data can be found in input pointcloud data sets
    octreeB.defineBoundingBox();
    octreeB.addPointsFromInputCloud();

    for (i = 0; i < cloudB->points.size (); i++)
    {

      std::vector<int> pointIdxVec;
      octreeB.voxelSearch(cloudB->points[i], pointIdxVec);

      bool bIdxFound = false;
      std::vector<int>::const_iterator current = pointIdxVec.begin();
      while (current != pointIdxVec.end())
      {
        if (*current == (int)i)
        {
          bIdxFound = true;
          break;
        }
        ++current;
      }

      ASSERT_EQ (bIdxFound, true);

    }

    // test octree pointcloud serialization

    std::vector<char> treeBinaryB;
    std::vector<char> treeBinaryC;

    std::vector<int> leafVectorB;
    std::vector<int> leafVectorC;

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

    // serialize octreeB
    octreeB.serializeTree(treeBinaryB, leafVectorB);
    octreeB.getBoundingBox ( minX, minY,minZ, maxX, maxY, maxZ );

    ASSERT_EQ (cloudB->points.size(), leafVectorB.size());

    // deserialize into octreeC
    octreeC.deleteTree();
    octreeC.defineBoundingBox ( minX, minY,minZ, maxX, maxY, maxZ );

    octreeC.deserializeTree(treeBinaryB, leafVectorB);

    // retrieve point data content of octreeC
    octreeC.serializeTree(treeBinaryC, leafVectorC);

    // check if data is consistent
    ASSERT_EQ (octreeB.getLeafCount(), octreeC.getLeafCount());
    ASSERT_EQ (treeBinaryB.size(), treeBinaryC.size() );
    ASSERT_EQ (octreeB.getLeafCount(), cloudB->points.size());

    for (i = 0; i < leafVectorB.size(); ++i)
    {
      ASSERT_EQ (leafVectorB[i], leafVectorC[i]);
    }

    // check if binary octree output of both trees is equal
    for (i = 0; i < treeBinaryB.size(); ++i)
    {
      ASSERT_EQ (treeBinaryB[i], treeBinaryC[i]);
    }

  }

}
Пример #3
0
TEST (PCL, Octree_Pointcloud_Test)
{
  size_t i;
  int test_runs = 100;
  int pointcount = 300;

  int test, point;

  float resolution = 0.01f;

  // instantiate OctreePointCloudSinglePoint and OctreePointCloudPointVector classes
  OctreePointCloudSinglePoint<PointXYZ> octreeA (resolution);
  OctreePointCloudSearch<PointXYZ> octreeB (resolution);
  OctreePointCloudPointVector<PointXYZ> octreeC (resolution);

  // create shared pointcloud instances
  PointCloud<PointXYZ>::Ptr cloudA (new PointCloud<PointXYZ> ());
  PointCloud<PointXYZ>::Ptr cloudB (new PointCloud<PointXYZ> ());

  // assign input point clouds to octree
  octreeA.setInputCloud (cloudA);
  octreeB.setInputCloud (cloudB);
  octreeC.setInputCloud (cloudB);

  for (test = 0; test < test_runs; ++test)
  {

    // clean up
    cloudA->points.clear ();
    octreeA.deleteTree ();

    cloudB->points.clear ();
    octreeB.deleteTree ();

    octreeC.deleteTree ();

    for (point = 0; point < pointcount; point++)
    {
      // gereate a random point
      PointXYZ newPoint (static_cast<float> (1024.0 * rand () / RAND_MAX),
                         static_cast<float> (1024.0 * rand () / RAND_MAX),
                         static_cast<float> (1024.0 * rand () / RAND_MAX));

      if (!octreeA.isVoxelOccupiedAtPoint (newPoint))
      {
        // add point only to OctreePointCloudSinglePoint if voxel at point location doesn't exist
        octreeA.addPointToCloud (newPoint, cloudA);
      }

      // OctreePointCloudPointVector can store all points..
      cloudB->push_back (newPoint);
    }

    ASSERT_EQ (cloudA->points.size (), octreeA.getLeafCount ());

    // checks for getVoxelDataAtPoint() and isVoxelOccupiedAtPoint() functionality
    for (i = 0; i < cloudA->points.size (); i++)
    {
      ASSERT_TRUE (octreeA.isVoxelOccupiedAtPoint (cloudA->points[i]));
      octreeA.deleteVoxelAtPoint (cloudA->points[i]);
      ASSERT_FALSE (octreeA.isVoxelOccupiedAtPoint (cloudA->points[i]));
    }

    ASSERT_EQ (static_cast<unsigned int> (0), octreeA.getLeafCount ());

    // check if all points from leaf data can be found in input pointcloud data sets
    octreeB.defineBoundingBox ();
    octreeB.addPointsFromInputCloud ();

    //  test iterator
    OctreePointCloudPointVector<PointXYZ>::Iterator b_it;
    OctreePointCloudPointVector<PointXYZ>::Iterator b_it_end = octreeB.end();

    // iterate over tree

    unsigned int node_count = 0;
    unsigned int branch_count = 0;
    unsigned int leaf_count = 0;

    double minx, miny, minz, maxx, maxy, maxz;
    octreeB.getBoundingBox (minx, miny, minz, maxx, maxy, maxz);

    // iterate over tree
    for (b_it = octreeB.begin(); b_it != b_it_end; ++b_it)
    {
      // depth should always be less than tree depth
      unsigned int depth = b_it.getCurrentOctreeDepth ();
      ASSERT_LE (depth, octreeB.getTreeDepth ());

      Eigen::Vector3f voxel_min, voxel_max;
      octreeB.getVoxelBounds (b_it, voxel_min, voxel_max);

      ASSERT_GE (voxel_min.x (), minx - 1e-4);
      ASSERT_GE (voxel_min.y (), miny - 1e-4);
      ASSERT_GE (voxel_min.z (), minz - 1e-4);

      ASSERT_LE (voxel_max.x (), maxx + 1e-4);
      ASSERT_LE (voxel_max.y (), maxy + 1e-4);
      ASSERT_LE (voxel_max.z (), maxz + 1e-4);

      // store node, branch and leaf count
      const OctreeNode* node = b_it.getCurrentOctreeNode ();
      if (node->getNodeType () == BRANCH_NODE)
      {
        branch_count++;
      }
      else if (node->getNodeType () == LEAF_NODE)
      {
        leaf_count++;
      }
      node_count++;
    }

    // compare node, branch and leaf count against actual tree values
    ASSERT_EQ (branch_count + leaf_count, node_count);
    ASSERT_EQ (octreeB.getLeafCount (), leaf_count);

    for (i = 0; i < cloudB->points.size (); i++)
    {
      std::vector<int> pointIdxVec;
      octreeB.voxelSearch (cloudB->points[i], pointIdxVec);

      bool bIdxFound = false;
      std::vector<int>::const_iterator current = pointIdxVec.begin ();
      while (current != pointIdxVec.end ())
      {
        if (*current == static_cast<int> (i))
        {
          bIdxFound = true;
          break;
        }
        ++current;
      }

      ASSERT_TRUE (bIdxFound);
    }
  }
}