diff --git a/test/octree/test_octree.cpp b/test/octree/test_octree.cpp index 8905d88be78..2c7b9fa5525 100644 --- a/test/octree/test_octree.cpp +++ b/test/octree/test_octree.cpp @@ -1,7 +1,10 @@ /* * Software License Agreement (BSD License) * + * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2017-, Open Perception, Inc. + * * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -96,19 +99,19 @@ TEST (PCL, Octree_Test) // retrieve data from leaf node voxel int* voxel_container = octreeA.createLeaf(voxels[i].x, voxels[i].y, voxels[i].z); // check if retrieved data is identical to data[] - ASSERT_EQ(*voxel_container, data[i]); + ASSERT_EQ (data[i], *voxel_container); } for (i = 128; i < 256; i++) { // check if leaf node exists in tree - ASSERT_EQ( octreeA.existLeaf(voxels[i].x,voxels[i].y,voxels[i].z), true); + ASSERT_TRUE (octreeA.existLeaf (voxels[i].x, voxels[i].y, voxels[i].z)); // remove leaf node octreeA.removeLeaf (voxels[i].x, voxels[i].y, voxels[i].z); // leaf node shouldn't exist in tree anymore - ASSERT_EQ( octreeA.existLeaf(voxels[i].x,voxels[i].y,voxels[i].z), false); + ASSERT_FALSE (octreeA.existLeaf (voxels[i].x, voxels[i].y, voxels[i].z)); } // test serialization @@ -128,33 +131,33 @@ TEST (PCL, Octree_Test) for (i = 0; i < 128; i++) { // check if leafs exist in both octrees - ASSERT_EQ( octreeA.existLeaf(voxels[i].x,voxels[i].y,voxels[i].z), true); - ASSERT_EQ( octreeB.existLeaf(voxels[i].x,voxels[i].y,voxels[i].z), true); + ASSERT_TRUE (octreeA.existLeaf (voxels[i].x, voxels[i].y, voxels[i].z)); + ASSERT_TRUE (octreeB.existLeaf (voxels[i].x, voxels[i].y, voxels[i].z)); } for (i = 128; i < 256; i++) { // these leafs were not copies and should not exist - ASSERT_EQ( octreeB.existLeaf(voxels[i].x,voxels[i].y,voxels[i].z), false); + ASSERT_FALSE (octreeB.existLeaf (voxels[i].x, voxels[i].y, voxels[i].z)); } // testing deleteTree(); octreeB.deleteTree (); // octreeB.getLeafCount() should be zero now; - ASSERT_EQ (0u, octreeB.getLeafCount()); + ASSERT_EQ (0u, octreeB.getLeafCount ()); // .. and previous leafs deleted.. for (i = 0; i < 128; i++) { - ASSERT_EQ(octreeB.existLeaf(voxels[i].x,voxels[i].y,voxels[i].z), false); + ASSERT_FALSE (octreeB.existLeaf (voxels[i].x, voxels[i].y, voxels[i].z)); } // test tree serialization octreeA.serializeTree (treeBinaryA, leafVectorA); // make sure, we retrieved all data objects - ASSERT_EQ(leafVectorA.size(), octreeA.getLeafCount()); + ASSERT_EQ (octreeA.getLeafCount (), leafVectorA.size ()); // check if leaf data is found in octree input data bool bFound; @@ -171,7 +174,7 @@ TEST (PCL, Octree_Test) break; } - ASSERT_EQ(bFound, true); + ASSERT_TRUE (bFound); } // test tree serialization @@ -190,7 +193,7 @@ TEST (PCL, Octree_Test) break; } - ASSERT_EQ(bFound, true); + ASSERT_TRUE (bFound); } // test tree serialization with leaf data vectors @@ -198,18 +201,18 @@ TEST (PCL, Octree_Test) octreeB.deserializeTree (treeBinaryA, leafVectorA); // test size and leaf count of reconstructed octree - ASSERT_EQ(octreeA.getLeafCount(), octreeB.getLeafCount()); - ASSERT_EQ(128u, octreeB.getLeafCount()); + ASSERT_EQ (octreeA.getLeafCount (), octreeB.getLeafCount ()); + ASSERT_EQ (128u, octreeB.getLeafCount ()); octreeB.serializeTree (treeBinaryB, leafVectorB); // compare octree data content of octree A and octree B - ASSERT_EQ(leafVectorB.size(), octreeB.getLeafCount()); - ASSERT_EQ(leafVectorA.size(), leafVectorB.size()); + ASSERT_EQ (leafVectorB.size (), octreeB.getLeafCount ()); + ASSERT_EQ (leafVectorA.size (), leafVectorB.size ()); for (i = 0; i < leafVectorB.size (); i++) { - ASSERT_EQ( (*leafVectorA[i] == *leafVectorB[i]), true); + ASSERT_EQ (*leafVectorA[i], *leafVectorB[i]); } // test iterator @@ -226,7 +229,7 @@ TEST (PCL, Octree_Test) { // depth should always be less than tree depth unsigned int depth = a_it.getCurrentOctreeDepth (); - ASSERT_LE(depth, octreeA.getTreeDepth()); + ASSERT_LE (depth, octreeA.getTreeDepth ()); // store node, branch and leaf count const OctreeNode* node = a_it.getCurrentOctreeNode (); @@ -242,9 +245,8 @@ TEST (PCL, Octree_Test) } // compare node, branch and leaf count against actual tree values - ASSERT_EQ(node_count, branch_count + leaf_count); - ASSERT_EQ(leaf_count, octreeA.getLeafCount ()); - + ASSERT_EQ (branch_count + leaf_count, node_count); + ASSERT_EQ (octreeA.getLeafCount (), leaf_count); } TEST (PCL, Octree_Dynamic_Depth_Test) @@ -299,7 +301,7 @@ TEST (PCL, Octree_Dynamic_Depth_Test) { ++leaf_node_counter; unsigned int depth = it2.getCurrentOctreeDepth (); - ASSERT_EQ((depth==1)||(depth==6), true); + ASSERT_TRUE ((depth == 1) || (depth == 6)); } // clean up @@ -336,11 +338,11 @@ TEST (PCL, Octree_Dynamic_Depth_Test) OctreeNode* node = it.getCurrentOctreeNode (); - ASSERT_EQ(node->getNodeType(), LEAF_NODE); + ASSERT_EQ (LEAF_NODE, node->getNodeType()); OctreeContainerPointIndices& container = it.getLeafContainer(); if (it.getCurrentOctreeDepth () < octree.getTreeDepth ()) - ASSERT_LE(container.getSize(), leafAggSize); + ASSERT_LE (container.getSize (), leafAggSize); // add points from leaf node to indexVector container.getPointIndices (indexVector); @@ -354,31 +356,31 @@ TEST (PCL, Octree_Dynamic_Depth_Test) for (i=0; ipoints[tmpVector[i]].x, min_pt(0)); - ASSERT_GE(cloud->points[tmpVector[i]].y, min_pt(1)); - ASSERT_GE(cloud->points[tmpVector[i]].z, min_pt(2)); - ASSERT_LE(cloud->points[tmpVector[i]].x, max_pt(0)); - ASSERT_LE(cloud->points[tmpVector[i]].y, max_pt(1)); - ASSERT_LE(cloud->points[tmpVector[i]].z, max_pt(2)); + ASSERT_GE (cloud->points[tmpVector[i]].x, min_pt(0)); + ASSERT_GE (cloud->points[tmpVector[i]].y, min_pt(1)); + ASSERT_GE (cloud->points[tmpVector[i]].z, min_pt(2)); + ASSERT_LE (cloud->points[tmpVector[i]].x, max_pt(0)); + ASSERT_LE (cloud->points[tmpVector[i]].y, max_pt(1)); + ASSERT_LE (cloud->points[tmpVector[i]].z, max_pt(2)); } leaf_count++; } - ASSERT_EQ(pointcount, indexVector.size()); + + ASSERT_EQ (indexVector.size(), pointcount); // make sure all indices are within indexVector for (i = 0; i < indexVector.size (); ++i) { #if !defined(__APPLE__) bool indexFound = (std::find(indexVector.begin(), indexVector.end(), i) != indexVector.end()); - ASSERT_EQ(indexFound, true); + ASSERT_TRUE (indexFound); #endif } // compare node, branch and leaf count against actual tree values - ASSERT_EQ(leaf_count, octree.getLeafCount ()); - + ASSERT_EQ (octree.getLeafCount (), leaf_count); } } @@ -421,25 +423,25 @@ TEST (PCL, Octree2Buf_Test) } - ASSERT_EQ(static_cast (256), octreeA.getLeafCount()); + ASSERT_EQ (static_cast (256), octreeA.getLeafCount ()); for (i = 0; i < 128; i++) { // retrieve and check data from leaf voxel int* voxel_container = octreeA.findLeaf(voxels[i].x, voxels[i].y, voxels[i].z); - ASSERT_EQ(*voxel_container, data[i]); + ASSERT_EQ (data[i], *voxel_container); } for (i = 128; i < 256; i++) { // check if leaf node exists in tree - ASSERT_EQ( octreeA.existLeaf(voxels[i].x,voxels[i].y,voxels[i].z), true); + ASSERT_TRUE (octreeA.existLeaf (voxels[i].x, voxels[i].y, voxels[i].z)); // remove leaf node octreeA.removeLeaf (voxels[i].x, voxels[i].y, voxels[i].z); // leaf node shouldn't exist in tree anymore - ASSERT_EQ( octreeA.existLeaf(voxels[i].x,voxels[i].y,voxels[i].z), false); + ASSERT_FALSE (octreeA.existLeaf (voxels[i].x, voxels[i].y, voxels[i].z)); } //////////// @@ -461,13 +463,13 @@ TEST (PCL, Octree2Buf_Test) // check if leafs exist in octrees for (i = 0; i < 128; i++) { - ASSERT_EQ( octreeB.existLeaf(voxels[i].x,voxels[i].y,voxels[i].z), true); + ASSERT_TRUE (octreeB.existLeaf (voxels[i].x, voxels[i].y, voxels[i].z)); } // these leafs should not exist.. for (i = 128; i < 256; i++) { - ASSERT_EQ( octreeB.existLeaf(voxels[i].x,voxels[i].y,voxels[i].z), false); + ASSERT_FALSE (octreeB.existLeaf (voxels[i].x, voxels[i].y, voxels[i].z)); } // checking deleteTree(); @@ -475,18 +477,18 @@ TEST (PCL, Octree2Buf_Test) octreeB.setTreeDepth (8); // octreeB.getLeafCount() should be zero now; - ASSERT_EQ(static_cast (0), octreeB.getLeafCount()); + ASSERT_EQ (static_cast (0), octreeB.getLeafCount ()); for (i = 0; i < 128; i++) { - ASSERT_EQ( octreeB.existLeaf(voxels[i].x,voxels[i].y,voxels[i].z), false); + ASSERT_FALSE (octreeB.existLeaf (voxels[i].x, voxels[i].y, voxels[i].z)); } // test tree serialization octreeA.serializeTree (treeBinaryA, leafVectorA); // make sure, we retrieved all data objects - ASSERT_EQ(leafVectorA.size(), octreeA.getLeafCount()); + ASSERT_EQ (octreeA.getLeafCount (), leafVectorA.size ()); // check if leaf data is found in octree input data bool bFound; @@ -503,7 +505,7 @@ TEST (PCL, Octree2Buf_Test) break; } - ASSERT_EQ(bFound, true); + ASSERT_TRUE (bFound); } // test tree serialization @@ -522,30 +524,28 @@ TEST (PCL, Octree2Buf_Test) break; } - ASSERT_EQ(bFound, true); + ASSERT_TRUE (bFound); } // test tree serialization with leaf data vectors octreeA.serializeTree (treeBinaryA, leafVectorA); octreeB.deserializeTree (treeBinaryA, leafVectorA); - ASSERT_EQ(octreeA.getLeafCount(), octreeB.getLeafCount()); - ASSERT_EQ(static_cast (128), octreeB.getLeafCount()); + ASSERT_EQ (octreeA.getLeafCount (), octreeB.getLeafCount ()); + ASSERT_EQ (static_cast (128), octreeB.getLeafCount ()); octreeB.serializeTree (treeBinaryB, leafVectorB); // test size and leaf count of reconstructed octree - ASSERT_EQ(leafVectorB.size(), octreeB.getLeafCount()); - ASSERT_EQ(leafVectorA.size(), leafVectorB.size()); + ASSERT_EQ (leafVectorB.size (), octreeB.getLeafCount ()); + ASSERT_EQ (leafVectorA.size (), leafVectorB.size ()); for (i = 0; i < leafVectorB.size (); i++) { - ASSERT_EQ( (*leafVectorA[i] == *leafVectorB[i]), true); + ASSERT_EQ (*leafVectorA[i], *leafVectorB[i]); } - } - TEST (PCL, Octree2Buf_Base_Double_Buffering_Test) { @@ -616,14 +616,14 @@ TEST (PCL, Octree2Buf_Base_Double_Buffering_Test) octreeB.serializeTree (treeBinaryB, leafVectorB, true); // check leaf count of rebuilt octree - ASSERT_EQ(octreeA.getLeafCount(), octreeB.getLeafCount()); - ASSERT_EQ(leafVectorB.size(), octreeB.getLeafCount()); - ASSERT_EQ(leafVectorA.size(), leafVectorB.size()); + ASSERT_EQ (octreeA.getLeafCount (), octreeB.getLeafCount ()); + ASSERT_EQ (leafVectorB.size (), octreeB.getLeafCount ()); + ASSERT_EQ (leafVectorA.size (), leafVectorB.size ()); // check if octree octree structure is consistent. for (i = 0; i < leafVectorB.size (); i++) { - ASSERT_EQ( (*leafVectorA[i] == *leafVectorB[i]), true); + ASSERT_EQ (*leafVectorA[i], *leafVectorB[i]); } } @@ -685,29 +685,26 @@ TEST (PCL, Octree2Buf_Base_Double_Buffering_XOR_Test) octreeB.serializeTree (treeBinaryB, leafVectorB, true); // check leaf count of rebuilt octree - ASSERT_EQ(octreeA.getLeafCount(), octreeB.getLeafCount()); - ASSERT_EQ(leafVectorB.size(), octreeB.getLeafCount()); - ASSERT_EQ(leafVectorA.size(), leafVectorB.size()); - ASSERT_EQ(treeBinaryA.size(), octreeB.getBranchCount()); - ASSERT_EQ(treeBinaryA.size(), treeBinaryB.size()); + ASSERT_EQ (octreeA.getLeafCount (), octreeB.getLeafCount ()); + ASSERT_EQ (leafVectorB.size (), octreeB.getLeafCount ()); + ASSERT_EQ (leafVectorA.size (), leafVectorB.size ()); + ASSERT_EQ (treeBinaryA.size (), octreeB.getBranchCount ()); + ASSERT_EQ (treeBinaryA.size (), treeBinaryB.size ()); // check if octree octree structure is consistent. for (i = 0; i < leafVectorB.size (); i++) { - ASSERT_EQ( (*leafVectorA[i] == *leafVectorB[i]), true); + ASSERT_EQ (*leafVectorA[i], *leafVectorB[i]); } // switch buffers octreeA.switchBuffers (); octreeB.switchBuffers (); - } - } TEST (PCL, Octree_Pointcloud_Test) { - size_t i; int test_runs = 100; int pointcount = 300; @@ -745,8 +742,8 @@ TEST (PCL, Octree_Pointcloud_Test) for (point = 0; point < pointcount; point++) { // gereate a random point - PointXYZ newPoint (static_cast (1024.0 * rand () / RAND_MAX), - static_cast (1024.0 * rand () / RAND_MAX), + PointXYZ newPoint (static_cast (1024.0 * rand () / RAND_MAX), + static_cast (1024.0 * rand () / RAND_MAX), static_cast (1024.0 * rand () / RAND_MAX)); if (!octreeA.isVoxelOccupiedAtPoint (newPoint)) @@ -759,17 +756,17 @@ TEST (PCL, Octree_Pointcloud_Test) cloudB->push_back (newPoint); } - ASSERT_EQ(octreeA.getLeafCount(), cloudA->points.size()); + ASSERT_EQ (cloudA->points.size (), octreeA.getLeafCount ()); // checks for getVoxelDataAtPoint() and isVoxelOccupiedAtPoint() functionality for (i = 0; i < cloudA->points.size (); i++) { - ASSERT_EQ(octreeA.isVoxelOccupiedAtPoint(cloudA->points[i]), true); + ASSERT_TRUE (octreeA.isVoxelOccupiedAtPoint (cloudA->points[i])); octreeA.deleteVoxelAtPoint (cloudA->points[i]); - ASSERT_EQ(octreeA.isVoxelOccupiedAtPoint(cloudA->points[i]), false); + ASSERT_FALSE (octreeA.isVoxelOccupiedAtPoint (cloudA->points[i])); } - ASSERT_EQ(octreeA.getLeafCount(), static_cast (0)); + ASSERT_EQ (static_cast (0), octreeA.getLeafCount ()); // check if all points from leaf data can be found in input pointcloud data sets octreeB.defineBoundingBox (); @@ -793,18 +790,18 @@ TEST (PCL, Octree_Pointcloud_Test) { // depth should always be less than tree depth unsigned int depth = b_it.getCurrentOctreeDepth (); - ASSERT_LE(depth, octreeB.getTreeDepth()); + 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_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); + 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 (); @@ -820,12 +817,11 @@ TEST (PCL, Octree_Pointcloud_Test) } // compare node, branch and leaf count against actual tree values - ASSERT_EQ(node_count, branch_count + leaf_count); - ASSERT_EQ(leaf_count, octreeB.getLeafCount ()); + ASSERT_EQ (branch_count + leaf_count, node_count); + ASSERT_EQ (octreeB.getLeafCount (), leaf_count); for (i = 0; i < cloudB->points.size (); i++) { - std::vector pointIdxVec; octreeB.voxelSearch (cloudB->points[i], pointIdxVec); @@ -841,17 +837,13 @@ TEST (PCL, Octree_Pointcloud_Test) ++current; } - ASSERT_EQ(bIdxFound, true); - + ASSERT_TRUE (bIdxFound); } - } - } TEST (PCL, Octree_Pointcloud_Density_Test) { - // instantiate point cloud and fill it with point data PointCloud::Ptr cloudIn (new PointCloud ()); @@ -881,12 +873,12 @@ TEST (PCL, Octree_Pointcloud_Density_Test) for (float z = 1.5f; z < 3.5f; z += 1.0f) for (float y = 1.5f; y < 3.5f; y += 1.0f) for (float x = 1.5f; x < 3.5f; x += 1.0f) - ASSERT_EQ(octreeA.getVoxelDensityAtPoint (PointXYZ(x, y, z)), 1000u); + ASSERT_EQ (1000u, octreeA.getVoxelDensityAtPoint (PointXYZ(x, y, z))); for (float z = 0.05f; z < 5.0f; z += 0.1f) for (float y = 0.05f; y < 5.0f; y += 0.1f) for (float x = 0.05f; x < 5.0f; x += 0.1f) - ASSERT_EQ(octreeB.getVoxelDensityAtPoint (PointXYZ(x, y, z)), 1u); + ASSERT_EQ (1u, octreeB.getVoxelDensityAtPoint (PointXYZ (x, y, z))); } TEST (PCL, Octree_Pointcloud_Iterator_Test) @@ -922,8 +914,8 @@ TEST (PCL, Octree_Pointcloud_Iterator_Test) leafNodeCounter++; } - ASSERT_EQ(indexVector.size(), cloudIn->points.size ()); - ASSERT_EQ(leafNodeCounter, octreeA.getLeafCount()); + ASSERT_EQ (cloudIn->points.size (), indexVector.size()); + ASSERT_EQ (octreeA.getLeafCount (), leafNodeCounter); OctreePointCloud::Iterator it2; OctreePointCloud::Iterator it2_end = octreeA.end(); @@ -934,7 +926,7 @@ TEST (PCL, Octree_Pointcloud_Iterator_Test) traversCounter++; } - ASSERT_EQ(octreeA.getLeafCount() + octreeA.getBranchCount(), traversCounter); + ASSERT_EQ (octreeA.getLeafCount () + octreeA.getBranchCount (), traversCounter); // breadth-first iterator test @@ -950,14 +942,14 @@ TEST (PCL, Octree_Pointcloud_Iterator_Test) for (bfIt = octreeA.breadth_begin(); bfIt != bfIt_end; ++bfIt) { // tree depth of visited nodes must grow - ASSERT_EQ( bfIt.getCurrentOctreeDepth()>=lastDepth, true); + ASSERT_TRUE (bfIt.getCurrentOctreeDepth () >= lastDepth); lastDepth = bfIt.getCurrentOctreeDepth (); if (bfIt.isBranchNode ()) { branchNodeCount++; // leaf nodes are traversed in the end - ASSERT_EQ( leafNodeVisited, false); + ASSERT_FALSE (leafNodeVisited); } if (bfIt.isLeafNode ()) @@ -968,9 +960,8 @@ TEST (PCL, Octree_Pointcloud_Iterator_Test) } // check if every branch node and every leaf node has been visited - ASSERT_EQ( leafNodeCount, octreeA.getLeafCount()); - ASSERT_EQ( branchNodeCount, octreeA.getBranchCount()); - + ASSERT_EQ (octreeA.getLeafCount (), leafNodeCount); + ASSERT_EQ (octreeA.getBranchCount (), branchNodeCount); } TEST(PCL, Octree_Pointcloud_Occupancy_Test) @@ -1010,13 +1001,11 @@ TEST(PCL, Octree_Pointcloud_Occupancy_Test) // check occupancy of voxels for (i = 0; i < 1000; i++) { - ASSERT_EQ(octree.isVoxelOccupiedAtPoint(cloudIn->points[i]), true); + ASSERT_TRUE (octree.isVoxelOccupiedAtPoint (cloudIn->points[i])); octree.deleteVoxelAtPoint (cloudIn->points[i]); - ASSERT_EQ(octree.isVoxelOccupiedAtPoint(cloudIn->points[i]), false); + ASSERT_FALSE (octree.isVoxelOccupiedAtPoint (cloudIn->points[i])); } - } - } TEST (PCL, Octree_Pointcloud_Change_Detector_Test) @@ -1059,7 +1048,7 @@ TEST (PCL, Octree_Pointcloud_Change_Detector_Test) for (i = 0; i < 1000; i++) { octree.addPointToCloud ( - PointXYZ (static_cast (100.0 + 5.0 * rand () / RAND_MAX), + PointXYZ (static_cast (100.0 + 5.0 * rand () / RAND_MAX), static_cast (100.0 + 10.0 * rand () / RAND_MAX), static_cast (100.0 + 10.0 * rand () / RAND_MAX)), cloudIn); @@ -1071,12 +1060,12 @@ TEST (PCL, Octree_Pointcloud_Change_Detector_Test) octree.getPointIndicesFromNewVoxels (newPointIdxVector); // should be 1000 - ASSERT_EQ( newPointIdxVector.size(), static_cast (1000)); + ASSERT_EQ (static_cast (1000), newPointIdxVector.size ()); // all point indices found should have an index of >= 1000 for (i = 0; i < 1000; i++) { - ASSERT_EQ( ( newPointIdxVector [i] >= 1000 ), true); + ASSERT_TRUE (newPointIdxVector[i] >= 1000); } } @@ -1118,14 +1107,14 @@ TEST (PCL, Octree_Pointcloud_Voxel_Centroid_Test) octree.getVoxelCentroids (voxelCentroids); // we expect 10 voxel centroids - ASSERT_EQ (voxelCentroids.size(), static_cast (10)); + ASSERT_EQ (static_cast (10), voxelCentroids.size()); // check centroid calculation for (i = 0; i < 10; i++) { - EXPECT_NEAR(voxelCentroids[i].x, static_cast (i)+0.4, 1e-4); - EXPECT_NEAR(voxelCentroids[i].y, static_cast (i)+0.4, 1e-4); - EXPECT_NEAR(voxelCentroids[i].z, static_cast (i)+0.4, 1e-4); + EXPECT_NEAR (voxelCentroids[i].x, static_cast (i) + 0.4, 1e-4); + EXPECT_NEAR (voxelCentroids[i].y, static_cast (i) + 0.4, 1e-4); + EXPECT_NEAR (voxelCentroids[i].z, static_cast (i) + 0.4, 1e-4); } // ADDING AN ADDITIONAL POINT CLOUD TO OctreePointCloudVoxelCentroid @@ -1149,9 +1138,9 @@ TEST (PCL, Octree_Pointcloud_Voxel_Centroid_Test) // check centroid calculation for (i = 0; i < 10; i++) { - EXPECT_NEAR(voxelCentroids[i].x, static_cast (i)+0.4, 1e-4); - EXPECT_NEAR(voxelCentroids[i].y, static_cast (i)+0.4, 1e-4); - EXPECT_NEAR(voxelCentroids[i].z, static_cast (i)+0.4, 1e-4); + EXPECT_NEAR (voxelCentroids[i].x, static_cast (i) + 0.4, 1e-4); + EXPECT_NEAR (voxelCentroids[i].y, static_cast (i) + 0.4, 1e-4); + EXPECT_NEAR (voxelCentroids[i].z, static_cast (i) + 0.4, 1e-4); } } @@ -1213,7 +1202,7 @@ TEST (PCL, Octree_Pointcloud_Nearest_K_Neighbour_Search) { // define a random search point - PointXYZ searchPoint (static_cast (10.0 * rand () / RAND_MAX), + PointXYZ searchPoint (static_cast (10.0 * rand () / RAND_MAX), static_cast (10.0 * rand () / RAND_MAX), static_cast (10.0 * rand () / RAND_MAX)); @@ -1266,31 +1255,28 @@ TEST (PCL, Octree_Pointcloud_Nearest_K_Neighbour_Search) pointCandidates.pop (); } - + // octree nearest neighbor search octree.deleteTree (); octree.addPointsFromInputCloud (); octree.nearestKSearch (searchPoint, static_cast (K), k_indices, k_sqr_distances); - ASSERT_EQ( k_indices.size(), k_indices_bruteforce.size()); + ASSERT_EQ (k_indices_bruteforce.size (), k_indices.size()); // compare nearest neighbor results of octree with bruteforce search i = 0; while (k_indices_bruteforce.size ()) { - ASSERT_EQ( k_indices.back(), k_indices_bruteforce.back()); - EXPECT_NEAR(k_sqr_distances.back(), k_sqr_distances_bruteforce.back(), 1e-4); + ASSERT_EQ (k_indices_bruteforce.back(), k_indices.back ()); + EXPECT_NEAR (k_sqr_distances_bruteforce.back (), k_sqr_distances.back (), 1e-4); k_indices_bruteforce.pop_back (); k_indices.pop_back (); k_sqr_distances_bruteforce.pop_back (); k_sqr_distances.pop_back (); - } - } - } TEST (PCL, Octree_Pointcloud_Box_Search) @@ -1360,10 +1346,8 @@ TEST (PCL, Octree_Pointcloud_Box_Search) idxInResults = true; } - ASSERT_EQ(idxInResults, inBox); - + ASSERT_EQ (idxInResults, inBox); } - } } @@ -1390,7 +1374,7 @@ TEST(PCL, Octree_Pointcloud_Approx_Nearest_Neighbour_Search) { // define a random search point - PointXYZ searchPoint (static_cast (10.0 * rand () / RAND_MAX), + PointXYZ searchPoint (static_cast (10.0 * rand () / RAND_MAX), static_cast (10.0 * rand () / RAND_MAX), static_cast (10.0 * rand () / RAND_MAX)); @@ -1434,15 +1418,14 @@ TEST(PCL, Octree_Pointcloud_Approx_Nearest_Neighbour_Search) if (BFindex == ANNindex) { - EXPECT_NEAR(ANNdistance, BFdistance, 1e-4); + EXPECT_NEAR (ANNdistance, BFdistance, 1e-4); bestMatchCount++; } } // we should have found the absolute nearest neighbor at least once - ASSERT_EQ( (bestMatchCount > 0), true); - + ASSERT_TRUE (bestMatchCount > 0); } TEST (PCL, Octree_Pointcloud_Neighbours_Within_Radius_Search) @@ -1462,7 +1445,7 @@ TEST (PCL, Octree_Pointcloud_Neighbours_Within_Radius_Search) for (test_id = 0; test_id < test_runs; test_id++) { // define a random search point - PointXYZ searchPoint (static_cast (10.0 * rand () / RAND_MAX), + PointXYZ searchPoint (static_cast (10.0 * rand () / RAND_MAX), static_cast (10.0 * rand () / RAND_MAX), static_cast (10.0 * rand () / RAND_MAX)); @@ -1509,7 +1492,7 @@ TEST (PCL, Octree_Pointcloud_Neighbours_Within_Radius_Search) // execute octree radius search octree.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius); - ASSERT_EQ( cloudNWRRadius.size(), cloudSearchBruteforce.size()); + ASSERT_EQ (cloudSearchBruteforce.size (), cloudNWRRadius.size ()); // check if result from octree radius search can be also found in bruteforce search std::vector::const_iterator current = cloudNWRSearch.begin (); @@ -1520,7 +1503,7 @@ TEST (PCL, Octree_Pointcloud_Neighbours_Within_Radius_Search) + (cloudIn->points[*current].y - searchPoint.y) * (cloudIn->points[*current].y - searchPoint.y) + (cloudIn->points[*current].z - searchPoint.z) * (cloudIn->points[*current].z - searchPoint.z)); - ASSERT_EQ( (pointDist<=searchRadius), true); + ASSERT_TRUE (pointDist <= searchRadius); ++current; } @@ -1528,7 +1511,7 @@ TEST (PCL, Octree_Pointcloud_Neighbours_Within_Radius_Search) // check if result limitation works octree.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5); - ASSERT_EQ( cloudNWRRadius.size() <= 5, true); + ASSERT_TRUE (cloudNWRRadius.size () <= 5); } @@ -1536,7 +1519,6 @@ TEST (PCL, Octree_Pointcloud_Neighbours_Within_Radius_Search) TEST (PCL, Octree_Pointcloud_Ray_Traversal) { - const unsigned int test_runs = 100; unsigned int test_id; @@ -1564,12 +1546,12 @@ TEST (PCL, Octree_Pointcloud_Ray_Traversal) cloudIn->height = 1; cloudIn->points.resize (cloudIn->width * cloudIn->height); - Eigen::Vector3f p (static_cast (10.0 * rand () / RAND_MAX), + Eigen::Vector3f p (static_cast (10.0 * rand () / RAND_MAX), static_cast (10.0 * rand () / RAND_MAX), static_cast (10.0 * rand () / RAND_MAX)); // origin - Eigen::Vector3f o (static_cast (12.0 * rand () / RAND_MAX), + Eigen::Vector3f o (static_cast (12.0 * rand () / RAND_MAX), static_cast (12.0 * rand () / RAND_MAX), static_cast (12.0 * rand () / RAND_MAX)); @@ -1594,51 +1576,46 @@ TEST (PCL, Octree_Pointcloud_Ray_Traversal) octree_search.getIntersectedVoxelIndices (o, dir, indicesInRay); // check if all voxels in the cloud are penetraded by the ray - ASSERT_EQ( voxelsInRay.size (), cloudIn->points.size ()); + ASSERT_EQ (cloudIn->points.size (), voxelsInRay.size ()); // check if all indices of penetrated voxels are in cloud - ASSERT_EQ( indicesInRay.size (), cloudIn->points.size ()); + ASSERT_EQ (cloudIn->points.size (), indicesInRay.size ()); octree_search.getIntersectedVoxelCenters (o, dir, voxelsInRay2, 1); octree_search.getIntersectedVoxelIndices (o, dir, indicesInRay2, 1); // check if only the point from a single voxel has been returned - ASSERT_EQ( voxelsInRay2.size (), 1u ); - ASSERT_EQ( indicesInRay2.size (), 1u ); + ASSERT_EQ (1u, voxelsInRay2.size ()); + ASSERT_EQ (1u, indicesInRay2.size ()); // check if this point is the closest point to the origin pcl::PointXYZ pt = cloudIn->points[ indicesInRay2[0] ]; Eigen::Vector3f d = Eigen::Vector3f (pt.x, pt.y, pt.z) - o; float min_dist = d.norm (); - for (unsigned int i = 0; i < cloudIn->width * cloudIn->height; i++) { - pt = cloudIn->points[i]; - d = Eigen::Vector3f (pt.x, pt.y, pt.z) - o; - ASSERT_GE( d.norm (), min_dist ); + for (unsigned int i = 0; i < cloudIn->width * cloudIn->height; i++) + { + pt = cloudIn->points[i]; + d = Eigen::Vector3f (pt.x, pt.y, pt.z) - o; + ASSERT_GE (d.norm (), min_dist); } - } - } TEST (PCL, Octree_Pointcloud_Adjacency) { - const unsigned int test_runs = 100; unsigned int test_id; - - - srand (static_cast (time (NULL))); for (test_id = 0; test_id < test_runs; test_id++) { // instantiate point cloud PointCloud::Ptr cloudIn (new PointCloud ()); - + float resolution = static_cast (0.01 * rand () / RAND_MAX) + 0.00001f; //Define a random point - PointXYZ point (static_cast (0.5 * rand () / RAND_MAX), + PointXYZ point (static_cast (0.5 * rand () / RAND_MAX), static_cast (0.5 * rand () / RAND_MAX), static_cast (0.5 * rand () / RAND_MAX)); //This is done to define the grid @@ -1655,22 +1632,22 @@ TEST (PCL, Octree_Pointcloud_Adjacency) { for (int dz = -1; dz <= 1; ++dz) { - float factor = 1.0f;//*std::sqrt (dx*dx + dy*dy + dz*dz); - PointXYZ neighbor (point.x+dx*resolution*factor, point.y+dy*resolution*factor, point.z+dz*resolution*factor); + float factor = 1.0f;//*std::sqrt (dx*dx + dy*dy + dz*dz); + PointXYZ neighbor (point.x+dx*resolution*factor, point.y+dy*resolution*factor, point.z+dz*resolution*factor); cloudIn->push_back (neighbor); } } } - + //Add another point that isn't touching previous or neighbors - PointXYZ point2 (static_cast (point.x + 10*resolution), + PointXYZ point2 (static_cast (point.x + 10*resolution), static_cast (point.y + 10*resolution), static_cast (point.z + 10*resolution)); cloudIn->push_back (point2); // Add points which are not neighbors for (int i = 0; i < 100; ++i) { - PointXYZ not_neighbor_point (static_cast (10.0 * rand () / RAND_MAX), + PointXYZ not_neighbor_point (static_cast (10.0 * rand () / RAND_MAX), static_cast (10.0 * rand () / RAND_MAX), static_cast (10.0 * rand () / RAND_MAX)); if ( (point2.getVector3fMap () - not_neighbor_point.getVector3fMap ()).norm () > 3*resolution ) @@ -1678,26 +1655,24 @@ TEST (PCL, Octree_Pointcloud_Adjacency) cloudIn->push_back(not_neighbor_point); } } - - + + OctreePointCloudAdjacency octree (resolution); octree.setInputCloud (cloudIn); octree.addPointsFromInputCloud (); - + OctreePointCloudAdjacencyContainer *leaf_container; - + leaf_container = octree.getLeafContainerAtPoint (point); //Point should have 26 neighbors, plus itself - ASSERT_EQ( leaf_container->size() == 27, true); + ASSERT_EQ (27, leaf_container->size ()); leaf_container = octree.getLeafContainerAtPoint (point2); - - //Other point should only have itself for neighbor - ASSERT_EQ( leaf_container->size() == 1, true); + //Other point should only have itself for neighbor + ASSERT_EQ (1, leaf_container->size ()); } - } TEST (PCL, Octree_Pointcloud_Bounds) @@ -1706,30 +1681,30 @@ TEST (PCL, Octree_Pointcloud_Bounds) const int SOME_DEPTH (4); const double DESIRED_MAX = ((1< tree (SOME_RESOLUTION); tree.defineBoundingBox (DESIRED_MIN, DESIRED_MIN, DESIRED_MIN, DESIRED_MAX, DESIRED_MAX, DESIRED_MAX); - + double min_x, min_y, min_z, max_x, max_y, max_z; tree.getBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z); - - ASSERT_GE( max_x, DESIRED_MAX ); - ASSERT_GE( DESIRED_MIN, min_x ); - + + ASSERT_GE (max_x, DESIRED_MAX); + ASSERT_GE (DESIRED_MIN, min_x); + const double LARGE_MIN = 1e7-45*SOME_RESOLUTION; const double LARGE_MAX = 1e7-5*SOME_RESOLUTION; tree.defineBoundingBox (LARGE_MIN, LARGE_MIN, LARGE_MIN, LARGE_MAX, LARGE_MAX, LARGE_MAX); tree.getBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z); const unsigned int depth = tree.getTreeDepth (); tree.defineBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z); - - ASSERT_EQ( depth == tree.getTreeDepth (), true ); - + + ASSERT_EQ (depth, tree.getTreeDepth ()); + double min_x2, min_y2, min_z2, max_x2, max_y2, max_z2; tree.getBoundingBox (min_x2, min_y2, min_z2, max_x2, max_y2, max_z2); - - ASSERT_EQ( min_x2 == min_x, true ); - ASSERT_EQ( max_x2 == max_x, true ); + + ASSERT_DOUBLE_EQ (min_x2, min_x); + ASSERT_DOUBLE_EQ (max_x2, max_x); } /* ---[ */ int