1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009-2012, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id: test_filters.cpp 7683 2012-10-23 02:49:03Z rusu $
38 *
39 */
40
41 #include <pcl/test/gtest.h>
42 #include <pcl/point_types.h>
43 #include <pcl/io/pcd_io.h>
44 #include <pcl/features/normal_3d.h>
45 #include <pcl/filters/covariance_sampling.h>
46 #include <pcl/filters/normal_space.h>
47 #include <pcl/filters/random_sample.h>
48
49
50 #include <pcl/common/transforms.h>
51
52 using namespace pcl;
53
54 PointCloud<PointXYZ>::Ptr cloud_walls (new PointCloud<PointXYZ> ()),
55 cloud_turtle (new PointCloud<PointXYZ> ());
56 PointCloud<PointNormal>::Ptr cloud_walls_normals (new PointCloud<PointNormal> ()),
57 cloud_turtle_normals (new PointCloud<PointNormal> ());
58
59
60 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST(CovarianceSampling,Filters)61 TEST (CovarianceSampling, Filters)
62 {
63 CovarianceSampling<PointNormal, PointNormal> covariance_sampling;
64 covariance_sampling.setInputCloud (cloud_walls_normals);
65 covariance_sampling.setNormals (cloud_walls_normals);
66 covariance_sampling.setNumberOfSamples (static_cast<unsigned int> (cloud_walls_normals->size ()) / 4);
67 double cond_num_walls = covariance_sampling.computeConditionNumber ();
68
69 // Conditioning number should be loosely close to the expected number. Adopting 10% of the reference value
70 EXPECT_NEAR (113.29773, cond_num_walls, 10.);
71
72 IndicesPtr walls_indices (new pcl::Indices ());
73 covariance_sampling.filter (*walls_indices);
74 covariance_sampling.setIndices (walls_indices);
75 double cond_num_walls_sampled = covariance_sampling.computeConditionNumber ();
76
77 // Conditioning number should be loosely close to the expected number. Adopting 10% of the reference value
78 EXPECT_NEAR (22.11506, cond_num_walls_sampled, 2.);
79
80 // Ensure it respects the requested sampling size
81 EXPECT_EQ (static_cast<unsigned int> (cloud_walls_normals->size ()) / 4, walls_indices->size ());
82
83 covariance_sampling.setInputCloud (cloud_turtle_normals);
84 covariance_sampling.setNormals (cloud_turtle_normals);
85 covariance_sampling.setIndices (IndicesPtr ());
86 covariance_sampling.setNumberOfSamples (static_cast<unsigned int> (cloud_turtle_normals->size ()) / 8);
87 double cond_num_turtle = covariance_sampling.computeConditionNumber ();
88
89 // Conditioning number should be loosely close to the expected number. Adopting 10% of the reference value
90 EXPECT_NEAR (cond_num_turtle, 20661.7663, 2e4);
91
92 IndicesPtr turtle_indices (new pcl::Indices ());
93 covariance_sampling.filter (*turtle_indices);
94 covariance_sampling.setIndices (turtle_indices);
95 double cond_num_turtle_sampled = covariance_sampling.computeConditionNumber ();
96
97 // Conditioning number should be loosely close to the expected number. Adopting 10% of the reference value
98 EXPECT_NEAR (cond_num_turtle_sampled, 5795.5057, 5e3);
99
100 // Ensure it respects the requested sampling size
101 EXPECT_EQ (static_cast<unsigned int> (cloud_turtle_normals->size ()) / 8, turtle_indices->size ());
102 }
103
104 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST(NormalSpaceSampling,Filters)105 TEST (NormalSpaceSampling, Filters)
106 {
107 // pcl::Normal is not precompiled by default so we use PointNormal
108 auto cloud = pcl::make_shared<PointCloud<PointNormal>> ();
109 // generate 16 points (8 unique) with unit norm
110 cloud->reserve (16);
111 // ensure that the normals have unit norm
112 const auto value = ::sqrt(1/3.f);
113 for (int unique = 0; unique < 8; ++unique) {
114 const auto i = ((unique % 2) < 1) ? -1 : 1; // points alternate sign
115 const auto j = ((unique % 4) < 2) ? -1 : 1; // 2 points negative, 2 positive
116 const auto k = ((unique % 8) < 4) ? -1 : 1; // first 4 points negative, rest positive
117 for (int duplicate = 0; duplicate < 2; ++duplicate) {
118 cloud->emplace_back (0.f, 0.f, 0.f, i * value, j * value, k * value);
119 }
120 }
121
122 NormalSpaceSampling<PointNormal, PointNormal> normal_space_sampling;
123 normal_space_sampling.setInputCloud (cloud);
124 normal_space_sampling.setNormals (cloud);
125 normal_space_sampling.setBins (2, 2, 2);
126 normal_space_sampling.setSeed (0);
127 normal_space_sampling.setSample (8);
128
129 IndicesPtr walls_indices = pcl::make_shared<Indices> ();
130 normal_space_sampling.filter (*walls_indices);
131
132 // The orientation space of the normals is divided into 2x2x2 buckets
133 // points are samples arbitrarily from each bucket in succession until the
134 // requested number of samples is met. This means we expect to see only one index
135 // for every two elements in the original array e.g. 0, 3, 4, 6, etc...
136 // if 0 is sampled, index 1 can no longer be there and so forth
137 std::array<std::set<index_t>, 8> buckets;
138 for (const auto index : *walls_indices)
139 buckets[index/2].insert (index);
140
141
142 EXPECT_EQ (8u, walls_indices->size ());
143 for (const auto& bucket : buckets)
144 EXPECT_EQ (1u, bucket.size ());
145 }
146
147 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST(RandomSample,Filters)148 TEST (RandomSample, Filters)
149 {
150 // Test the PointCloud<PointT> method
151 // Randomly sample 10 points from cloud
152 RandomSample<PointXYZ> sample (true); // Extract removed indices
153 sample.setInputCloud (cloud_walls);
154 sample.setSample (10);
155
156 // Indices
157 pcl::Indices indices;
158 sample.filter (indices);
159
160 EXPECT_EQ (int (indices.size ()), 10);
161
162 // Cloud
163 PointCloud<PointXYZ> cloud_out;
164 sample.filter(cloud_out);
165
166 EXPECT_EQ (int (cloud_out.width), 10);
167 EXPECT_EQ (int (indices.size ()), int (cloud_out.size ()));
168
169 for (std::size_t i = 0; i < indices.size () - 1; ++i)
170 {
171 // Check that indices are sorted
172 EXPECT_LT (indices[i], indices[i+1]);
173 // Compare original points with sampled indices against sampled points
174 EXPECT_NEAR ((*cloud_walls)[indices[i]].x, cloud_out[i].x, 1e-4);
175 EXPECT_NEAR ((*cloud_walls)[indices[i]].y, cloud_out[i].y, 1e-4);
176 EXPECT_NEAR ((*cloud_walls)[indices[i]].z, cloud_out[i].z, 1e-4);
177 }
178
179 IndicesConstPtr removed = sample.getRemovedIndices ();
180 EXPECT_EQ (removed->size (), cloud_walls->size () - 10);
181 // Organized
182 // (sdmiller) Removing for now, to debug the Linux 32-bit segfault offline
183 sample.setKeepOrganized (true);
184 sample.filter(cloud_out);
185 removed = sample.getRemovedIndices ();
186 EXPECT_EQ (int (removed->size ()), cloud_walls->size () - 10);
187 for (std::size_t i = 0; i < removed->size (); ++i)
188 {
189 EXPECT_TRUE (std::isnan (cloud_out.at ((*removed)[i]).x));
190 EXPECT_TRUE (std::isnan (cloud_out.at ((*removed)[i]).y));
191 EXPECT_TRUE (std::isnan (cloud_out.at ((*removed)[i]).z));
192 }
193
194 EXPECT_EQ (cloud_out.width, cloud_walls->width);
195 EXPECT_EQ (cloud_out.height, cloud_walls->height);
196 // Negative
197 sample.setKeepOrganized (false);
198 sample.setNegative (true);
199 sample.filter(cloud_out);
200 removed = sample.getRemovedIndices ();
201 EXPECT_EQ (int (removed->size ()), 10);
202 EXPECT_EQ (int (cloud_out.size ()), int (cloud_walls->size () - 10));
203
204 // Make sure sampling >N works
205 sample.setSample (static_cast<unsigned int> (cloud_walls->size ()+10));
206 sample.setNegative (false);
207 sample.filter (cloud_out);
208 EXPECT_EQ (cloud_out.size (), cloud_walls->size ());
209 removed = sample.getRemovedIndices ();
210 EXPECT_TRUE (removed->empty ());
211
212 // Test the pcl::PCLPointCloud2 method
213 // Randomly sample 10 points from cloud
214 pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2 ());
215 toPCLPointCloud2 (*cloud_walls, *cloud_blob);
216 RandomSample<pcl::PCLPointCloud2> sample2;
217 sample2.setInputCloud (cloud_blob);
218 sample2.setSample (10);
219
220 // Indices
221 pcl::Indices indices2;
222 sample2.filter (indices2);
223
224 EXPECT_EQ (int (indices2.size ()), 10);
225
226 // Cloud
227 pcl::PCLPointCloud2 output_blob;
228 sample2.filter (output_blob);
229
230 fromPCLPointCloud2 (output_blob, cloud_out);
231
232 EXPECT_EQ (int (cloud_out.width), 10);
233 EXPECT_EQ (int (indices2.size ()), int (cloud_out.size ()));
234
235 for (std::size_t i = 0; i < indices2.size () - 1; ++i)
236 {
237 // Check that indices are sorted
238 EXPECT_LT (indices2[i], indices2[i+1]);
239 // Compare original points with sampled indices against sampled points
240 EXPECT_NEAR ((*cloud_walls)[indices2[i]].x, cloud_out[i].x, 1e-4);
241 EXPECT_NEAR ((*cloud_walls)[indices2[i]].y, cloud_out[i].y, 1e-4);
242 EXPECT_NEAR ((*cloud_walls)[indices2[i]].z, cloud_out[i].z, 1e-4);
243 }
244 }
245
246
247 /* ---[ */
248 int
main(int argc,char ** argv)249 main (int argc, char** argv)
250 {
251 // Load two standard PCD files from disk
252 if (argc < 3)
253 {
254 std::cerr << "No test files given. Please download `sac_plane_test.pcd` and 'cturtle.pcd' and pass them path to the test." << std::endl;
255 return (-1);
256 }
257
258 // Load in the point clouds
259 io::loadPCDFile (argv[1], *cloud_walls);
260 io::loadPCDFile (argv[2], *cloud_turtle);
261
262
263
264 // Compute the normals for each cloud, and then clean them up of any NaN values
265 NormalEstimation<PointXYZ,PointNormal> ne;
266 ne.setInputCloud (cloud_walls);
267 ne.setRadiusSearch (0.05);
268 ne.compute (*cloud_walls_normals);
269 copyPointCloud (*cloud_walls, *cloud_walls_normals);
270
271 pcl::Indices aux_indices;
272 removeNaNFromPointCloud (*cloud_walls_normals, *cloud_walls_normals, aux_indices);
273 removeNaNNormalsFromPointCloud (*cloud_walls_normals, *cloud_walls_normals, aux_indices);
274
275 ne = NormalEstimation<PointXYZ, PointNormal> ();
276 ne.setInputCloud (cloud_turtle);
277 ne.setKSearch (5);
278 ne.compute (*cloud_turtle_normals);
279 copyPointCloud (*cloud_turtle, *cloud_turtle_normals);
280 removeNaNFromPointCloud (*cloud_turtle_normals, *cloud_turtle_normals, aux_indices);
281 removeNaNNormalsFromPointCloud (*cloud_turtle_normals, *cloud_turtle_normals, aux_indices);
282
283 testing::InitGoogleTest (&argc, argv);
284 return (RUN_ALL_TESTS ());
285 }
286 /* ]--- */
287