1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the copyright holder(s) nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 *
35 */
36 /** \author Julius Kammerl (julius@kammerl.de)*/
37
38 #include <pcl/test/gtest.h>
39
40 #include <vector>
41
42
43 #include <pcl/common/time.h>
44 #include <pcl/point_cloud.h>
45 #include <pcl/point_types.h>
46 #include <pcl/search/organized.h> // for OrganizedNeighbor
47
48 using namespace pcl;
49
50 // helper class for priority queue
51 class prioPointQueueEntry
52 {
53 public:
prioPointQueueEntry()54 prioPointQueueEntry ()
55 {
56 }
prioPointQueueEntry(PointXYZ & point_arg,double pointDistance_arg,int pointIdx_arg)57 prioPointQueueEntry (PointXYZ& point_arg, double pointDistance_arg, int pointIdx_arg)
58 {
59 point_ = point_arg;
60 pointDistance_ = pointDistance_arg;
61 pointIdx_ = pointIdx_arg;
62 }
63
64 bool
operator <(const prioPointQueueEntry & rhs_arg) const65 operator< (const prioPointQueueEntry& rhs_arg) const
66 {
67 return (this->pointDistance_ < rhs_arg.pointDistance_);
68 }
69
70 PointXYZ point_;
71 double pointDistance_;int pointIdx_;
72
73 };
74
TEST(PCL,Organized_Neighbor_Pointcloud_Nearest_K_Neighbour_Search)75 TEST (PCL, Organized_Neighbor_Pointcloud_Nearest_K_Neighbour_Search)
76 {
77 constexpr unsigned int test_runs = 2;
78
79 // instantiate point cloud
80 PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
81
82 const unsigned int seed = time (nullptr);
83 srand (seed);
84 SCOPED_TRACE("seed=" + std::to_string(seed));
85
86 // create organized search
87 search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch;
88
89 pcl::Indices k_indices;
90 std::vector<float> k_sqr_distances;
91
92 std::vector<int> k_indices_bruteforce;
93 std::vector<float> k_sqr_distances_bruteforce;
94
95 // typical focal length from kinect
96 constexpr double oneOverFocalLength = 0.0018;
97
98 for (unsigned int test_id = 0; test_id < test_runs; test_id++)
99 {
100 // define a random search point
101
102 const unsigned int K = (rand () % 10)+1;
103
104 // generate point cloud
105 cloudIn->width = 128;
106 cloudIn->height = 32;
107 cloudIn->points.clear();
108 cloudIn->points.reserve (cloudIn->width * cloudIn->height);
109
110 int centerX = cloudIn->width >> 1;
111 int centerY = cloudIn->height >> 1;
112
113 for (int ypos = -centerY; ypos < centerY; ypos++)
114 for (int xpos = -centerX; xpos < centerX; xpos++)
115 {
116 double z = 15.0 * (double (rand ()) / double (RAND_MAX+1.0))+20;
117 double y = ypos * oneOverFocalLength * z;
118 double x = xpos * oneOverFocalLength * z;
119
120 cloudIn->points.emplace_back(float (x), float (y), float (z));
121 }
122
123 unsigned int searchIdx = rand()%(cloudIn->width * cloudIn->height);
124 const PointXYZ& searchPoint = (*cloudIn)[searchIdx];
125
126 k_indices.clear();
127 k_sqr_distances.clear();
128
129 // organized nearest neighbor search
130 organizedNeighborSearch.setInputCloud (cloudIn);
131 organizedNeighborSearch.nearestKSearch (searchPoint, int (K), k_indices, k_sqr_distances);
132
133 k_indices_bruteforce.clear();
134 k_sqr_distances_bruteforce.clear();
135
136 std::priority_queue<prioPointQueueEntry, std::vector<prioPointQueueEntry, Eigen::aligned_allocator<prioPointQueueEntry> > > pointCandidates;
137
138
139 // push all points and their distance to the search point into a priority queue - bruteforce approach.
140 for (std::size_t i = 0; i < cloudIn->size (); i++)
141 {
142 double pointDist = (((*cloudIn)[i].x - searchPoint.x) * ((*cloudIn)[i].x - searchPoint.x) +
143 ((*cloudIn)[i].y - searchPoint.y) * ((*cloudIn)[i].y - searchPoint.y) +
144 ((*cloudIn)[i].z - searchPoint.z) * ((*cloudIn)[i].z - searchPoint.z));
145
146 prioPointQueueEntry pointEntry ((*cloudIn)[i], pointDist, int (i));
147
148 pointCandidates.push (pointEntry);
149 }
150
151 // pop priority queue until we have the nearest K elements
152 while (pointCandidates.size () > K)
153 pointCandidates.pop ();
154
155 // copy results into vectors
156 while (!pointCandidates.empty ())
157 {
158 k_indices_bruteforce.push_back (pointCandidates.top ().pointIdx_);
159 k_sqr_distances_bruteforce.push_back (float (pointCandidates.top ().pointDistance_));
160
161 pointCandidates.pop ();
162 }
163
164
165 ASSERT_EQ ( k_indices.size() , k_indices_bruteforce.size() );
166
167 // compare nearest neighbor results of organized search with bruteforce search
168 for (std::size_t i = 0; i < k_indices.size (); i++)
169 {
170 ASSERT_EQ ( k_indices[i] , k_indices_bruteforce.back() );
171 EXPECT_NEAR (k_sqr_distances[i], k_sqr_distances_bruteforce.back(), 1e-4);
172
173 k_indices_bruteforce.pop_back();
174 k_sqr_distances_bruteforce.pop_back();
175 }
176
177 }
178
179 }
180
TEST(PCL,Organized_Neighbor_Pointcloud_Neighbours_Within_Radius_Search)181 TEST (PCL, Organized_Neighbor_Pointcloud_Neighbours_Within_Radius_Search)
182 {
183 constexpr unsigned int test_runs = 10;
184
185 const unsigned int seed = time (nullptr);
186 srand (seed);
187 SCOPED_TRACE("seed=" + std::to_string(seed));
188
189 search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch;
190
191 // typical focal length from kinect
192 constexpr double oneOverFocalLength = 0.0018;
193
194 for (unsigned int test_id = 0; test_id < test_runs; test_id++)
195 {
196 // generate point cloud
197 PointCloud<PointXYZ>::Ptr cloudIn (new PointCloud<PointXYZ> ());
198
199 cloudIn->width = 640;
200 cloudIn->height = 480;
201 cloudIn->points.clear();
202 cloudIn->points.resize (cloudIn->width * cloudIn->height);
203
204 int centerX = cloudIn->width >> 1;
205 int centerY = cloudIn->height >> 1;
206
207 int idx = 0;
208 for (int ypos = -centerY; ypos < centerY; ypos++)
209 for (int xpos = -centerX; xpos < centerX; xpos++)
210 {
211 double z = 5.0 * ( (double (rand ()) / double (RAND_MAX)))+5;
212 double y = ypos*oneOverFocalLength*z;
213 double x = xpos*oneOverFocalLength*z;
214
215 (*cloudIn)[idx++]= PointXYZ (float (x), float (y), float (z));
216 }
217
218 unsigned int randomIdx = rand()%(cloudIn->width * cloudIn->height);
219
220 const PointXYZ& searchPoint = (*cloudIn)[randomIdx];
221
222 double pointDist;
223 double searchRadius = 1.0 * (double (rand ()) / double (RAND_MAX));
224
225 // bruteforce radius search
226 std::vector<int> cloudSearchBruteforce;
227 cloudSearchBruteforce.clear();
228
229 for (std::size_t i = 0; i < cloudIn->size (); i++)
230 {
231 pointDist = sqrt (
232 ((*cloudIn)[i].x - searchPoint.x) * ((*cloudIn)[i].x - searchPoint.x)
233 + ((*cloudIn)[i].y - searchPoint.y) * ((*cloudIn)[i].y - searchPoint.y)
234 + ((*cloudIn)[i].z - searchPoint.z) * ((*cloudIn)[i].z - searchPoint.z));
235
236 if (pointDist <= searchRadius)
237 {
238 // add point candidates to vector list
239 cloudSearchBruteforce.push_back (int (i));
240 }
241 }
242
243 pcl::Indices cloudNWRSearch;
244 std::vector<float> cloudNWRRadius;
245
246 // execute organized search
247 organizedNeighborSearch.setInputCloud (cloudIn);
248 organizedNeighborSearch.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius);
249
250 // check if result from organized radius search can be also found in bruteforce search
251 for (const auto& current : cloudNWRSearch)
252 {
253 pointDist = sqrt (
254 ((*cloudIn)[current].x-searchPoint.x) * ((*cloudIn)[current].x-searchPoint.x) +
255 ((*cloudIn)[current].y-searchPoint.y) * ((*cloudIn)[current].y-searchPoint.y) +
256 ((*cloudIn)[current].z-searchPoint.z) * ((*cloudIn)[current].z-searchPoint.z)
257 );
258
259 ASSERT_LE (pointDist, searchRadius);
260 }
261
262
263 // check if bruteforce result from organized radius search can be also found in bruteforce search
264 for (const auto& current : cloudSearchBruteforce)
265 {
266 pointDist = sqrt (
267 ((*cloudIn)[current].x-searchPoint.x) * ((*cloudIn)[current].x-searchPoint.x) +
268 ((*cloudIn)[current].y-searchPoint.y) * ((*cloudIn)[current].y-searchPoint.y) +
269 ((*cloudIn)[current].z-searchPoint.z) * ((*cloudIn)[current].z-searchPoint.z)
270 );
271
272 ASSERT_LE (pointDist, searchRadius);
273 }
274
275 ASSERT_EQ (cloudNWRRadius.size() , cloudSearchBruteforce.size ());
276
277 // check if result limitation works
278 organizedNeighborSearch.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5);
279
280 ASSERT_LE (cloudNWRRadius.size (), 5);
281 }
282 }
283
284 /* ---[ */
285 int
main(int argc,char ** argv)286 main (int argc, char** argv)
287 {
288 testing::InitGoogleTest (&argc, argv);
289 return (RUN_ALL_TESTS ());
290 }
291 /* ]--- */
292