Skip to content

Commit 8b645cc

Browse files
kunaltyagikunaltyagi
committed
Add makeNonTrivial helper in Search 
1 parent 7a3e484 commit 8b645cc

File tree

3 files changed

+268
-14
lines changed

3 files changed

+268
-14
lines changed

‎search/include/pcl/search/impl/search.hpp

Lines changed: 113 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -40,9 +40,11 @@
4040

4141
#include <pcl/search/search.h>
4242

43+
namespace pcl{
44+
namespace search {
4345
///////////////////////////////////////////////////////////////////////////////////////////
4446
template <typename PointT>
45-
pcl::search::Search<PointT>::Search (const std::string& name, bool sorted)
47+
Search<PointT>::Search (const std::string& name, bool sorted)
4648
: input_ ()
4749
, sorted_results_ (sorted)
4850
, name_ (name)
@@ -51,28 +53,28 @@ pcl::search::Search<PointT>::Search (const std::string& name, bool sorted)
5153

5254
///////////////////////////////////////////////////////////////////////////////////////////
5355
template <typename PointT> const std::string&
54-
pcl::search::Search<PointT>::getName () const
56+
Search<PointT>::getName () const
5557
{
5658
return (name_);
5759
}
5860

5961
///////////////////////////////////////////////////////////////////////////////////////////
6062
template <typename PointT> void
61-
pcl::search::Search<PointT>::setSortedResults (bool sorted)
63+
Search<PointT>::setSortedResults (bool sorted)
6264
{
6365
sorted_results_ = sorted;
6466
}
6567

6668
///////////////////////////////////////////////////////////////////////////////////////////
6769
template <typename PointT> bool
68-
pcl::search::Search<PointT>::getSortedResults ()
70+
Search<PointT>::getSortedResults () const
6971
{
7072
return (sorted_results_);
7173
}
7274

7375
///////////////////////////////////////////////////////////////////////////////////////////
7476
template <typename PointT> void
75-
pcl::search::Search<PointT>::setInputCloud (
77+
Search<PointT>::setInputCloud (
7678
const PointCloudConstPtr& cloud, const IndicesConstPtr &indices)
7779
{
7880
input_ = cloud;
@@ -82,7 +84,7 @@ pcl::search::Search<PointT>::setInputCloud (
8284

8385
///////////////////////////////////////////////////////////////////////////////////////////
8486
template <typename PointT> int
85-
pcl::search::Search<PointT>::nearestKSearch (
87+
Search<PointT>::nearestKSearch (
8688
const PointCloud &cloud, index_t index, int k,
8789
Indices &k_indices, std::vector<float> &k_sqr_distances) const
8890
{
@@ -92,7 +94,7 @@ pcl::search::Search<PointT>::nearestKSearch (
9294

9395
///////////////////////////////////////////////////////////////////////////////////////////
9496
template <typename PointT> int
95-
pcl::search::Search<PointT>::nearestKSearch (
97+
Search<PointT>::nearestKSearch (
9698
index_t index, int k,
9799
Indices &k_indices,
98100
std::vector<float> &k_sqr_distances) const
@@ -110,7 +112,7 @@ pcl::search::Search<PointT>::nearestKSearch (
110112

111113
///////////////////////////////////////////////////////////////////////////////////////////
112114
template <typename PointT> void
113-
pcl::search::Search<PointT>::nearestKSearch (
115+
Search<PointT>::nearestKSearch (
114116
const PointCloud& cloud, const Indices& indices,
115117
int k, std::vector<Indices>& k_indices,
116118
std::vector< std::vector<float> >& k_sqr_distances) const
@@ -133,7 +135,7 @@ pcl::search::Search<PointT>::nearestKSearch (
133135

134136
///////////////////////////////////////////////////////////////////////////////////////////
135137
template <typename PointT> int
136-
pcl::search::Search<PointT>::radiusSearch (
138+
Search<PointT>::radiusSearch (
137139
const PointCloud &cloud, index_t index, double radius,
138140
Indices &k_indices, std::vector<float> &k_sqr_distances,
139141
unsigned int max_nn) const
@@ -144,7 +146,7 @@ pcl::search::Search<PointT>::radiusSearch (
144146

145147
///////////////////////////////////////////////////////////////////////////////////////////
146148
template <typename PointT> int
147-
pcl::search::Search<PointT>::radiusSearch (
149+
Search<PointT>::radiusSearch (
148150
index_t index, double radius, Indices &k_indices,
149151
std::vector<float> &k_sqr_distances, unsigned int max_nn ) const
150152
{
@@ -159,7 +161,7 @@ pcl::search::Search<PointT>::radiusSearch (
159161

160162
///////////////////////////////////////////////////////////////////////////////////////////
161163
template <typename PointT> void
162-
pcl::search::Search<PointT>::radiusSearch (
164+
Search<PointT>::radiusSearch (
163165
const PointCloud& cloud,
164166
const Indices& indices,
165167
double radius,
@@ -185,7 +187,7 @@ pcl::search::Search<PointT>::radiusSearch (
185187

186188
///////////////////////////////////////////////////////////////////////////////////////////
187189
template <typename PointT> void
188-
pcl::search::Search<PointT>::sortResults (
190+
Search<PointT>::sortResults (
189191
Indices& indices, std::vector<float>& distances) const
190192
{
191193
Indices order (indices.size ());
@@ -205,6 +207,105 @@ pcl::search::Search<PointT>::sortResults (
205207
sort (distances.begin (), distances.end ());
206208
}
207209

210+
template <typename PointT>
211+
std::size_t
212+
Search<PointT>::makeNonTrivial(Indices& k_indices,
213+
std::vector<float>& k_sqr_distances) const
214+
{
215+
assert(k_indices.size() == k_sqr_distances.size());
216+
if (k_indices.empty()) {
217+
return 0;
218+
}
219+
220+
if (getSortedResults()) {
221+
std::size_t num_zero_elements = 0;
222+
// distances are sorted, so if we encounter a non-zero, we can bail early
223+
for (const auto& distance : k_sqr_distances) {
224+
if (distance) {
225+
break;
226+
}
227+
++num_zero_elements;
228+
}
229+
// We need to remove `num_zero_elements` initial elements
230+
k_indices.erase(k_indices.begin(), k_indices.begin() + num_zero_elements);
231+
k_sqr_distances.erase(k_sqr_distances.begin(),
232+
k_sqr_distances.begin() + num_zero_elements);
233+
}
234+
else {
235+
const auto zero_distance_it =
236+
std::find(k_sqr_distances.begin(), k_sqr_distances.end(), 0);
237+
const auto zero_distance_idx =
238+
std::distance(k_sqr_distances.begin(), zero_distance_it);
239+
// From zero_distance_idx, we start removing elements
240+
std::size_t last_good_idx = zero_distance_idx - 1;
241+
for (std::size_t i = zero_distance_idx + 1; i < k_sqr_distances.size(); ++i) {
242+
if (k_sqr_distances[i] == 0) {
243+
continue;
244+
}
245+
++last_good_idx;
246+
k_sqr_distances[last_good_idx] = k_sqr_distances[i];
247+
k_indices[last_good_idx] = k_indices[i];
248+
}
249+
// We need to remove elements after `last_good_idx`
250+
const auto new_end = last_good_idx + 1;
251+
k_indices.erase(k_indices.begin() + new_end, k_indices.end());
252+
k_sqr_distances.erase(k_sqr_distances.begin() + new_end, k_sqr_distances.end());
253+
}
254+
assert(k_indices.size() == k_sqr_distances.size());
255+
return k_indices.size();
256+
}
257+
258+
template <typename PointT>
259+
std::size_t
260+
Search<PointT>::makeNonTrivial(index_t index,
261+
Indices& k_indices,
262+
std::vector<float>& k_sqr_distances) const
263+
{
264+
assert(k_indices.size() == k_sqr_distances.size());
265+
if (k_indices.empty()) {
266+
return 0;
267+
}
268+
269+
if (getSortedResults()) {
270+
std::size_t same_idx_elements = 0;
271+
// distances are sorted, so if we encounter a non-zero, we can bail early
272+
for (const auto& idx : k_indices) {
273+
if (idx == index) {
274+
break;
275+
}
276+
++same_idx_elements;
277+
}
278+
// We need to remove `same_idx_elements` initial elements
279+
k_indices.erase(k_indices.begin(), k_indices.begin() + same_idx_elements);
280+
k_sqr_distances.erase(k_sqr_distances.begin(),
281+
k_sqr_distances.begin() + same_idx_elements);
282+
}
283+
else {
284+
const auto same_idx_it =
285+
std::find(k_indices.begin(), k_indices.end(), index);
286+
const auto same_idx = std::distance(k_indices.begin(), same_idx_it);
287+
// From same_idx, we start removing elements
288+
std::size_t last_good_idx = same_idx - 1;
289+
for (std::size_t i = same_idx + 1; i < k_indices.size(); ++i) {
290+
if (k_indices[i] == index) {
291+
continue;
292+
}
293+
++last_good_idx;
294+
k_sqr_distances[last_good_idx] = k_sqr_distances[i];
295+
k_indices[last_good_idx] = k_indices[i];
296+
}
297+
// We need to remove elements after `last_good_idx`
298+
const auto new_end = last_good_idx + 1;
299+
k_indices.erase(k_indices.begin() + new_end, k_indices.end());
300+
k_sqr_distances.erase(k_sqr_distances.begin() + new_end, k_sqr_distances.end());
301+
}
302+
303+
assert(k_indices.size() == k_sqr_distances.size());
304+
return k_indices.size();
305+
}
306+
} // namespace search
307+
} // namespace pcl
308+
208309
#define PCL_INSTANTIATE_Search(T) template class PCL_EXPORTS pcl::search::Search<T>;
209310

210311
#endif //#ifndef _PCL_SEARCH_SEARCH_IMPL_HPP_

‎search/include/pcl/search/search.h

Lines changed: 93 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -109,7 +109,7 @@ namespace pcl
109109
* Otherwise the results may be returned in any order.
110110
*/
111111
virtual bool
112-
getSortedResults ();
112+
getSortedResults () const;
113113

114114

115115
/** \brief Pass the input dataset that the search will be performed on.
@@ -396,6 +396,97 @@ namespace pcl
396396
}
397397
}
398398

399+
/**
400+
* \brief Removes indices and sqr distances, where sqr_distance == 0
401+
* \param k_indices Indices returned from \a nearestKSearch{,T} or \a
402+
* radiusSearch{,T}
403+
* \param k_sqr_distances Distances returned from \a nearestKSearch{,T} or \a
404+
* radiusSearch{,T}
405+
* \return std::size_t number of non trivial neighbors found
406+
* \details Example usage:
407+
* \code {.cpp}
408+
* // First populate the two vectors
409+
* nearestKSearch(point, k + 1, k_indices, k_sqr_distances);
410+
* const auto num_neighbors = makeNonTrivial(k_indices, k_sqr_distances);
411+
* \endcode
412+
*/
413+
std::size_t
414+
makeNonTrivial(Indices& k_indices, std::vector<float>& k_sqr_distances) const;
415+
416+
/**
417+
* \brief Removes indices and sqr distances, where sqr_distance == 0
418+
* \param k_indices Indices returned from \a nearestKSearch{,T} or \a
419+
* radiusSearch{,T}, k_indices[i] corresponds to the neighbors of the query
420+
* point i
421+
* \param k_sqr_distances Distances returned from \a nearestKSearch{,T} or \a
422+
* radiusSearch{,T}, k_sqr_distances[i] corresponds to the neighbors of the
423+
* query point i
424+
* \details Example usage:
425+
* \code {.cpp}
426+
* // First populate the two vectors
427+
* nearestKSearch(point, k + 1, k_indices, k_sqr_distances);
428+
* makeNonTrivial(k_indices, k_sqr_distances);
429+
* \endcode
430+
*/
431+
void
432+
makeNonTrivial(std::vector<Indices>& k_indices,
433+
std::vector<std::vector<float>>& k_sqr_distances) const
434+
{
435+
assert(k_indices.size() == k_sqr_distances.size ());
436+
for (std::size_t i = 0; i < k_indices.size(); ++i) {
437+
makeNonTrivial(k_indices[i], k_sqr_distances[i]);
438+
}
439+
}
440+
441+
/**
442+
* \brief Removes indices and sqr distances, where k_indices[i] == index
443+
* \param[in] index The index used in a previous search call
444+
* \param k_indices Indices returned from \a nearestKSearch{,T} or \a
445+
* radiusSearch{,T}
446+
* \param k_sqr_distances Distances returned from \a nearestKSearch{,T} or \a
447+
* radiusSearch{,T}
448+
* \return std::size_t number of non trivial neighbors found
449+
* \details Example usage:
450+
* \code {.cpp}
451+
* // First populate the two vectors
452+
* nearestKSearch(point, k + 1, k_indices, k_sqr_distances);
453+
* const auto num_neighbors = makeNonTrivial(k_indices, k_sqr_distances);
454+
* \endcode
455+
*/
456+
std::size_t
457+
makeNonTrivial(index_t index,
458+
Indices& k_indices,
459+
std::vector<float>& k_sqr_distances) const;
460+
461+
/**
462+
* \brief Removes indices and sqr distances, where k_indices[i][j] == indices[i]
463+
* \param[in] indices a vector of point cloud indices to query for nearest neighbors
464+
* \param k_indices Indices returned from \a nearestKSearch{,T} or \a
465+
* radiusSearch{,T}, k_indices[i] corresponds to the neighbors of the query
466+
* point i
467+
* \param k_sqr_distances Distances returned from \a nearestKSearch{,T} or \a
468+
* radiusSearch{,T}, k_sqr_distances[i] corresponds to the neighbors of the
469+
* query point i
470+
* \details Example usage:
471+
* \code {.cpp}
472+
* // First populate the two vectors
473+
* nearestKSearch(point, k + 1, k_indices, k_sqr_distances);
474+
* makeNonTrivial(k_indices, k_sqr_distances);
475+
* \endcode
476+
*/
477+
void
478+
makeNonTrivial(const Indices& indices,
479+
std::vector<Indices>& k_indices,
480+
std::vector<std::vector<float>>& k_sqr_distances) const
481+
{
482+
assert(indices.size() == k_indices.size());
483+
assert(k_indices.size() == k_sqr_distances.size());
484+
485+
for (std::size_t i = 0; i < k_indices.size(); ++i) {
486+
makeNonTrivial(indices[i], k_indices[i], k_sqr_distances[i]);
487+
}
488+
}
489+
399490
protected:
400491
void
401492
sortResults (Indices& indices, std::vector<float>& distances) const;
@@ -421,7 +512,7 @@ namespace pcl
421512

422513
const std::vector<float>& distances_;
423514
};
424-
}; // class Search
515+
}; // class Search
425516
} // namespace search
426517
} // namespace pcl
427518

‎test/search/test_search.cpp

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -538,6 +538,68 @@ TEST (PCL, Organized_Sparse_View_Radius)
538538
}
539539
#endif
540540

541+
TEST(PCL, Search_nonTrivialDistance)
542+
{
543+
Indices test_indices = {1, 2, 3, 4, 5, 6, 7, 8, 9};
544+
// 0 added on the end to showcase this case with sorted vs unsorted
545+
std::vector<float> test_distances = {0, 0, 0.0001, 2, 5, 0, 0, 0, 10};
546+
547+
// on sorted
548+
{
549+
pcl::search::BruteForce<pcl::PointXYZ> sorted{true};
550+
auto indices = test_indices;
551+
auto distances = test_distances;
552+
sorted.makeNonTrivial(indices, distances);
553+
554+
EXPECT_EQ(distances.size(), 7);
555+
EXPECT_EQ_VECTOR(indices, Indices{3, 4, 5, 6, 7, 8, 9});
556+
}
557+
558+
// on unsorted
559+
{
560+
pcl::search::BruteForce<pcl::PointXYZ> unsorted{false};
561+
auto indices = test_indices;
562+
auto distances = test_distances;
563+
unsorted.makeNonTrivial(indices, distances);
564+
565+
EXPECT_EQ(distances.size(), 4);
566+
EXPECT_EQ_VECTOR(indices, Indices{3, 4, 5, 9});
567+
}
568+
}
569+
// @TODO
570+
TEST(PCL, Search_nonTrivialDistances) {}
571+
TEST(PCL, Search_nonTrivialIdx)
572+
{
573+
index_t test_idx = 1;
574+
Indices test_indices = {1, 1, 2, 3, 1, 1, 1, 4, 5};
575+
// 0 added on the end to showcase this case with sorted vs unsorted
576+
std::vector<float> test_distances = {0, 0, 0.0001, 2, 5, 0, 0, 0, 10};
577+
578+
// on sorted
579+
{
580+
pcl::search::BruteForce<pcl::PointXYZ> sorted{true};
581+
auto indices = test_indices;
582+
auto distances = test_distances;
583+
sorted.makeNonTrivial(test_idx, indices, distances);
584+
585+
EXPECT_EQ(distances.size(), 7);
586+
EXPECT_EQ_VECTOR(indices, Indices{2, 3, 1, 1, 1, 4, 5});
587+
}
588+
589+
// on unsorted
590+
{
591+
pcl::search::BruteForce<pcl::PointXYZ> unsorted{false};
592+
auto indices = test_indices;
593+
auto distances = test_distances;
594+
unsorted.makeNonTrivial(test_idx, indices, distances);
595+
596+
EXPECT_EQ(distances.size(), 4);
597+
EXPECT_EQ_VECTOR(indices, Indices{2, 3, 4, 5});
598+
}
599+
}
600+
// @TODO
601+
TEST (PCL, Search_nonTrivialIdices) {}
602+
541603
/** \brief create subset of point in cloud to use as query points
542604
* \param[out] query_indices resulting query indices - not guaranteed to have size of query_count but guaranteed not to exceed that value
543605
* \param cloud input cloud required to check for nans and to get number of points

0 commit comments

Comments
 (0)