diff --git a/include/small_gicp/ann/flat_container.hpp b/include/small_gicp/ann/flat_container.hpp index f16bde3..0d48d18 100644 --- a/include/small_gicp/ann/flat_container.hpp +++ b/include/small_gicp/ann/flat_container.hpp @@ -7,6 +7,7 @@ #include #include #include +#include namespace small_gicp { @@ -67,21 +68,25 @@ struct FlatContainer { return 0; } - size_t min_index = -1; - double min_sq_dist = std::numeric_limits::max(); + KnnResult<1> result(k_index, k_sq_dist); + knn_search(pt, result); + return result.num_found(); + } - for (size_t i = 0; i < points.size(); i++) { - const double sq_dist = (points[i] - pt).squaredNorm(); - if (sq_dist < min_sq_dist) { - min_index = i; - min_sq_dist = sq_dist; - } + /// @brief Find k nearest neighbors. + /// @param pt Query point + /// @param k Number of neighbors + /// @param k_index Indices of nearest neighbors + /// @param k_sq_dist Squared distances to nearest neighbors + /// @return Number of found points + size_t knn_search(const Eigen::Vector4d& pt, int k, size_t* k_indices, double* k_sq_dists) const { + if (points.empty()) { + return 0; } - *k_index = min_index; - *k_sq_dist = min_sq_dist; - - return 1; + KnnResult<-1> result(k_indices, k_sq_dists, k); + knn_search(pt, result); + return result.num_found(); } /// @brief Find k nearest neighbors. @@ -90,28 +95,16 @@ struct FlatContainer { /// @param k_index Indices of nearest neighbors /// @param k_sq_dist Squared distances to nearest neighbors /// @return Number of found points - size_t knn_search(const Eigen::Vector4d& pt, int k, size_t* k_index, double* k_sq_dist) const { + template + void knn_search(const Eigen::Vector4d& pt, Result& result) const { if (points.empty()) { - return 0; + return; } - std::priority_queue> queue; for (size_t i = 0; i < points.size(); i++) { const double sq_dist = (points[i] - pt).squaredNorm(); - queue.push({i, sq_dist}); - if (queue.size() > k) { - queue.pop(); - } + result.push(i, sq_dist); } - - const size_t n = queue.size(); - while (!queue.empty()) { - k_index[queue.size() - 1] = queue.top().first; - k_sq_dist[queue.size() - 1] = queue.top().second; - queue.pop(); - } - - return n; } public: @@ -151,6 +144,11 @@ struct Traits> { static size_t knn_search(const FlatContainer& container, const Eigen::Vector4d& pt, size_t k, size_t* k_index, double* k_sq_dist) { return container.knn_search(pt, k, k_index, k_sq_dist); } + + template + static void knn_search(const FlatContainer& container, const Eigen::Vector4d& pt, Result& result) { + container.knn_search(pt, result); + } }; } // namespace traits diff --git a/include/small_gicp/ann/gaussian_voxelmap.hpp b/include/small_gicp/ann/gaussian_voxelmap.hpp index 7f7fc38..51d1aad 100644 --- a/include/small_gicp/ann/gaussian_voxelmap.hpp +++ b/include/small_gicp/ann/gaussian_voxelmap.hpp @@ -79,6 +79,11 @@ struct Traits { static size_t knn_search(const GaussianVoxel& voxel, const Eigen::Vector4d& pt, size_t k, size_t* k_index, double* k_sq_dist) { return nearest_neighbor_search(voxel, pt, k_index, k_sq_dist); } + + template + static void knn_search(const GaussianVoxel& voxel, const Eigen::Vector4d& pt, Result& result) { + result.push(0, (voxel.mean - pt).squaredNorm()); + } }; } // namespace traits diff --git a/include/small_gicp/ann/incremental_voxelmap.hpp b/include/small_gicp/ann/incremental_voxelmap.hpp index 73ecba1..2080ecb 100644 --- a/include/small_gicp/ann/incremental_voxelmap.hpp +++ b/include/small_gicp/ann/incremental_voxelmap.hpp @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -103,13 +104,10 @@ struct IncrementalVoxelMap { const size_t voxel_index = found->second; const auto& voxel = flat_voxels[voxel_index]->second; - size_t point_index; - if (traits::nearest_neighbor_search(voxel, pt, &point_index, sq_dist) == 0) { - return 0; - } - - *index = calc_index(voxel_index, point_index); - return 1; + const auto index_transform = [=](size_t i) { return calc_index(voxel_index, i); }; + KnnResult<1, decltype(index_transform)> result(index, sq_dist, -1, index_transform); + traits::Traits::knn_search(voxel, pt, result); + return result.num_found(); } /// @brief Find k nearest neighbors @@ -128,15 +126,10 @@ struct IncrementalVoxelMap { const size_t voxel_index = found->second; const auto& voxel = flat_voxels[voxel_index]->second; - std::vector point_indices(k); - std::vector sq_dists(k); - const size_t num_found = traits::knn_search(voxel, pt, k, point_indices.data(), sq_dists.data()); - - for (size_t i = 0; i < num_found; i++) { - k_indices[i] = calc_index(voxel_index, point_indices[i]); - k_sq_dists[i] = sq_dists[i]; - } - return num_found; + const auto index_transform = [=](size_t i) { return calc_index(voxel_index, i); }; + KnnResult<-1, decltype(index_transform)> result(k_indices, k_sq_dists, k, index_transform); + traits::Traits::knn_search(voxel, pt, result); + return result.num_found(); } /// @brief Calculate the global point index from the voxel index and the point index. diff --git a/include/small_gicp/ann/knn_result.hpp b/include/small_gicp/ann/knn_result.hpp index 53ee375..edaa419 100644 --- a/include/small_gicp/ann/knn_result.hpp +++ b/include/small_gicp/ann/knn_result.hpp @@ -22,9 +22,14 @@ struct KnnSetting { double epsilon = 0.0; ///< Early termination threshold }; +/// @brief Identity transform (alternative to std::identity in C++20). +struct identity_transform { + size_t operator()(size_t i) const { return i; } +}; + /// @brief K-nearest neighbor search result container. /// @tparam N Number of neighbors to search. If N == -1, the number of neighbors is dynamicaly determined. -template +template struct KnnResult { public: static constexpr size_t INVALID = std::numeric_limits::max(); @@ -33,7 +38,12 @@ struct KnnResult { /// @param indices Buffer to store indices (must be larger than k=max(N, num_neighbors)) /// @param distances Buffer to store distances (must be larger than k=max(N, num_neighbors)) /// @param num_neighbors Number of neighbors to search (must be -1 for static case N > 0) - explicit KnnResult(size_t* indices, double* distances, int num_neighbors = -1) : capacity(num_neighbors), num_found_neighbors(0), indices(indices), distances(distances) { + explicit KnnResult(size_t* indices, double* distances, int num_neighbors = -1, const IndexTransform& index_transform = identity_transform()) + : index_transform(index_transform), + capacity(num_neighbors), + num_found_neighbors(0), + indices(indices), + distances(distances) { if constexpr (N > 0) { if (num_neighbors >= 0) { std::cerr << "warning: Specifying dynamic num_neighbors=" << num_neighbors << " for a static KNN result container (N=" << N << ")" << std::endl; @@ -72,7 +82,7 @@ struct KnnResult { } if constexpr (N == 1) { - indices[0] = index; + indices[0] = index_transform(index); distances[0] = distance; } else { int insert_loc = std::min(num_found_neighbors, buffer_size() - 1); @@ -81,7 +91,7 @@ struct KnnResult { distances[insert_loc] = distances[insert_loc - 1]; } - indices[insert_loc] = index; + indices[insert_loc] = index_transform(index); distances[insert_loc] = distance; } @@ -89,10 +99,11 @@ struct KnnResult { } public: - const int capacity; ///< Maximum number of neighbors to search - int num_found_neighbors; ///< Number of found neighbors - size_t* indices; ///< Indices of neighbors - double* distances; ///< Distances to neighbors + const IndexTransform index_transform; ///< Point index transformation (e.g., local point index to global point/voxel index) + const int capacity; ///< Maximum number of neighbors to search + int num_found_neighbors; ///< Number of found neighbors + size_t* indices; ///< Indices of neighbors + double* distances; ///< Distances to neighbors }; } // namespace small_gicp