From 8bd1213e914bcf9cd0729ed34c0c8351d41d342c Mon Sep 17 00:00:00 2001 From: Markus Vieth Date: Sun, 21 Mar 2021 11:07:49 +0100 Subject: [PATCH 1/3] Improve test_octree: test search functions also on trees with dynamic depth And remove unused variable cloudOut --- test/octree/test_octree.cpp | 422 +++++++++++++++++++----------------- 1 file changed, 218 insertions(+), 204 deletions(-) diff --git a/test/octree/test_octree.cpp b/test/octree/test_octree.cpp index c0811f996b0..0a4df9e6e3d 100644 --- a/test/octree/test_octree.cpp +++ b/test/octree/test_octree.cpp @@ -1142,80 +1142,84 @@ TEST (PCL, Octree_Pointcloud_Nearest_K_Neighbour_Search) Indices k_indices_bruteforce; std::vector k_sqr_distances_bruteforce; - for (unsigned int test_id = 0; test_id < test_runs; test_id++) - { - // define a random search point + for (const std::size_t maxObjsPerLeaf: {0, 5}) { + if (maxObjsPerLeaf != 0) + octree.enableDynamicDepth (maxObjsPerLeaf); + for (unsigned int test_id = 0; test_id < test_runs; test_id++) + { + // define a random search point - PointXYZ searchPoint (static_cast (10.0 * rand () / RAND_MAX), - static_cast (10.0 * rand () / RAND_MAX), - 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)); - unsigned int K = 1 + rand () % 10; + unsigned int K = 1 + rand () % 10; - // generate point cloud - cloudIn->width = 1000; - cloudIn->height = 1; - cloudIn->points.resize (cloudIn->width * cloudIn->height); - for (std::size_t i = 0; i < 1000; i++) - { - (*cloudIn)[i] = PointXYZ (static_cast (5.0 * rand () / RAND_MAX), - static_cast (10.0 * rand () / RAND_MAX), - static_cast (10.0 * rand () / RAND_MAX)); - } + // generate point cloud + cloudIn->width = 1000; + cloudIn->height = 1; + cloudIn->points.resize (cloudIn->width * cloudIn->height); + for (std::size_t i = 0; i < 1000; i++) + { + (*cloudIn)[i] = PointXYZ (static_cast (5.0 * rand () / RAND_MAX), + static_cast (10.0 * rand () / RAND_MAX), + static_cast (10.0 * rand () / RAND_MAX)); + } - k_indices.clear (); - k_sqr_distances.clear (); + k_indices.clear (); + k_sqr_distances.clear (); - k_indices_bruteforce.clear (); - k_sqr_distances_bruteforce.clear (); + k_indices_bruteforce.clear (); + k_sqr_distances_bruteforce.clear (); - // push all points and their distance to the search point into a priority queue - bruteforce approach. - for (std::size_t i = 0; i < cloudIn->size (); i++) - { - double pointDist = (((*cloudIn)[i].x - searchPoint.x) * ((*cloudIn)[i].x - searchPoint.x) + - ((*cloudIn)[i].y - searchPoint.y) * ((*cloudIn)[i].y - searchPoint.y) + - ((*cloudIn)[i].z - searchPoint.z) * ((*cloudIn)[i].z - searchPoint.z)); + // push all points and their distance to the search point into a priority queue - bruteforce approach. + for (std::size_t i = 0; i < cloudIn->size (); i++) + { + double pointDist = (((*cloudIn)[i].x - searchPoint.x) * ((*cloudIn)[i].x - searchPoint.x) + + ((*cloudIn)[i].y - searchPoint.y) * ((*cloudIn)[i].y - searchPoint.y) + + ((*cloudIn)[i].z - searchPoint.z) * ((*cloudIn)[i].z - searchPoint.z)); - prioPointQueueEntry pointEntry ((*cloudIn)[i], pointDist, static_cast (i)); + prioPointQueueEntry pointEntry ((*cloudIn)[i], pointDist, static_cast (i)); - pointCandidates.push (pointEntry); - } + pointCandidates.push (pointEntry); + } - // pop priority queue until we have the nearest K elements - while (pointCandidates.size () > K) - pointCandidates.pop (); + // pop priority queue until we have the nearest K elements + while (pointCandidates.size () > K) + pointCandidates.pop (); - // copy results into vectors - unsigned idx = static_cast (pointCandidates.size ()); - k_indices_bruteforce.resize (idx); - k_sqr_distances_bruteforce.resize (idx); - while (!pointCandidates.empty ()) - { - --idx; - k_indices_bruteforce [idx] = pointCandidates.top ().pointIdx_; - k_sqr_distances_bruteforce [idx] = static_cast (pointCandidates.top ().pointDistance_); + // copy results into vectors + unsigned idx = static_cast (pointCandidates.size ()); + k_indices_bruteforce.resize (idx); + k_sqr_distances_bruteforce.resize (idx); + while (!pointCandidates.empty ()) + { + --idx; + k_indices_bruteforce [idx] = pointCandidates.top ().pointIdx_; + k_sqr_distances_bruteforce [idx] = static_cast (pointCandidates.top ().pointDistance_); - pointCandidates.pop (); - } + pointCandidates.pop (); + } - // octree nearest neighbor search - octree.deleteTree (); - octree.addPointsFromInputCloud (); - octree.nearestKSearch (searchPoint, static_cast (K), k_indices, k_sqr_distances); + // octree nearest neighbor search + octree.deleteTree (); + octree.addPointsFromInputCloud (); + octree.nearestKSearch (searchPoint, static_cast (K), k_indices, k_sqr_distances); - ASSERT_EQ (k_indices_bruteforce.size (), k_indices.size()); + ASSERT_EQ (k_indices_bruteforce.size (), k_indices.size()); - // compare nearest neighbor results of octree with bruteforce search - while (!k_indices_bruteforce.empty ()) - { - ASSERT_EQ (k_indices_bruteforce.back(), k_indices.back ()); - EXPECT_NEAR (k_sqr_distances_bruteforce.back (), k_sqr_distances.back (), 1e-4); + // compare nearest neighbor results of octree with bruteforce search + while (!k_indices_bruteforce.empty ()) + { + 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_indices_bruteforce.pop_back (); + k_indices.pop_back (); - k_sqr_distances_bruteforce.pop_back (); - k_sqr_distances.pop_back (); + k_sqr_distances_bruteforce.pop_back (); + k_sqr_distances.pop_back (); + } } } } @@ -1233,56 +1237,60 @@ TEST (PCL, Octree_Pointcloud_Box_Search) OctreePointCloudSearch octree (1); octree.setInputCloud (cloudIn); - for (unsigned int test_id = 0; test_id < test_runs; test_id++) - { - Indices k_indices; - - // generate point cloud - cloudIn->width = 300; - cloudIn->height = 1; - cloudIn->points.resize (cloudIn->width * cloudIn->height); - for (auto &point : cloudIn->points) + for (const std::size_t maxObjsPerLeaf: {0, 5}) { + if (maxObjsPerLeaf != 0) + octree.enableDynamicDepth (maxObjsPerLeaf); + for (unsigned int test_id = 0; test_id < test_runs; test_id++) { - point = PointXYZ (static_cast (10.0 * rand () / RAND_MAX), - static_cast (10.0 * rand () / RAND_MAX), - static_cast (10.0 * rand () / RAND_MAX)); - } + Indices k_indices; + // generate point cloud + cloudIn->width = 300; + cloudIn->height = 1; + cloudIn->points.resize (cloudIn->width * cloudIn->height); + for (auto &point : cloudIn->points) + { + point = PointXYZ (static_cast (10.0 * rand () / RAND_MAX), + static_cast (10.0 * rand () / RAND_MAX), + static_cast (10.0 * rand () / RAND_MAX)); + } - // octree points to octree - octree.deleteTree (); - octree.addPointsFromInputCloud (); - - // define a random search area - Eigen::Vector3f lowerBoxCorner (static_cast (4.0 * rand () / RAND_MAX), - static_cast (4.0 * rand () / RAND_MAX), - static_cast (4.0 * rand () / RAND_MAX)); - Eigen::Vector3f upperBoxCorner (static_cast (5.0 + 4.0 * rand () / RAND_MAX), - static_cast (5.0 + 4.0 * rand () / RAND_MAX), - static_cast (5.0 + 4.0 * rand () / RAND_MAX)); + // octree points to octree + octree.deleteTree (); + octree.addPointsFromInputCloud (); - octree.boxSearch (lowerBoxCorner, upperBoxCorner, k_indices); + // define a random search area - // test every point in point cloud - for (std::size_t i = 0; i < 300; i++) - { - bool inBox; - bool idxInResults; - const PointXYZ& pt = (*cloudIn)[i]; + Eigen::Vector3f lowerBoxCorner (static_cast (4.0 * rand () / RAND_MAX), + static_cast (4.0 * rand () / RAND_MAX), + static_cast (4.0 * rand () / RAND_MAX)); + Eigen::Vector3f upperBoxCorner (static_cast (5.0 + 4.0 * rand () / RAND_MAX), + static_cast (5.0 + 4.0 * rand () / RAND_MAX), + static_cast (5.0 + 4.0 * rand () / RAND_MAX)); - inBox = (pt.x >= lowerBoxCorner (0)) && (pt.x <= upperBoxCorner (0)) && - (pt.y >= lowerBoxCorner (1)) && (pt.y <= upperBoxCorner (1)) && - (pt.z >= lowerBoxCorner (2)) && (pt.z <= upperBoxCorner (2)); + octree.boxSearch (lowerBoxCorner, upperBoxCorner, k_indices); - idxInResults = false; - for (std::size_t j = 0; (j < k_indices.size ()) && (!idxInResults); ++j) + // test every point in point cloud + for (std::size_t i = 0; i < cloudIn->size(); i++) { - if (i == static_cast (k_indices[j])) - idxInResults = true; - } + bool inBox; + bool idxInResults; + const PointXYZ& pt = (*cloudIn)[i]; - ASSERT_EQ (idxInResults, inBox); + inBox = (pt.x >= lowerBoxCorner (0)) && (pt.x <= upperBoxCorner (0)) && + (pt.y >= lowerBoxCorner (1)) && (pt.y <= upperBoxCorner (1)) && + (pt.z >= lowerBoxCorner (2)) && (pt.z <= upperBoxCorner (2)); + + idxInResults = false; + for (std::size_t j = 0; (j < k_indices.size ()) && (!idxInResults); ++j) + { + if (i == static_cast (k_indices[j])) + idxInResults = true; + } + + ASSERT_EQ (idxInResults, inBox); + } } } } @@ -1291,74 +1299,78 @@ TEST(PCL, Octree_Pointcloud_Approx_Nearest_Neighbour_Search) { constexpr unsigned int test_runs = 100; - unsigned int bestMatchCount = 0; - - // instantiate point cloud - PointCloud::Ptr cloudIn (new PointCloud ()); - - srand (static_cast (time (nullptr))); + for (const std::size_t maxObjsPerLeaf: {0, 5}) { + unsigned int bestMatchCount = 0; - constexpr double voxelResolution = 0.1; + // instantiate point cloud + PointCloud::Ptr cloudIn (new PointCloud ()); - // create octree - OctreePointCloudSearch octree (voxelResolution); - octree.setInputCloud (cloudIn); + srand (static_cast (time (nullptr))); - for (unsigned int test_id = 0; test_id < test_runs; test_id++) - { - // define a random search point + constexpr double voxelResolution = 0.1; - PointXYZ searchPoint (static_cast (10.0 * rand () / RAND_MAX), - static_cast (10.0 * rand () / RAND_MAX), - static_cast (10.0 * rand () / RAND_MAX)); + // create octree + OctreePointCloudSearch octree (voxelResolution); + octree.setInputCloud (cloudIn); + if (maxObjsPerLeaf != 0) + octree.enableDynamicDepth (maxObjsPerLeaf); - // generate point cloud - cloudIn->width = 1000; - cloudIn->height = 1; - cloudIn->points.resize (cloudIn->width * cloudIn->height); - for (std::size_t i = 0; i < 1000; i++) + for (unsigned int test_id = 0; test_id < test_runs; test_id++) { - (*cloudIn)[i] = PointXYZ (static_cast (5.0 * rand () / RAND_MAX), - static_cast (10.0 * rand () / RAND_MAX), - static_cast (10.0 * rand () / RAND_MAX)); - } + // define a random search point - // brute force search - double BFdistance = std::numeric_limits::max (); - pcl::index_t BFindex = 0; + PointXYZ searchPoint (static_cast (10.0 * rand () / RAND_MAX), + static_cast (10.0 * rand () / RAND_MAX), + static_cast (10.0 * rand () / RAND_MAX)); - for (std::size_t i = 0; i < cloudIn->size (); i++) - { - double pointDist = (((*cloudIn)[i].x - searchPoint.x) * ((*cloudIn)[i].x - searchPoint.x) + - ((*cloudIn)[i].y - searchPoint.y) * ((*cloudIn)[i].y - searchPoint.y) + - ((*cloudIn)[i].z - searchPoint.z) * ((*cloudIn)[i].z - searchPoint.z)); + // generate point cloud + cloudIn->width = 1000; + cloudIn->height = 1; + cloudIn->points.resize (cloudIn->width * cloudIn->height); + for (std::size_t i = 0; i < 1000; i++) + { + (*cloudIn)[i] = PointXYZ (static_cast (5.0 * rand () / RAND_MAX), + static_cast (10.0 * rand () / RAND_MAX), + static_cast (10.0 * rand () / RAND_MAX)); + } - if (pointDist < BFdistance) + // brute force search + double BFdistance = std::numeric_limits::max (); + pcl::index_t BFindex = 0; + + for (std::size_t i = 0; i < cloudIn->size (); i++) { - BFindex = static_cast (i); - BFdistance = pointDist; + double pointDist = (((*cloudIn)[i].x - searchPoint.x) * ((*cloudIn)[i].x - searchPoint.x) + + ((*cloudIn)[i].y - searchPoint.y) * ((*cloudIn)[i].y - searchPoint.y) + + ((*cloudIn)[i].z - searchPoint.z) * ((*cloudIn)[i].z - searchPoint.z)); + + if (pointDist < BFdistance) + { + BFindex = static_cast (i); + BFdistance = pointDist; + } + } - } + index_t ANNindex; + float ANNdistance; - index_t ANNindex; - float ANNdistance; + // octree approx. nearest neighbor search + octree.deleteTree (); + octree.addPointsFromInputCloud (); + octree.approxNearestSearch (searchPoint, ANNindex, ANNdistance); - // octree approx. nearest neighbor search - octree.deleteTree (); - octree.addPointsFromInputCloud (); - octree.approxNearestSearch (searchPoint, ANNindex, ANNdistance); + if (BFindex == ANNindex) + { + EXPECT_NEAR (ANNdistance, BFdistance, 1e-4); + bestMatchCount++; + } - if (BFindex == ANNindex) - { - EXPECT_NEAR (ANNdistance, BFdistance, 1e-4); - bestMatchCount++; } + // we should have found the absolute nearest neighbor at least once + ASSERT_GT (bestMatchCount, 0); } - - // we should have found the absolute nearest neighbor at least once - ASSERT_GT (bestMatchCount, 0); } TEST (PCL, Octree_Pointcloud_Neighbours_Within_Radius_Search) @@ -1367,83 +1379,85 @@ TEST (PCL, Octree_Pointcloud_Neighbours_Within_Radius_Search) // instantiate point clouds PointCloud::Ptr cloudIn (new PointCloud ()); - PointCloud::Ptr cloudOut (new PointCloud ()); srand (static_cast (time (nullptr))); - for (unsigned int test_id = 0; test_id < test_runs; test_id++) - { - // define a random search point - PointXYZ searchPoint (static_cast (10.0 * rand () / RAND_MAX), - static_cast (10.0 * rand () / RAND_MAX), - static_cast (10.0 * rand () / RAND_MAX)); - - cloudIn->width = 1000; - cloudIn->height = 1; - cloudIn->points.resize (cloudIn->width * cloudIn->height); - - // generate point cloud data - for (std::size_t i = 0; i < 1000; i++) + for (const std::size_t maxObjsPerLeaf: {0, 5}) { + for (unsigned int test_id = 0; test_id < test_runs; test_id++) { - (*cloudIn)[i] = PointXYZ (static_cast (10.0 * rand () / RAND_MAX), - static_cast (10.0 * rand () / RAND_MAX), - static_cast (5.0 * rand () / RAND_MAX)); - } + // define a random search point + PointXYZ searchPoint (static_cast (10.0 * rand () / RAND_MAX), + static_cast (10.0 * rand () / RAND_MAX), + static_cast (10.0 * rand () / RAND_MAX)); - OctreePointCloudSearch octree (0.001); + cloudIn->width = 1000; + cloudIn->height = 1; + cloudIn->points.resize (cloudIn->width * cloudIn->height); - // build octree - octree.setInputCloud (cloudIn); - octree.addPointsFromInputCloud (); + // generate point cloud data + for (std::size_t i = 0; i < 1000; i++) + { + (*cloudIn)[i] = PointXYZ (static_cast (10.0 * rand () / RAND_MAX), + static_cast (10.0 * rand () / RAND_MAX), + static_cast (5.0 * rand () / RAND_MAX)); + } - double pointDist; - double searchRadius = 5.0 * static_cast (rand () / RAND_MAX); + OctreePointCloudSearch octree (0.001); - // bruteforce radius search - std::vector cloudSearchBruteforce; - for (std::size_t i = 0; i < cloudIn->size (); i++) - { - pointDist = sqrt ( - ((*cloudIn)[i].x - searchPoint.x) * ((*cloudIn)[i].x - searchPoint.x) - + ((*cloudIn)[i].y - searchPoint.y) * ((*cloudIn)[i].y - searchPoint.y) - + ((*cloudIn)[i].z - searchPoint.z) * ((*cloudIn)[i].z - searchPoint.z)); + // build octree + octree.setInputCloud (cloudIn); + if (maxObjsPerLeaf != 0) + octree.enableDynamicDepth (maxObjsPerLeaf); + octree.addPointsFromInputCloud (); - if (pointDist <= searchRadius) + double pointDist; + double searchRadius = 5.0 * static_cast (rand () / RAND_MAX); + + // bruteforce radius search + std::vector cloudSearchBruteforce; + for (std::size_t i = 0; i < cloudIn->size (); i++) { - // add point candidates to vector list - cloudSearchBruteforce.push_back (static_cast (i)); + pointDist = sqrt ( + ((*cloudIn)[i].x - searchPoint.x) * ((*cloudIn)[i].x - searchPoint.x) + + ((*cloudIn)[i].y - searchPoint.y) * ((*cloudIn)[i].y - searchPoint.y) + + ((*cloudIn)[i].z - searchPoint.z) * ((*cloudIn)[i].z - searchPoint.z)); + + if (pointDist <= searchRadius) + { + // add point candidates to vector list + cloudSearchBruteforce.push_back (static_cast (i)); + } } - } - Indices cloudNWRSearch; - std::vector cloudNWRRadius; + Indices cloudNWRSearch; + std::vector cloudNWRRadius; - // execute octree radius search - octree.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius); + // execute octree radius search + octree.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius); - ASSERT_EQ (cloudSearchBruteforce.size (), cloudNWRRadius.size ()); + ASSERT_EQ (cloudSearchBruteforce.size (), cloudNWRRadius.size ()); - // check if result from octree radius search can be also found in bruteforce search - auto current = cloudNWRSearch.cbegin (); - while (current != cloudNWRSearch.cend ()) - { - pointDist = sqrt ( - ((*cloudIn)[*current].x - searchPoint.x) * ((*cloudIn)[*current].x - searchPoint.x) - + ((*cloudIn)[*current].y - searchPoint.y) * ((*cloudIn)[*current].y - searchPoint.y) - + ((*cloudIn)[*current].z - searchPoint.z) * ((*cloudIn)[*current].z - searchPoint.z)); + // check if result from octree radius search can be also found in bruteforce search + auto current = cloudNWRSearch.cbegin (); + while (current != cloudNWRSearch.cend ()) + { + pointDist = sqrt ( + ((*cloudIn)[*current].x - searchPoint.x) * ((*cloudIn)[*current].x - searchPoint.x) + + ((*cloudIn)[*current].y - searchPoint.y) * ((*cloudIn)[*current].y - searchPoint.y) + + ((*cloudIn)[*current].z - searchPoint.z) * ((*cloudIn)[*current].z - searchPoint.z)); - ASSERT_LE (pointDist, searchRadius); + ASSERT_LE (pointDist, searchRadius); - ++current; - } + ++current; + } - // check if result limitation works - octree.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5); + // check if result limitation works + octree.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5); - ASSERT_LE (cloudNWRRadius.size (), 5); + ASSERT_LE (cloudNWRRadius.size (), 5); + } } - } TEST (PCL, Octree_Pointcloud_Ray_Traversal) From 582da42cdc85d70fa00762686c22479c629b8aad Mon Sep 17 00:00:00 2001 From: Markus Vieth Date: Sun, 21 Mar 2021 11:09:01 +0100 Subject: [PATCH 2/3] Octree search: check node type before casting This fixes segmentation faults for octrees with dynamic depth when a node was cast to a branch node, but was actually a leaf node. --- octree/include/pcl/octree/impl/octree_search.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/octree/include/pcl/octree/impl/octree_search.hpp b/octree/include/pcl/octree/impl/octree_search.hpp index d385e185c70..07ad568c83c 100644 --- a/octree/include/pcl/octree/impl/octree_search.hpp +++ b/octree/include/pcl/octree/impl/octree_search.hpp @@ -277,7 +277,7 @@ OctreePointCloudSearch:: child_node = search_heap.back().node; new_key = search_heap.back().key; - if (tree_depth < this->octree_depth_) { + if (child_node->getNodeType() == BRANCH_NODE) { // we have not reached maximum tree depth smallest_squared_dist = getKNearestNeighborRecursive(point, @@ -373,7 +373,7 @@ OctreePointCloudSearch:: voxel_squared_diameter / 4.0 + radiusSquared + sqrt(voxel_squared_diameter * radiusSquared)) { - if (tree_depth < this->octree_depth_) { + if (child_node->getNodeType() == BRANCH_NODE) { // we have not reached maximum tree depth getNeighborsWithinRadiusRecursive(point, radiusSquared, @@ -468,7 +468,7 @@ OctreePointCloudSearch:: child_node = this->getBranchChildPtr(*node, min_child_idx); - if (tree_depth < this->octree_depth_) { + if (child_node->getNodeType() == BRANCH_NODE) { // we have not reached maximum tree depth approxNearestSearchRecursive(point, static_cast(child_node), @@ -552,7 +552,7 @@ OctreePointCloudSearch::boxSearchRecur (lower_voxel_corner(1) > max_pt(1)) || (min_pt(1) > upper_voxel_corner(1)) || (lower_voxel_corner(2) > max_pt(2)) || (min_pt(2) > upper_voxel_corner(2)))) { - if (tree_depth < this->octree_depth_) { + if (child_node->getNodeType() == BRANCH_NODE) { // we have not reached maximum tree depth boxSearchRecursive(min_pt, max_pt, From 7abcd66865ca2a382d7d8eca0092a9584a7083b1 Mon Sep 17 00:00:00 2001 From: Markus Vieth Date: Sun, 21 Mar 2021 11:09:47 +0100 Subject: [PATCH 3/3] Octree search: use more float instead of double pointSquaredDist returns a float, so it makes no sense to cast it back-and-forth --- octree/include/pcl/octree/impl/octree_search.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/octree/include/pcl/octree/impl/octree_search.hpp b/octree/include/pcl/octree/impl/octree_search.hpp index 07ad568c83c..548b97fc413 100644 --- a/octree/include/pcl/octree/impl/octree_search.hpp +++ b/octree/include/pcl/octree/impl/octree_search.hpp @@ -483,7 +483,7 @@ OctreePointCloudSearch:: const LeafNode* child_leaf = static_cast(child_node); - double smallest_squared_dist = std::numeric_limits::max(); + float smallest_squared_dist = std::numeric_limits::max(); // decode leaf node into decoded_point_vector (**child_leaf).getPointIndices(decoded_point_vector); @@ -493,7 +493,7 @@ OctreePointCloudSearch:: const PointT& candidate_point = this->getPointByIndex(index); // calculate point distance to search point - double squared_dist = pointSquaredDist(candidate_point, point); + float squared_dist = pointSquaredDist(candidate_point, point); // check if a closer match is found if (squared_dist >= smallest_squared_dist) @@ -501,7 +501,7 @@ OctreePointCloudSearch:: result_index = index; smallest_squared_dist = squared_dist; - sqr_distance = static_cast(squared_dist); + sqr_distance = squared_dist; } } }