diff --git a/search/include/pcl/search/impl/search.hpp b/search/include/pcl/search/impl/search.hpp index aa8d229fd89..8db7c1f255d 100644 --- a/search/include/pcl/search/impl/search.hpp +++ b/search/include/pcl/search/impl/search.hpp @@ -40,9 +40,11 @@ #include +namespace pcl{ +namespace search { /////////////////////////////////////////////////////////////////////////////////////////// template -pcl::search::Search::Search (const std::string& name, bool sorted) +Search::Search (const std::string& name, bool sorted) : input_ () , sorted_results_ (sorted) , name_ (name) @@ -51,28 +53,28 @@ pcl::search::Search::Search (const std::string& name, bool sorted) /////////////////////////////////////////////////////////////////////////////////////////// template const std::string& -pcl::search::Search::getName () const +Search::getName () const { return (name_); } /////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::search::Search::setSortedResults (bool sorted) +Search::setSortedResults (bool sorted) { sorted_results_ = sorted; } /////////////////////////////////////////////////////////////////////////////////////////// template bool -pcl::search::Search::getSortedResults () +Search::getSortedResults () const { return (sorted_results_); } /////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::search::Search::setInputCloud ( +Search::setInputCloud ( const PointCloudConstPtr& cloud, const IndicesConstPtr &indices) { input_ = cloud; @@ -82,7 +84,7 @@ pcl::search::Search::setInputCloud ( /////////////////////////////////////////////////////////////////////////////////////////// template int -pcl::search::Search::nearestKSearch ( +Search::nearestKSearch ( const PointCloud &cloud, index_t index, int k, Indices &k_indices, std::vector &k_sqr_distances) const { @@ -92,7 +94,7 @@ pcl::search::Search::nearestKSearch ( /////////////////////////////////////////////////////////////////////////////////////////// template int -pcl::search::Search::nearestKSearch ( +Search::nearestKSearch ( index_t index, int k, Indices &k_indices, std::vector &k_sqr_distances) const @@ -110,7 +112,7 @@ pcl::search::Search::nearestKSearch ( /////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::search::Search::nearestKSearch ( +Search::nearestKSearch ( const PointCloud& cloud, const Indices& indices, int k, std::vector& k_indices, std::vector< std::vector >& k_sqr_distances) const @@ -133,7 +135,7 @@ pcl::search::Search::nearestKSearch ( /////////////////////////////////////////////////////////////////////////////////////////// template int -pcl::search::Search::radiusSearch ( +Search::radiusSearch ( const PointCloud &cloud, index_t index, double radius, Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn) const @@ -144,7 +146,7 @@ pcl::search::Search::radiusSearch ( /////////////////////////////////////////////////////////////////////////////////////////// template int -pcl::search::Search::radiusSearch ( +Search::radiusSearch ( index_t index, double radius, Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn ) const { @@ -159,7 +161,7 @@ pcl::search::Search::radiusSearch ( /////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::search::Search::radiusSearch ( +Search::radiusSearch ( const PointCloud& cloud, const Indices& indices, double radius, @@ -185,7 +187,7 @@ pcl::search::Search::radiusSearch ( /////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::search::Search::sortResults ( +Search::sortResults ( Indices& indices, std::vector& distances) const { Indices order (indices.size ()); @@ -205,6 +207,105 @@ pcl::search::Search::sortResults ( sort (distances.begin (), distances.end ()); } +template +std::size_t +Search::makeNonTrivial(Indices& k_indices, + std::vector& k_sqr_distances) const +{ + assert(k_indices.size() == k_sqr_distances.size()); + if (k_indices.empty()) { + return 0; + } + + if (getSortedResults()) { + std::size_t num_zero_elements = 0; + // distances are sorted, so if we encounter a non-zero, we can bail early + for (const auto& distance : k_sqr_distances) { + if (distance) { + break; + } + ++num_zero_elements; + } + // We need to remove `num_zero_elements` initial elements + k_indices.erase(k_indices.begin(), k_indices.begin() + num_zero_elements); + k_sqr_distances.erase(k_sqr_distances.begin(), + k_sqr_distances.begin() + num_zero_elements); + } + else { + const auto zero_distance_it = + std::find(k_sqr_distances.begin(), k_sqr_distances.end(), 0); + const auto zero_distance_idx = + std::distance(k_sqr_distances.begin(), zero_distance_it); + // From zero_distance_idx, we start removing elements + std::size_t last_good_idx = zero_distance_idx - 1; + for (std::size_t i = zero_distance_idx + 1; i < k_sqr_distances.size(); ++i) { + if (k_sqr_distances[i] == 0) { + continue; + } + ++last_good_idx; + k_sqr_distances[last_good_idx] = k_sqr_distances[i]; + k_indices[last_good_idx] = k_indices[i]; + } + // We need to remove elements after `last_good_idx` + const auto new_end = last_good_idx + 1; + k_indices.erase(k_indices.begin() + new_end, k_indices.end()); + k_sqr_distances.erase(k_sqr_distances.begin() + new_end, k_sqr_distances.end()); + } + assert(k_indices.size() == k_sqr_distances.size()); + return k_indices.size(); +} + +template +std::size_t +Search::makeNonTrivial(index_t index, + Indices& k_indices, + std::vector& k_sqr_distances) const +{ + assert(k_indices.size() == k_sqr_distances.size()); + if (k_indices.empty()) { + return 0; + } + + if (getSortedResults()) { + std::size_t same_idx_elements = 0; + // distances are sorted, so if we encounter a non-zero, we can bail early + for (const auto& idx : k_indices) { + if (idx != index) { + break; + } + ++same_idx_elements; + } + // We need to remove `same_idx_elements` initial elements + k_indices.erase(k_indices.begin(), k_indices.begin() + same_idx_elements); + k_sqr_distances.erase(k_sqr_distances.begin(), + k_sqr_distances.begin() + same_idx_elements); + } + else { + const auto same_idx_it = + std::find(k_indices.begin(), k_indices.end(), index); + const auto same_idx = std::distance(k_indices.begin(), same_idx_it); + // From same_idx, we start removing elements + std::size_t last_good_idx = same_idx - 1; + for (std::size_t i = same_idx + 1; i < k_indices.size(); ++i) { + if (k_indices[i] == index) { + continue; + } + ++last_good_idx; + k_sqr_distances[last_good_idx] = k_sqr_distances[i]; + k_indices[last_good_idx] = k_indices[i]; + } + // We need to remove elements after `last_good_idx` + const auto new_end = last_good_idx + 1; + k_indices.erase(k_indices.begin() + new_end, k_indices.end()); + k_sqr_distances.erase(k_sqr_distances.begin() + new_end, k_sqr_distances.end()); + } + + assert(k_indices.size() == k_sqr_distances.size()); + return k_indices.size(); +} +} // namespace search +} // namespace pcl + #define PCL_INSTANTIATE_Search(T) template class PCL_EXPORTS pcl::search::Search; #endif //#ifndef _PCL_SEARCH_SEARCH_IMPL_HPP_ diff --git a/search/include/pcl/search/search.h b/search/include/pcl/search/search.h index 201b92568a9..6d92989e1d5 100644 --- a/search/include/pcl/search/search.h +++ b/search/include/pcl/search/search.h @@ -109,7 +109,7 @@ namespace pcl * Otherwise the results may be returned in any order. */ virtual bool - getSortedResults (); + getSortedResults () const; /** \brief Pass the input dataset that the search will be performed on. @@ -396,6 +396,97 @@ namespace pcl } } + /** + * \brief Removes indices and sqr distances, where sqr_distance == 0 + * \param k_indices Indices returned from \a nearestKSearch{,T} or \a + * radiusSearch{,T} + * \param k_sqr_distances Distances returned from \a nearestKSearch{,T} or \a + * radiusSearch{,T} + * \return std::size_t number of non trivial neighbors found + * \details Example usage: + * \code {.cpp} + * // First populate the two vectors + * nearestKSearch(point, k + 1, k_indices, k_sqr_distances); + * const auto num_neighbors = makeNonTrivial(k_indices, k_sqr_distances); + * \endcode + */ + std::size_t + makeNonTrivial(Indices& k_indices, std::vector& k_sqr_distances) const; + + /** + * \brief Removes indices and sqr distances, where sqr_distance == 0 + * \param k_indices Indices returned from \a nearestKSearch{,T} or \a + * radiusSearch{,T}, k_indices[i] corresponds to the neighbors of the query + * point i + * \param k_sqr_distances Distances returned from \a nearestKSearch{,T} or \a + * radiusSearch{,T}, k_sqr_distances[i] corresponds to the neighbors of the + * query point i + * \details Example usage: + * \code {.cpp} + * // First populate the two vectors + * nearestKSearch(point, k + 1, k_indices, k_sqr_distances); + * makeNonTrivial(k_indices, k_sqr_distances); + * \endcode + */ + void + makeNonTrivial(std::vector& k_indices, + std::vector>& k_sqr_distances) const + { + assert(k_indices.size() == k_sqr_distances.size ()); + for (std::size_t i = 0; i < k_indices.size(); ++i) { + makeNonTrivial(k_indices[i], k_sqr_distances[i]); + } + } + + /** + * \brief Removes indices and sqr distances, where k_indices[i] == index + * \param[in] index The index used in a previous search call + * \param k_indices Indices returned from \a nearestKSearch{,T} or \a + * radiusSearch{,T} + * \param k_sqr_distances Distances returned from \a nearestKSearch{,T} or \a + * radiusSearch{,T} + * \return std::size_t number of non trivial neighbors found + * \details Example usage: + * \code {.cpp} + * // First populate the two vectors + * nearestKSearch(point, k + 1, k_indices, k_sqr_distances); + * const auto num_neighbors = makeNonTrivial(k_indices, k_sqr_distances); + * \endcode + */ + std::size_t + makeNonTrivial(index_t index, + Indices& k_indices, + std::vector& k_sqr_distances) const; + + /** + * \brief Removes indices and sqr distances, where k_indices[i][j] == indices[i] + * \param[in] indices a vector of point cloud indices to query for nearest neighbors + * \param k_indices Indices returned from \a nearestKSearch{,T} or \a + * radiusSearch{,T}, k_indices[i] corresponds to the neighbors of the query + * point i + * \param k_sqr_distances Distances returned from \a nearestKSearch{,T} or \a + * radiusSearch{,T}, k_sqr_distances[i] corresponds to the neighbors of the + * query point i + * \details Example usage: + * \code {.cpp} + * // First populate the two vectors + * nearestKSearch(point, k + 1, k_indices, k_sqr_distances); + * makeNonTrivial(k_indices, k_sqr_distances); + * \endcode + */ + void + makeNonTrivial(const Indices& indices, + std::vector& k_indices, + std::vector>& k_sqr_distances) const + { + assert(indices.size() == k_indices.size()); + assert(k_indices.size() == k_sqr_distances.size()); + + for (std::size_t i = 0; i < k_indices.size(); ++i) { + makeNonTrivial(indices[i], k_indices[i], k_sqr_distances[i]); + } + } + protected: void sortResults (Indices& indices, std::vector& distances) const; @@ -421,7 +512,7 @@ namespace pcl const std::vector& distances_; }; - }; // class Search + }; // class Search } // namespace search } // namespace pcl diff --git a/test/search/test_search.cpp b/test/search/test_search.cpp index 59cdd319aed..743e18276c8 100644 --- a/test/search/test_search.cpp +++ b/test/search/test_search.cpp @@ -45,6 +45,7 @@ #include #include #include // for pcl::isFinite +#include // for EXPECT_EQ_VECTOR using namespace pcl; @@ -538,6 +539,96 @@ TEST (PCL, Organized_Sparse_View_Radius) } #endif +class MakeNonTrivialTests : public ::testing::TestWithParam { + void + SetUp() override + { + search = pcl::make_shared>(GetParam()); + } + +protected: + std::vector test_indices = {{1, 2, 3, 4, 5, 6, 7, 8, 9}, + {1, 1, 2, 3, 1, 1, 1, 4, 5}}; + std::vector> test_distances = {{0, 0, 0.0001, 2, 5, 0, 0, 0, 10}, + {1, 0, 0.0001, 2, 5, 0, 0, 0, 10}}; + + Indices test_seeds = {1, 9}; + + pcl::shared_ptr> search; +}; + +TEST_P(MakeNonTrivialTests, distance) +{ + std::vector expected_sorted_sizes = {7, 9}, + expected_unsorted_sizes = {4, 5}; + + auto& expected_sizes = + search->getSortedResults() ? expected_sorted_sizes : expected_unsorted_sizes; + + for (unsigned int i = 0; i < test_distances.size(); i++) { + auto indices = test_indices[i]; + auto distances = test_distances[i]; + auto size = search->makeNonTrivial(indices, distances); + + EXPECT_EQ(indices.size(), size); + EXPECT_EQ(distances.size(), size); + + EXPECT_LE(distances.size(), test_distances[i].size()); + EXPECT_EQ(size, expected_sizes[i]); + + if (distances.empty()) { + continue; + } + + if (search->getSortedResults()) { + EXPECT_TRUE(distances[0]); + } + else { + for (const auto& dist : distances) { + EXPECT_TRUE(dist); + } + } + } +} + +TEST_P(MakeNonTrivialTests, index) +{ + // vector of vectors, per seed + std::vector> expected_sorted_sizes = {{8, 7}, {9, 9}}, + expected_unsorted_sizes = {{8, 4}, {8, 9}}; + + auto& expected_sizes = + search->getSortedResults() ? expected_sorted_sizes : expected_unsorted_sizes; + + for (unsigned int seed_idx = 0; seed_idx < test_seeds.size(); seed_idx++) { + for (unsigned int i = 0; i < test_distances.size(); i++) { + auto indices = test_indices[i]; + auto distances = test_distances[i]; + auto size = search->makeNonTrivial(test_seeds[seed_idx], indices, distances); + + EXPECT_EQ(indices.size(), size); + EXPECT_EQ(distances.size(), size); + + EXPECT_LE(indices.size(), test_indices[i].size()); + EXPECT_EQ(size, expected_sizes[seed_idx][i]); + + if (distances.empty()) { + continue; + } + if (search->getSortedResults()) { + EXPECT_NE(indices[0], test_seeds[seed_idx]); + } + else { + for (const auto& idx : indices) { + EXPECT_NE(idx, test_seeds[seed_idx]); + } + } + } + } +} + +INSTANTIATE_TEST_SUITE_P(PCL, MakeNonTrivialTests, testing::Values(false, true)); + /** \brief create subset of point in cloud to use as query points * \param[out] query_indices resulting query indices - not guaranteed to have size of query_count but guaranteed not to exceed that value * \param cloud input cloud required to check for nans and to get number of points