00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 #include <gtest/gtest.h>
00042 #include <pcl/point_types.h>
00043 #include <pcl/io/pcd_io.h>
00044 #include <pcl/features/normal_3d.h>
00045 #include <pcl/filters/filter.h>
00046 #include <pcl/filters/passthrough.h>
00047 #include <pcl/filters/shadowpoints.h>
00048 #include <pcl/filters/frustum_culling.h>
00049 #include <pcl/filters/sampling_surface_normal.h>
00050 #include <pcl/filters/voxel_grid.h>
00051 #include <pcl/filters/voxel_grid_covariance.h>
00052 #include <pcl/filters/extract_indices.h>
00053 #include <pcl/filters/project_inliers.h>
00054 #include <pcl/filters/radius_outlier_removal.h>
00055 #include <pcl/filters/statistical_outlier_removal.h>
00056 #include <pcl/filters/conditional_removal.h>
00057 #include <pcl/filters/crop_box.h>
00058 #include <pcl/filters/median_filter.h>
00059 #include <pcl/filters/normal_refinement.h>
00060
00061 #include <pcl/common/transforms.h>
00062 #include <pcl/common/eigen.h>
00063
00064 #include <pcl/segmentation/sac_segmentation.h>
00065
00066 using namespace pcl;
00067 using namespace pcl::io;
00068 using namespace std;
00069 using namespace Eigen;
00070
00071
00072 PCLPointCloud2::Ptr cloud_blob (new PCLPointCloud2);
00073 PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
00074 vector<int> indices_;
00075
00076 PointCloud<PointXYZRGB>::Ptr cloud_organized (new PointCloud<PointXYZRGB>);
00077
00078
00079
00080
00082 TEST (ExtractIndicesSelf, Filters)
00083 {
00084
00085 ExtractIndices<PointXYZ> ei;
00086 boost::shared_ptr<vector<int> > indices (new vector<int> (2));
00087 (*indices)[0] = 0;
00088 (*indices)[1] = static_cast<int> (cloud->points.size ()) - 1;
00089
00090 PointCloud<PointXYZ>::Ptr output (new PointCloud<PointXYZ>);
00091 ei.setInputCloud (cloud);
00092 ei.setIndices (indices);
00093 ei.filter (*output);
00094
00095 EXPECT_EQ (int (output->points.size ()), 2);
00096 EXPECT_EQ (int (output->width), 2);
00097 EXPECT_EQ (int (output->height), 1);
00098
00099 EXPECT_EQ (cloud->points[0].x, output->points[0].x);
00100 EXPECT_EQ (cloud->points[0].y, output->points[0].y);
00101 EXPECT_EQ (cloud->points[0].z, output->points[0].z);
00102
00103 EXPECT_EQ (cloud->points[cloud->points.size () - 1].x, output->points[1].x);
00104 EXPECT_EQ (cloud->points[cloud->points.size () - 1].y, output->points[1].y);
00105 EXPECT_EQ (cloud->points[cloud->points.size () - 1].z, output->points[1].z);
00106 }
00107
00109 TEST (ExtractIndices, Filters)
00110 {
00111
00112 ExtractIndices<PointXYZ> ei;
00113 boost::shared_ptr<vector<int> > indices (new vector<int> (2));
00114 (*indices)[0] = 0;
00115 (*indices)[1] = static_cast<int> (cloud->points.size ()) - 1;
00116
00117 PointCloud<PointXYZ> output;
00118 ei.setInputCloud (cloud);
00119 ei.setIndices (indices);
00120 ei.filter (output);
00121
00122 EXPECT_EQ (int (output.points.size ()), 2);
00123 EXPECT_EQ (int (output.width), 2);
00124 EXPECT_EQ (int (output.height), 1);
00125
00126 EXPECT_EQ (cloud->points[0].x, output.points[0].x);
00127 EXPECT_EQ (cloud->points[0].y, output.points[0].y);
00128 EXPECT_EQ (cloud->points[0].z, output.points[0].z);
00129
00130 EXPECT_EQ (cloud->points[cloud->points.size () - 1].x, output.points[1].x);
00131 EXPECT_EQ (cloud->points[cloud->points.size () - 1].y, output.points[1].y);
00132 EXPECT_EQ (cloud->points[cloud->points.size () - 1].z, output.points[1].z);
00133
00134 ei.setNegative (true);
00135 ei.filter (output);
00136
00137 EXPECT_EQ (output.points.size (), cloud->points.size () - 2);
00138 EXPECT_EQ (output.width, cloud->points.size () - 2);
00139 EXPECT_EQ (int (output.height), 1);
00140
00141 EXPECT_EQ (cloud->points[1].x, output.points[0].x);
00142 EXPECT_EQ (cloud->points[1].y, output.points[0].y);
00143 EXPECT_EQ (cloud->points[1].z, output.points[0].z);
00144
00145 EXPECT_EQ (cloud->points[cloud->points.size () - 2].x, output.points[output.points.size () - 1].x);
00146 EXPECT_EQ (cloud->points[cloud->points.size () - 2].y, output.points[output.points.size () - 1].y);
00147 EXPECT_EQ (cloud->points[cloud->points.size () - 2].z, output.points[output.points.size () - 1].z);
00148
00149
00150 ExtractIndices<PCLPointCloud2> ei2;
00151
00152 PCLPointCloud2 output_blob;
00153 ei2.setInputCloud (cloud_blob);
00154 ei2.setIndices (indices);
00155 ei2.filter (output_blob);
00156
00157 fromPCLPointCloud2 (output_blob, output);
00158
00159 EXPECT_EQ (int (output.points.size ()), 2);
00160 EXPECT_EQ (int (output.width), 2);
00161 EXPECT_EQ (int (output.height), 1);
00162
00163 EXPECT_EQ (cloud->points[0].x, output.points[0].x);
00164 EXPECT_EQ (cloud->points[0].y, output.points[0].y);
00165 EXPECT_EQ (cloud->points[0].z, output.points[0].z);
00166
00167 EXPECT_EQ (cloud->points[cloud->points.size () - 1].x, output.points[1].x);
00168 EXPECT_EQ (cloud->points[cloud->points.size () - 1].y, output.points[1].y);
00169 EXPECT_EQ (cloud->points[cloud->points.size () - 1].z, output.points[1].z);
00170
00171 ei2.setNegative (true);
00172 ei2.filter (output_blob);
00173
00174 fromPCLPointCloud2 (output_blob, output);
00175
00176 EXPECT_EQ (output.points.size (), cloud->points.size () - 2);
00177 EXPECT_EQ (output.width, cloud->points.size () - 2);
00178 EXPECT_EQ (int (output.height), 1);
00179
00180 EXPECT_EQ (cloud->points[1].x, output.points[0].x);
00181 EXPECT_EQ (cloud->points[1].y, output.points[0].y);
00182 EXPECT_EQ (cloud->points[1].z, output.points[0].z);
00183
00184 EXPECT_EQ (cloud->points[cloud->points.size () - 2].x, output.points[output.points.size () - 1].x);
00185 EXPECT_EQ (cloud->points[cloud->points.size () - 2].y, output.points[output.points.size () - 1].y);
00186 EXPECT_EQ (cloud->points[cloud->points.size () - 2].z, output.points[output.points.size () - 1].z);
00187
00188
00189 PointCloud<PointXYZ> empty, result;
00190 ExtractIndices<PointXYZ> eie;
00191 eie.setInputCloud (empty.makeShared ());
00192 eie.setNegative (false);
00193 eie.filter (result);
00194
00195 EXPECT_EQ (int (result.points.size ()), 0);
00196 eie.setNegative (true);
00197 eie.filter (result);
00198 EXPECT_EQ (int (result.points.size ()), 0);
00199
00200 boost::shared_ptr<vector<int> > idx (new vector<int> (10));
00201 eie.setIndices (idx);
00202 eie.setNegative (false);
00203 eie.filter (result);
00204 EXPECT_EQ (int (result.points.size ()), 0);
00205 eie.setNegative (true);
00206 eie.filter (result);
00207 EXPECT_EQ (int (result.points.size ()), 0);
00208
00209 empty.points.resize (10);
00210 empty.width = 10; empty.height = 1;
00211 eie.setInputCloud (empty.makeShared ());
00212 for (int i = 0; i < 10; ++i)
00213 (*idx)[i] = i;
00214 eie.setIndices (idx);
00215 eie.setNegative (false);
00216 eie.filter (result);
00217 EXPECT_EQ (int (result.points.size ()), 10);
00218 eie.setNegative (true);
00219 eie.filter (result);
00220 EXPECT_EQ (int (result.points.size ()), 0);
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243 }
00244
00246 TEST (PassThrough, Filters)
00247 {
00248
00249 PointCloud<PointXYZ> output;
00250 PassThrough<PointXYZ> pt;
00251
00252 pt.setInputCloud (cloud);
00253 pt.filter (output);
00254
00255 EXPECT_EQ (output.points.size (), cloud->points.size ());
00256 EXPECT_EQ (output.width, cloud->width);
00257 EXPECT_EQ (output.height, cloud->height);
00258
00259 pt.setFilterFieldName ("z");
00260 pt.setFilterLimits (0.05f, 0.1f);
00261 pt.filter (output);
00262
00263 EXPECT_EQ (int (output.points.size ()), 42);
00264 EXPECT_EQ (int (output.width), 42);
00265 EXPECT_EQ (int (output.height), 1);
00266 EXPECT_EQ (bool (output.is_dense), true);
00267
00268 EXPECT_NEAR (output.points[0].x, -0.074556, 1e-5);
00269 EXPECT_NEAR (output.points[0].y, 0.13415, 1e-5);
00270 EXPECT_NEAR (output.points[0].z, 0.051046, 1e-5);
00271
00272 EXPECT_NEAR (output.points[41].x, -0.030331, 1e-5);
00273 EXPECT_NEAR (output.points[41].y, 0.039749, 1e-5);
00274 EXPECT_NEAR (output.points[41].z, 0.052133, 1e-5);
00275
00276 pt.setFilterLimitsNegative (true);
00277 pt.filter (output);
00278
00279 EXPECT_EQ (int (output.points.size ()), 355);
00280 EXPECT_EQ (int (output.width), 355);
00281 EXPECT_EQ (int (output.height), 1);
00282 EXPECT_EQ (bool (output.is_dense), true);
00283
00284 EXPECT_NEAR (output.points[0].x, 0.0054216, 1e-5);
00285 EXPECT_NEAR (output.points[0].y, 0.11349, 1e-5);
00286 EXPECT_NEAR (output.points[0].z, 0.040749, 1e-5);
00287
00288 EXPECT_NEAR (output.points[354].x, -0.07793, 1e-5);
00289 EXPECT_NEAR (output.points[354].y, 0.17516, 1e-5);
00290 EXPECT_NEAR (output.points[354].z, -0.0444, 1e-5);
00291
00292 PassThrough<PointXYZ> pt_(true);
00293
00294 pt_.setInputCloud (cloud);
00295 pt_.filter (output);
00296
00297 EXPECT_EQ (pt_.getRemovedIndices()->size(), 0);
00298 EXPECT_EQ (output.points.size (), cloud->points.size ());
00299 EXPECT_EQ (output.width, cloud->width);
00300 EXPECT_EQ (output.height, cloud->height);
00301
00302 pt_.setFilterFieldName ("z");
00303 pt_.setFilterLimits (0.05f, 0.1f);
00304 pt_.filter (output);
00305
00306 EXPECT_EQ (int (output.points.size ()), 42);
00307 EXPECT_EQ (int (output.width), 42);
00308 EXPECT_EQ (int (output.height), 1);
00309 EXPECT_EQ (bool (output.is_dense), true);
00310 EXPECT_EQ (int (output.points.size ()), cloud->points.size ()-pt_.getRemovedIndices()->size());
00311
00312 EXPECT_NEAR (output.points[0].x, -0.074556, 1e-5);
00313 EXPECT_NEAR (output.points[0].y, 0.13415, 1e-5);
00314 EXPECT_NEAR (output.points[0].z, 0.051046, 1e-5);
00315
00316 EXPECT_NEAR (output.points[41].x, -0.030331, 1e-5);
00317 EXPECT_NEAR (output.points[41].y, 0.039749, 1e-5);
00318 EXPECT_NEAR (output.points[41].z, 0.052133, 1e-5);
00319
00320 pt_.setFilterLimitsNegative (true);
00321 pt_.filter (output);
00322
00323 EXPECT_EQ (int (output.points.size ()), 355);
00324 EXPECT_EQ (int (output.width), 355);
00325 EXPECT_EQ (int (output.height), 1);
00326 EXPECT_EQ (bool (output.is_dense), true);
00327 EXPECT_EQ (int (output.points.size ()), cloud->points.size ()-pt_.getRemovedIndices()->size());
00328
00329 EXPECT_NEAR (output.points[0].x, 0.0054216, 1e-5);
00330 EXPECT_NEAR (output.points[0].y, 0.11349, 1e-5);
00331 EXPECT_NEAR (output.points[0].z, 0.040749, 1e-5);
00332
00333 EXPECT_NEAR (output.points[354].x, -0.07793, 1e-5);
00334 EXPECT_NEAR (output.points[354].y, 0.17516, 1e-5);
00335 EXPECT_NEAR (output.points[354].z, -0.0444, 1e-5);
00336
00337
00338 pt.setUserFilterValue (std::numeric_limits<float>::quiet_NaN ());
00339 pt.setFilterFieldName ("");
00340 pt.filter (output);
00341
00342 EXPECT_EQ (output.points.size (), cloud->points.size ());
00343 EXPECT_EQ (output.width, cloud->width);
00344 EXPECT_EQ (output.height, cloud->height);
00345 EXPECT_EQ (output.is_dense, cloud->is_dense);
00346 EXPECT_NEAR (output.points[0].x, cloud->points[0].x, 1e-5);
00347 EXPECT_NEAR (output.points[output.points.size () - 1].x, cloud->points[cloud->points.size () - 1].x, 1e-5);
00348
00349 pt.setFilterFieldName ("z");
00350 pt.setFilterLimitsNegative (false);
00351 pt.setKeepOrganized (true);
00352 pt.filter (output);
00353
00354 EXPECT_EQ (output.points.size (), cloud->points.size ());
00355 EXPECT_EQ (output.width, cloud->width);
00356 EXPECT_EQ (output.height, cloud->height);
00357 EXPECT_EQ (bool (output.is_dense), false);
00358
00359 if (!pcl_isnan (output.points[0].x)) EXPECT_EQ (1, 0);
00360 if (!pcl_isnan (output.points[0].y)) EXPECT_EQ (1, 0);
00361 if (!pcl_isnan (output.points[0].z)) EXPECT_EQ (1, 0);
00362
00363 if (!pcl_isnan (output.points[41].x)) EXPECT_EQ (1, 0);
00364 if (!pcl_isnan (output.points[41].y)) EXPECT_EQ (1, 0);
00365 if (!pcl_isnan (output.points[41].z)) EXPECT_EQ (1, 0);
00366
00367 pt.setFilterLimitsNegative (true);
00368 pt.filter (output);
00369
00370 EXPECT_EQ (output.points.size (), cloud->points.size ());
00371 EXPECT_EQ (output.width, cloud->width);
00372 EXPECT_EQ (output.height, cloud->height);
00373 EXPECT_EQ (bool (output.is_dense), false);
00374
00375 EXPECT_NEAR (output.points[0].x, cloud->points[0].x, 1e-5);
00376 EXPECT_NEAR (output.points[0].y, cloud->points[0].y, 1e-5);
00377 EXPECT_NEAR (output.points[0].z, cloud->points[0].z, 1e-5);
00378
00379 EXPECT_NEAR (output.points[41].x, cloud->points[41].x, 1e-5);
00380 EXPECT_NEAR (output.points[41].y, cloud->points[41].y, 1e-5);
00381 EXPECT_NEAR (output.points[41].z, cloud->points[41].z, 1e-5);
00382
00383
00384 PassThrough<PCLPointCloud2> pt2;
00385
00386 PCLPointCloud2 output_blob;
00387 pt2.setInputCloud (cloud_blob);
00388 pt2.filter (output_blob);
00389
00390 fromPCLPointCloud2 (output_blob, output);
00391
00392 EXPECT_EQ (output.points.size (), cloud->points.size ());
00393 EXPECT_EQ (output.width, cloud->width);
00394 EXPECT_EQ (output.height, cloud->height);
00395
00396 pt2.setFilterFieldName ("z");
00397 pt2.setFilterLimits (0.05, 0.1);
00398 pt2.filter (output_blob);
00399
00400 fromPCLPointCloud2 (output_blob, output);
00401
00402 EXPECT_EQ (int (output.points.size ()), 42);
00403 EXPECT_EQ (int (output.width), 42);
00404 EXPECT_EQ (int (output.height), 1);
00405 EXPECT_EQ (bool (output.is_dense), true);
00406
00407 EXPECT_NEAR (output.points[0].x, -0.074556, 1e-5);
00408 EXPECT_NEAR (output.points[0].y, 0.13415, 1e-5);
00409 EXPECT_NEAR (output.points[0].z, 0.051046, 1e-5);
00410
00411 EXPECT_NEAR (output.points[41].x, -0.030331, 1e-5);
00412 EXPECT_NEAR (output.points[41].y, 0.039749, 1e-5);
00413 EXPECT_NEAR (output.points[41].z, 0.052133, 1e-5);
00414
00415 pt2.setFilterLimitsNegative (true);
00416 pt2.filter (output_blob);
00417
00418 fromPCLPointCloud2 (output_blob, output);
00419
00420 EXPECT_EQ (int (output.points.size ()), 355);
00421 EXPECT_EQ (int (output.width), 355);
00422 EXPECT_EQ (int (output.height), 1);
00423 EXPECT_EQ (bool (output.is_dense), true);
00424
00425 EXPECT_NEAR (output.points[0].x, 0.0054216, 1e-5);
00426 EXPECT_NEAR (output.points[0].y, 0.11349, 1e-5);
00427 EXPECT_NEAR (output.points[0].z, 0.040749, 1e-5);
00428
00429 EXPECT_NEAR (output.points[354].x, -0.07793, 1e-5);
00430 EXPECT_NEAR (output.points[354].y, 0.17516, 1e-5);
00431 EXPECT_NEAR (output.points[354].z, -0.0444, 1e-5);
00432
00433 PassThrough<PCLPointCloud2> pt2_(true);
00434 pt2_.setInputCloud (cloud_blob);
00435 pt2_.filter (output_blob);
00436
00437 fromPCLPointCloud2 (output_blob, output);
00438
00439 EXPECT_EQ (pt2_.getRemovedIndices()->size(), 0);
00440 EXPECT_EQ (output.points.size (), cloud->points.size ());
00441 EXPECT_EQ (output.width, cloud->width);
00442 EXPECT_EQ (output.height, cloud->height);
00443
00444 pt2_.setFilterFieldName ("z");
00445 pt2_.setFilterLimits (0.05, 0.1);
00446 pt2_.filter (output_blob);
00447
00448 fromPCLPointCloud2 (output_blob, output);
00449
00450 EXPECT_EQ (int (output.points.size ()), 42);
00451 EXPECT_EQ (int (output.width), 42);
00452 EXPECT_EQ (int (output.height), 1);
00453 EXPECT_EQ (bool (output.is_dense), true);
00454 EXPECT_EQ (int (output.points.size ()), cloud->points.size ()-pt2_.getRemovedIndices()->size());
00455
00456 EXPECT_NEAR (output.points[0].x, -0.074556, 1e-5);
00457 EXPECT_NEAR (output.points[0].y, 0.13415, 1e-5);
00458 EXPECT_NEAR (output.points[0].z, 0.051046, 1e-5);
00459
00460 EXPECT_NEAR (output.points[41].x, -0.030331, 1e-5);
00461 EXPECT_NEAR (output.points[41].y, 0.039749, 1e-5);
00462 EXPECT_NEAR (output.points[41].z, 0.052133, 1e-5);
00463
00464 pt2_.setFilterLimitsNegative (true);
00465 pt2_.filter (output_blob);
00466
00467 fromPCLPointCloud2 (output_blob, output);
00468
00469 EXPECT_EQ (int (output.points.size ()), 355);
00470 EXPECT_EQ (int (output.width), 355);
00471 EXPECT_EQ (int (output.height), 1);
00472 EXPECT_EQ (bool (output.is_dense), true);
00473 EXPECT_EQ (int (output.points.size ()), cloud->points.size ()-pt2_.getRemovedIndices()->size());
00474
00475 EXPECT_NEAR (output.points[0].x, 0.0054216, 1e-5);
00476 EXPECT_NEAR (output.points[0].y, 0.11349, 1e-5);
00477 EXPECT_NEAR (output.points[0].z, 0.040749, 1e-5);
00478
00479 EXPECT_NEAR (output.points[354].x, -0.07793, 1e-5);
00480 EXPECT_NEAR (output.points[354].y, 0.17516, 1e-5);
00481 EXPECT_NEAR (output.points[354].z, -0.0444, 1e-5);
00482
00483
00484 pt2.setUserFilterValue (std::numeric_limits<float>::quiet_NaN ());
00485 pt2.setFilterFieldName ("");
00486 pt2.filter (output_blob);
00487 fromPCLPointCloud2 (output_blob, output);
00488
00489 EXPECT_EQ (output.points.size (), cloud->points.size ());
00490 EXPECT_EQ (output.width, cloud->width);
00491 EXPECT_EQ (output.height, cloud->height);
00492 EXPECT_EQ (output.is_dense, cloud->is_dense);
00493 EXPECT_NEAR (output.points[0].x, cloud->points[0].x, 1e-5);
00494 EXPECT_NEAR (output.points[output.points.size () - 1].x, cloud->points[cloud->points.size () - 1].x, 1e-5);
00495
00496 pt2.setFilterFieldName ("z");
00497 pt2.setFilterLimitsNegative (false);
00498 pt2.setKeepOrganized (true);
00499 pt2.filter (output_blob);
00500 fromPCLPointCloud2 (output_blob, output);
00501
00502 EXPECT_EQ (output.points.size (), cloud->points.size ());
00503 EXPECT_EQ (output.width, cloud->width);
00504 EXPECT_EQ (output.height, cloud->height);
00505 EXPECT_EQ (bool (output.is_dense), false);
00506
00507 if (!pcl_isnan (output.points[0].x)) EXPECT_EQ (1, 0);
00508 if (!pcl_isnan (output.points[0].y)) EXPECT_EQ (1, 0);
00509 if (!pcl_isnan (output.points[0].z)) EXPECT_EQ (1, 0);
00510
00511 if (!pcl_isnan (output.points[41].x)) EXPECT_EQ (1, 0);
00512 if (!pcl_isnan (output.points[41].y)) EXPECT_EQ (1, 0);
00513 if (!pcl_isnan (output.points[41].z)) EXPECT_EQ (1, 0);
00514
00515 pt2.setFilterLimitsNegative (true);
00516 pt2.filter (output_blob);
00517 fromPCLPointCloud2 (output_blob, output);
00518
00519 EXPECT_EQ (output.points.size (), cloud->points.size ());
00520 EXPECT_EQ (output.width, cloud->width);
00521 EXPECT_EQ (output.height, cloud->height);
00522 EXPECT_EQ (bool (output.is_dense), false);
00523
00524 EXPECT_NEAR (output.points[0].x, cloud->points[0].x, 1e-5);
00525 EXPECT_NEAR (output.points[0].y, cloud->points[0].y, 1e-5);
00526 EXPECT_NEAR (output.points[0].z, cloud->points[0].z, 1e-5);
00527
00528 EXPECT_NEAR (output.points[41].x, cloud->points[41].x, 1e-5);
00529 EXPECT_NEAR (output.points[41].y, cloud->points[41].y, 1e-5);
00530 EXPECT_NEAR (output.points[41].z, cloud->points[41].z, 1e-5);
00531 }
00532
00534 TEST (VoxelGrid, Filters)
00535 {
00536
00537 PointCloud<PointXYZ> output;
00538 VoxelGrid<PointXYZ> grid;
00539
00540 grid.setLeafSize (0.02f, 0.02f, 0.02f);
00541 grid.setInputCloud (cloud);
00542 grid.filter (output);
00543
00544 EXPECT_EQ (int (output.points.size ()), 103);
00545 EXPECT_EQ (int (output.width), 103);
00546 EXPECT_EQ (int (output.height), 1);
00547 EXPECT_EQ (bool (output.is_dense), true);
00548
00549 grid.setFilterFieldName ("z");
00550 grid.setFilterLimits (0.05, 0.1);
00551 grid.filter (output);
00552
00553 EXPECT_EQ (int (output.points.size ()), 14);
00554 EXPECT_EQ (int (output.width), 14);
00555 EXPECT_EQ (int (output.height), 1);
00556 EXPECT_EQ (bool (output.is_dense), true);
00557
00558 EXPECT_NEAR (output.points[0].x, -0.026125, 1e-4);
00559 EXPECT_NEAR (output.points[0].y, 0.039788, 1e-4);
00560 EXPECT_NEAR (output.points[0].z, 0.052827, 1e-4);
00561
00562 EXPECT_NEAR (output.points[13].x, -0.073202, 1e-4);
00563 EXPECT_NEAR (output.points[13].y, 0.1296, 1e-4);
00564 EXPECT_NEAR (output.points[13].z, 0.051333, 1e-4);
00565
00566 grid.setFilterLimitsNegative (true);
00567 grid.setSaveLeafLayout(true);
00568 grid.filter (output);
00569
00570 EXPECT_EQ (int (output.points.size ()), 100);
00571 EXPECT_EQ (int (output.width), 100);
00572 EXPECT_EQ (int (output.height), 1);
00573 EXPECT_EQ (bool (output.is_dense), true);
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584 EXPECT_EQ (grid.getCentroidIndex (output.points[0]), 0);
00585 EXPECT_EQ (grid.getCentroidIndex (output.points[99]), 99);
00586 EXPECT_EQ (grid.getCentroidIndexAt (grid.getGridCoordinates (-1,-1,-1)), -1);
00587
00588
00589
00590 int centroidIdx = grid.getCentroidIndex (cloud->points[195]);
00591
00592
00593 EXPECT_LE (fabs (output.points[centroidIdx].x - cloud->points[195].x), 0.02);
00594 EXPECT_LE (fabs (output.points[centroidIdx].y - cloud->points[195].y), 0.02);
00595 EXPECT_LE (fabs (output.points[centroidIdx].z - cloud->points[195].z), 0.02);
00596
00597
00598 EXPECT_EQ (grid.getNeighborCentroidIndices (output.points[0], Eigen::MatrixXi::Zero(3,1))[0], 0);
00599 EXPECT_EQ (grid.getNeighborCentroidIndices (output.points[99], Eigen::MatrixXi::Zero(3,1))[0], 99);
00600
00601
00602 Eigen::MatrixXi directions = Eigen::Vector3i (0, 0, 1);
00603 vector<int> neighbors = grid.getNeighborCentroidIndices (cloud->points[195], directions);
00604 EXPECT_EQ (neighbors.size (), size_t (directions.cols ()));
00605 EXPECT_NE (neighbors.at (0), -1);
00606 EXPECT_LE (fabs (output.points[neighbors.at (0)].x - output.points[centroidIdx].x), 0.02);
00607 EXPECT_LE (fabs (output.points[neighbors.at (0)].y - output.points[centroidIdx].y), 0.02);
00608 EXPECT_LE ( output.points[neighbors.at (0)].z - output.points[centroidIdx].z, 0.02 * 2);
00609
00610
00611 VoxelGrid<PCLPointCloud2> grid2;
00612
00613 PCLPointCloud2 output_blob;
00614
00615 grid2.setLeafSize (0.02f, 0.02f, 0.02f);
00616 grid2.setInputCloud (cloud_blob);
00617 grid2.filter (output_blob);
00618
00619 fromPCLPointCloud2 (output_blob, output);
00620
00621 EXPECT_EQ (int (output.points.size ()), 103);
00622 EXPECT_EQ (int (output.width), 103);
00623 EXPECT_EQ (int (output.height), 1);
00624 EXPECT_EQ (bool (output.is_dense), true);
00625
00626 grid2.setFilterFieldName ("z");
00627 grid2.setFilterLimits (0.05, 0.1);
00628 grid2.filter (output_blob);
00629
00630 fromPCLPointCloud2 (output_blob, output);
00631
00632 EXPECT_EQ (int (output.points.size ()), 14);
00633 EXPECT_EQ (int (output.width), 14);
00634 EXPECT_EQ (int (output.height), 1);
00635 EXPECT_EQ (bool (output.is_dense), true);
00636
00637 EXPECT_NEAR (output.points[0].x, -0.026125, 1e-4);
00638 EXPECT_NEAR (output.points[0].y, 0.039788, 1e-4);
00639 EXPECT_NEAR (output.points[0].z, 0.052827, 1e-4);
00640
00641 EXPECT_NEAR (output.points[13].x, -0.073202, 1e-4);
00642 EXPECT_NEAR (output.points[13].y, 0.1296, 1e-4);
00643 EXPECT_NEAR (output.points[13].z, 0.051333, 1e-4);
00644
00645 grid2.setFilterLimitsNegative (true);
00646 grid2.setSaveLeafLayout(true);
00647 grid2.filter (output_blob);
00648
00649 fromPCLPointCloud2 (output_blob, output);
00650
00651 EXPECT_EQ (int (output.points.size ()), 100);
00652 EXPECT_EQ (int (output.width), 100);
00653 EXPECT_EQ (int (output.height), 1);
00654 EXPECT_EQ (bool (output.is_dense), true);
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664
00665 EXPECT_EQ (grid2.getCentroidIndex (output.points[0].x, output.points[0].y, output.points[0].z), 0);
00666 EXPECT_EQ (grid2.getCentroidIndex (output.points[99].x, output.points[99].y, output.points[99].z), 99);
00667 EXPECT_EQ (grid2.getCentroidIndexAt (grid2.getGridCoordinates (-1,-1,-1)), -1);
00668
00669
00670
00671 int centroidIdx2 = grid2.getCentroidIndex (0.048722f, 0.073760f, 0.017434f);
00672 EXPECT_NE (centroidIdx2, -1);
00673
00674
00675 EXPECT_LE (fabs (output.points[centroidIdx2].x - 0.048722), 0.02);
00676 EXPECT_LE (fabs (output.points[centroidIdx2].y - 0.073760), 0.02);
00677 EXPECT_LE (fabs (output.points[centroidIdx2].z - 0.017434), 0.02);
00678
00679
00680 EXPECT_EQ (grid2.getNeighborCentroidIndices (output.points[0].x, output.points[0].y, output.points[0].z, Eigen::MatrixXi::Zero(3,1))[0], 0);
00681 EXPECT_EQ (grid2.getNeighborCentroidIndices (output.points[99].x, output.points[99].y, output.points[99].z, Eigen::MatrixXi::Zero(3,1))[0], 99);
00682
00683
00684 Eigen::MatrixXi directions2 = Eigen::Vector3i (0, 0, 1);
00685 vector<int> neighbors2 = grid2.getNeighborCentroidIndices (0.048722f, 0.073760f, 0.017434f, directions2);
00686 EXPECT_EQ (neighbors2.size (), size_t (directions2.cols ()));
00687 EXPECT_NE (neighbors2.at (0), -1);
00688 EXPECT_LE (fabs (output.points[neighbors2.at (0)].x - output.points[centroidIdx2].x), 0.02);
00689 EXPECT_LE (fabs (output.points[neighbors2.at (0)].y - output.points[centroidIdx2].y), 0.02);
00690 EXPECT_LE (output.points[neighbors2.at (0)].z - output.points[centroidIdx2].z, 0.02 * 2);
00691 }
00692
00694 TEST (VoxelGrid_RGB, Filters)
00695 {
00696 PCLPointCloud2 cloud_rgb_blob_;
00697 PCLPointCloud2::Ptr cloud_rgb_blob_ptr_;
00698 PointCloud<PointXYZRGB> cloud_rgb_;
00699 PointCloud<PointXYZRGB>::Ptr cloud_rgb_ptr_;
00700
00701 int col_r[] = {214, 193, 180, 164, 133, 119, 158, 179, 178, 212};
00702 int col_g[] = {10, 39, 219, 231, 142, 169, 84, 158, 139, 214};
00703 int col_b[] = {101, 26, 46, 189, 211, 154, 246, 16, 139, 153};
00704 float ave_r = 0.0f;
00705 float ave_b = 0.0f;
00706 float ave_g = 0.0f;
00707 for (int i = 0; i < 10; ++i)
00708 {
00709 ave_r += static_cast<float> (col_r[i]);
00710 ave_g += static_cast<float> (col_g[i]);
00711 ave_b += static_cast<float> (col_b[i]);
00712 }
00713 ave_r /= 10.0f;
00714 ave_g /= 10.0f;
00715 ave_b /= 10.0f;
00716
00717 for (int i = 0; i < 10; ++i)
00718 {
00719 PointXYZRGB pt;
00720 int rgb = (col_r[i] << 16) | (col_g[i] << 8) | col_b[i];
00721 pt.x = 0.0f;
00722 pt.y = 0.0f;
00723 pt.z = 0.0f;
00724 pt.rgb = *reinterpret_cast<float*> (&rgb);
00725 cloud_rgb_.points.push_back (pt);
00726 }
00727
00728 toPCLPointCloud2 (cloud_rgb_, cloud_rgb_blob_);
00729 cloud_rgb_blob_ptr_.reset (new PCLPointCloud2 (cloud_rgb_blob_));
00730 cloud_rgb_ptr_.reset (new PointCloud<PointXYZRGB> (cloud_rgb_));
00731
00732 PointCloud<PointXYZRGB> output_rgb;
00733 VoxelGrid<PointXYZRGB> grid_rgb;
00734
00735 grid_rgb.setLeafSize (0.03f, 0.03f, 0.03f);
00736 grid_rgb.setInputCloud (cloud_rgb_ptr_);
00737 grid_rgb.filter (output_rgb);
00738
00739 EXPECT_EQ (int (output_rgb.points.size ()), 1);
00740 EXPECT_EQ (int (output_rgb.width), 1);
00741 EXPECT_EQ (int (output_rgb.height), 1);
00742 EXPECT_EQ (bool (output_rgb.is_dense), true);
00743 {
00744 int rgb;
00745 int r,g,b;
00746 memcpy (&rgb, &(output_rgb.points[0].rgb), sizeof(int));
00747 r = (rgb >> 16) & 0xFF; g = (rgb >> 8 ) & 0xFF; b = (rgb >> 0 ) & 0xFF;
00748 EXPECT_NEAR (output_rgb.points[0].x, 0.0, 1e-4);
00749 EXPECT_NEAR (output_rgb.points[0].y, 0.0, 1e-4);
00750 EXPECT_NEAR (output_rgb.points[0].z, 0.0, 1e-4);
00751 EXPECT_NEAR (r, ave_r, 1.0);
00752 EXPECT_NEAR (g, ave_g, 1.0);
00753 EXPECT_NEAR (b, ave_b, 1.0);
00754 }
00755
00756 VoxelGrid<PCLPointCloud2> grid2;
00757 PCLPointCloud2 output_rgb_blob;
00758
00759 grid2.setLeafSize (0.03f, 0.03f, 0.03f);
00760 grid2.setInputCloud (cloud_rgb_blob_ptr_);
00761 grid2.filter (output_rgb_blob);
00762
00763 fromPCLPointCloud2 (output_rgb_blob, output_rgb);
00764
00765 EXPECT_EQ (int (output_rgb.points.size ()), 1);
00766 EXPECT_EQ (int (output_rgb.width), 1);
00767 EXPECT_EQ (int (output_rgb.height), 1);
00768 EXPECT_EQ (bool (output_rgb.is_dense), true);
00769 {
00770 int rgb;
00771 int r,g,b;
00772 memcpy (&rgb, &(output_rgb.points[0].rgb), sizeof(int));
00773 r = (rgb >> 16) & 0xFF; g = (rgb >> 8 ) & 0xFF; b = (rgb >> 0 ) & 0xFF;
00774 EXPECT_NEAR (output_rgb.points[0].x, 0.0, 1e-4);
00775 EXPECT_NEAR (output_rgb.points[0].y, 0.0, 1e-4);
00776 EXPECT_NEAR (output_rgb.points[0].z, 0.0, 1e-4);
00777 EXPECT_NEAR (r, ave_r, 1.0);
00778 EXPECT_NEAR (g, ave_g, 1.0);
00779 EXPECT_NEAR (b, ave_b, 1.0);
00780 }
00781 }
00782
00783 #if 0
00784
00785 float getRandomNumber (float max = 1.0, float min = 0.0)
00786 {
00787 return (max - min) * float(rand ()) / float (RAND_MAX) + min;
00788 }
00789
00791 TEST (VoxelGrid_XYZNormal, Filters)
00792 {
00793 PCLPointCloud2 cloud_blob_;
00794 PCLPointCloud2::Ptr cloud_blob_ptr_;
00795
00796 PointCloud<PointNormal>::Ptr input (new PointCloud<PointNormal>);
00797 PointCloud<PointNormal> output;
00798 input->reserve (16);
00799 input->is_dense = false;
00800
00801 PointNormal point;
00802 PointNormal ground_truth[2][2][2];
00803 for (unsigned zIdx = 0; zIdx < 2; ++zIdx)
00804 {
00805
00806 for (unsigned yIdx = 0; yIdx < 2; ++yIdx)
00807 {
00808 for (unsigned xIdx = 0; xIdx < 2; ++xIdx)
00809 {
00810
00811
00812
00813
00814 PointNormal& voxel = ground_truth [xIdx][yIdx][zIdx];
00815
00816 point.x = xIdx * 1.99;
00817 point.y = yIdx * 1.99;
00818 point.z = zIdx * 1.99;
00819 point.normal_x = getRandomNumber (1.0, -1.0);
00820 point.normal_y = getRandomNumber (1.0, -1.0);
00821 point.normal_z = getRandomNumber (1.0, -1.0);
00822
00823 float norm = 1.0f / sqrt (point.normal_x * point.normal_x + point.normal_y * point.normal_y + point.normal_z * point.normal_z );
00824 point.normal_x *= norm;
00825 point.normal_y *= norm;
00826 point.normal_z *= norm;
00827
00828
00829
00830 input->push_back (point);
00831
00832 voxel = point;
00833
00834 if (xIdx != 0)
00835 {
00836 point.x = getRandomNumber (0.99) + float (xIdx);
00837 point.y = getRandomNumber (0.99) + float (yIdx);
00838 point.z = getRandomNumber (0.99) + float (zIdx);
00839 }
00840 if (yIdx == 0 && zIdx == 0)
00841 {
00842 point.normal_x *= -1.0;
00843 point.normal_y *= -1.0;
00844 point.normal_z *= -1.0;
00845 }
00846 else if (yIdx == 0 && zIdx == 1)
00847 {
00848 point.normal_x = std::numeric_limits<float>::quiet_NaN ();
00849 point.normal_y = std::numeric_limits<float>::quiet_NaN ();
00850 point.normal_z = std::numeric_limits<float>::quiet_NaN ();
00851 }
00852 else if (yIdx == 1 && zIdx == 0)
00853 {
00854 point.normal_x = voxel.normal_y - voxel.normal_z;
00855 point.normal_y = voxel.normal_z - voxel.normal_x;
00856 point.normal_z = voxel.normal_x - voxel.normal_y;
00857 }
00858 else if (yIdx == 1 && zIdx == 1)
00859 {
00860 point.normal_x = getRandomNumber (1.0, -1.0);
00861 point.normal_y = getRandomNumber (1.0, -1.0);
00862 point.normal_z = getRandomNumber (1.0, -1.0);
00863 }
00864
00865 voxel.x += point.x;
00866 voxel.y += point.y;
00867 voxel.z += point.z;
00868
00869 voxel.x *= 0.5;
00870 voxel.y *= 0.5;
00871 voxel.z *= 0.5;
00872
00873 if (yIdx == 0 && zIdx == 0)
00874 {
00875 voxel.normal_x = std::numeric_limits<float>::quiet_NaN ();
00876 voxel.normal_y = std::numeric_limits<float>::quiet_NaN ();
00877 voxel.normal_z = std::numeric_limits<float>::quiet_NaN ();
00878 }
00879 else if (pcl_isfinite (point.normal_x))
00880 {
00881 float norm = 1.0f / sqrt (point.normal_x * point.normal_x + point.normal_y * point.normal_y + point.normal_z * point.normal_z );
00882 point.normal_x *= norm;
00883 point.normal_y *= norm;
00884 point.normal_z *= norm;
00885
00886 voxel.normal_x += point.normal_x;
00887 voxel.normal_y += point.normal_y;
00888 voxel.normal_z += point.normal_z;
00889
00890 norm = 1.0f / sqrt (voxel.normal_x * voxel.normal_x + voxel.normal_y * voxel.normal_y + voxel.normal_z * voxel.normal_z );
00891
00892 voxel.normal_x *= norm;
00893 voxel.normal_y *= norm;
00894 voxel.normal_z *= norm;
00895 }
00896
00897
00898 input->push_back (point);
00899
00900
00901
00902 }
00903 }
00904 }
00905
00906 VoxelGrid<PointNormal> grid;
00907 grid.setLeafSize (1.0f, 1.0f, 1.0f);
00908 grid.setFilterLimits (0.0, 2.0);
00909 grid.setInputCloud (input);
00910 grid.filter (output);
00911
00912
00913 for (unsigned idx = 0, zIdx = 0; zIdx < 2; ++zIdx)
00914 {
00915 for (unsigned yIdx = 0; yIdx < 2; ++yIdx)
00916 {
00917 for (unsigned xIdx = 0; xIdx < 2; ++xIdx, ++idx)
00918 {
00919 PointNormal& voxel = ground_truth [xIdx][yIdx][zIdx];
00920 PointNormal& point = output.points [idx];
00921
00922 EXPECT_EQ (voxel.x, point.x);
00923 EXPECT_EQ (voxel.y, point.y);
00924 EXPECT_EQ (voxel.z, point.z);
00925
00926 if (pcl_isfinite(voxel.normal_x) || pcl_isfinite (point.normal_x))
00927 {
00928 EXPECT_EQ (voxel.normal_x, point.normal_x);
00929 EXPECT_EQ (voxel.normal_y, point.normal_y);
00930 EXPECT_EQ (voxel.normal_z, point.normal_z);
00931 }
00932 }
00933 }
00934 }
00935
00936 toPCLPointCloud2 (*input, cloud_blob_);
00937 cloud_blob_ptr_.reset (new PCLPointCloud2 (cloud_blob_));
00938
00939 VoxelGrid<PCLPointCloud2> grid2;
00940 PCLPointCloud2 output_blob;
00941
00942 grid2.setLeafSize (1.0f, 1.0f, 1.0f);
00943 grid2.setFilterLimits (0.0f, 2.0f);
00944 grid2.setInputCloud (cloud_blob_ptr_);
00945 grid2.filter (output_blob);
00946
00947 fromPCLPointCloud2 (output_blob, output);
00948
00949 for (unsigned idx = 0, zIdx = 0; zIdx < 2; ++zIdx)
00950 {
00951 for (unsigned yIdx = 0; yIdx < 2; ++yIdx)
00952 {
00953 for (unsigned xIdx = 0; xIdx < 2; ++xIdx, ++idx)
00954 {
00955 PointNormal& voxel = ground_truth [xIdx][yIdx][zIdx];
00956 PointNormal& point = output.points [idx];
00957
00958 EXPECT_EQ (voxel.x, point.x);
00959 EXPECT_EQ (voxel.y, point.y);
00960 EXPECT_EQ (voxel.z, point.z);
00961
00962 if (pcl_isfinite(voxel.normal_x) || pcl_isfinite (point.normal_x))
00963 {
00964 EXPECT_EQ (voxel.normal_x, point.normal_x);
00965 EXPECT_EQ (voxel.normal_y, point.normal_y);
00966 EXPECT_EQ (voxel.normal_z, point.normal_z);
00967 }
00968 }
00969 }
00970 }
00971 }
00972
00973 #endif
00974
00976 TEST (VoxelGridCovariance, Filters)
00977 {
00978
00979 PointCloud<PointXYZ> output;
00980 VoxelGridCovariance<PointXYZ> grid;
00981
00982 grid.setLeafSize (0.02f, 0.02f, 0.02f);
00983 grid.setInputCloud (cloud);
00984 grid.filter (output);
00985
00986 EXPECT_EQ (int (output.points.size ()), 23);
00987 EXPECT_EQ (int (output.width), 23);
00988 EXPECT_EQ (int (output.height), 1);
00989 EXPECT_EQ (bool (output.is_dense), true);
00990
00991 EXPECT_NEAR (output.points[0].x, -0.073619894683361053, 1e-4);
00992 EXPECT_NEAR (output.points[0].y, 0.16789889335632324, 1e-4);
00993 EXPECT_NEAR (output.points[0].z, -0.03018110990524292, 1e-4);
00994
00995 EXPECT_NEAR (output.points[13].x, -0.06865914911031723, 1e-4);
00996 EXPECT_NEAR (output.points[13].y, 0.15243285894393921, 1e-4);
00997 EXPECT_NEAR (output.points[13].z, 0.03266800194978714, 1e-4);
00998
00999 grid.setSaveLeafLayout (true);
01000 grid.filter (output);
01001
01002
01003 EXPECT_EQ (grid.getCentroidIndex (output.points[0]), 0);
01004 EXPECT_EQ (grid.getCentroidIndex (output.points[17]), 17);
01005 EXPECT_EQ (grid.getCentroidIndexAt (grid.getGridCoordinates (-1,-1,-1)), -1);
01006
01007
01008
01009 int centroidIdx = grid.getCentroidIndex (cloud->points[38]);
01010 EXPECT_NE (centroidIdx, -1);
01011
01012
01013 EXPECT_EQ (grid.getNeighborCentroidIndices (output.points[0], Eigen::MatrixXi::Zero(3,1))[0], 0);
01014 EXPECT_EQ (grid.getNeighborCentroidIndices (output.points[17], Eigen::MatrixXi::Zero(3,1))[0], 17);
01015
01016
01017 Eigen::MatrixXi directions = Eigen::Vector3i (0, 1, 0);
01018 vector<int> neighbors = grid.getNeighborCentroidIndices (cloud->points[38], directions);
01019 EXPECT_EQ (neighbors.size (), size_t (directions.cols ()));
01020 EXPECT_NE (neighbors.at (0), -1);
01021 EXPECT_LE (fabs (output.points[neighbors.at (0)].x - output.points[centroidIdx].x), 0.02);
01022 EXPECT_LE (fabs (output.points[neighbors.at (0)].y - output.points[centroidIdx].y), 0.02);
01023 EXPECT_LE (output.points[neighbors.at (0)].z - output.points[centroidIdx].z, 0.02 * 2);
01024
01025
01026 grid.setSaveLeafLayout (false);
01027 grid.filter (output, true);
01028
01029
01030 vector<VoxelGridCovariance<pcl::PointXYZ>::LeafConstPtr> leaves;
01031 vector<float> distances;
01032 grid.nearestKSearch (PointXYZ(0,1,0), 1, leaves, distances);
01033
01034 EXPECT_EQ (int (leaves.size ()), 1);
01035
01036 EXPECT_NEAR (leaves[0]->getMean ()[0], -0.0284687, 1e-4);
01037 EXPECT_NEAR (leaves[0]->getMean ()[1], 0.170919, 1e-4);
01038 EXPECT_NEAR (leaves[0]->getMean ()[2], -0.00765753, 1e-4);
01039
01040
01041 grid.radiusSearch (PointXYZ (0,0,0), 0.075, leaves, distances);
01042
01043 EXPECT_EQ (leaves.size (), 3);
01044
01045 EXPECT_NEAR (leaves[0]->getMean ()[0], 0.0322579, 1e-4);
01046 EXPECT_NEAR (leaves[0]->getMean ()[1], 0.0469001, 1e-4);
01047 EXPECT_NEAR (leaves[0]->getMean ()[2], 0.0328501, 1e-4);
01048
01049 EXPECT_NEAR (leaves[1]->getMean ()[0], 0.0124421, 1e-4);
01050 EXPECT_NEAR (leaves[1]->getMean ()[1], 0.0524267, 1e-4);
01051 EXPECT_NEAR (leaves[1]->getMean ()[2], 0.0488767, 1e-4);
01052
01053 EXPECT_NEAR (leaves[2]->getMean ()[0], -0.00936106, 1e-4);
01054 EXPECT_NEAR (leaves[2]->getMean ()[1], 0.0516725, 1e-4);
01055 EXPECT_NEAR (leaves[2]->getMean ()[2], 0.0508024, 1e-4);
01056 }
01057
01059 TEST (ProjectInliers, Filters)
01060 {
01061
01062 ProjectInliers<PointXYZ> proj;
01063 PointCloud<PointXYZ> output;
01064
01065 ModelCoefficients::Ptr coefficients (new ModelCoefficients ());
01066 coefficients->values.resize (4);
01067 coefficients->values[0] = coefficients->values[1] = 0;
01068 coefficients->values[2] = 1.0;
01069 coefficients->values[3] = 0;
01070
01071 proj.setModelType (SACMODEL_PLANE);
01072 proj.setInputCloud (cloud);
01073 proj.setModelCoefficients (coefficients);
01074 proj.filter (output);
01075
01076 for (size_t i = 0; i < output.points.size (); ++i)
01077 EXPECT_NEAR (output.points[i].z, 0.0, 1e-4);
01078
01079
01080 ProjectInliers<PCLPointCloud2> proj2;
01081
01082 PCLPointCloud2 output_blob;
01083
01084 proj2.setModelType (SACMODEL_PLANE);
01085 proj2.setInputCloud (cloud_blob);
01086 proj2.setModelCoefficients (coefficients);
01087 proj2.filter (output_blob);
01088
01089 fromPCLPointCloud2 (output_blob, output);
01090
01091 for (size_t i = 0; i < output.points.size (); ++i)
01092 EXPECT_NEAR (output.points[i].z, 0.0, 1e-4);
01093 }
01094
01096 TEST (RadiusOutlierRemoval, Filters)
01097 {
01098
01099 PointCloud<PointXYZ> cloud_out;
01100
01101 RadiusOutlierRemoval<PointXYZ> outrem;
01102 outrem.setInputCloud (cloud);
01103 outrem.setRadiusSearch (0.02);
01104 outrem.setMinNeighborsInRadius (14);
01105 outrem.filter (cloud_out);
01106
01107 EXPECT_EQ (int (cloud_out.points.size ()), 307);
01108 EXPECT_EQ (int (cloud_out.width), 307);
01109 EXPECT_EQ (bool (cloud_out.is_dense), true);
01110 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].x, -0.077893, 1e-4);
01111 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].y, 0.16039, 1e-4);
01112 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].z, -0.021299, 1e-4);
01113
01114
01115 PCLPointCloud2 cloud_out2;
01116 RadiusOutlierRemoval<PCLPointCloud2> outrem2;
01117 outrem2.setInputCloud (cloud_blob);
01118 outrem2.setRadiusSearch (0.02);
01119 outrem2.setMinNeighborsInRadius (15);
01120 outrem2.filter (cloud_out2);
01121
01122 fromPCLPointCloud2 (cloud_out2, cloud_out);
01123 EXPECT_EQ (int (cloud_out.points.size ()), 307);
01124 EXPECT_EQ (int (cloud_out.width), 307);
01125 EXPECT_EQ (bool (cloud_out.is_dense), true);
01126 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].x, -0.077893, 1e-4);
01127 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].y, 0.16039, 1e-4);
01128 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].z, -0.021299, 1e-4);
01129
01130
01131 RadiusOutlierRemoval<PointXYZ> outrem_(true);
01132 outrem_.setInputCloud (cloud);
01133 outrem_.setRadiusSearch (0.02);
01134 outrem_.setMinNeighborsInRadius (14);
01135 outrem_.filter (cloud_out);
01136
01137 EXPECT_EQ (int (cloud_out.points.size ()), 307);
01138 EXPECT_EQ (int (cloud_out.width), 307);
01139 EXPECT_EQ (bool (cloud_out.is_dense), true);
01140 EXPECT_EQ (int (cloud_out.points.size ()), cloud->points.size ()-outrem_.getRemovedIndices()->size());
01141
01142 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].x, -0.077893, 1e-4);
01143 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].y, 0.16039, 1e-4);
01144 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].z, -0.021299, 1e-4);
01145
01146
01147 RadiusOutlierRemoval<PCLPointCloud2> outrem2_(true);
01148 outrem2_.setInputCloud (cloud_blob);
01149 outrem2_.setRadiusSearch (0.02);
01150 outrem2_.setMinNeighborsInRadius (15);
01151 outrem2_.filter (cloud_out2);
01152
01153 fromPCLPointCloud2 (cloud_out2, cloud_out);
01154 EXPECT_EQ (int (cloud_out.points.size ()), 307);
01155 EXPECT_EQ (int (cloud_out.width), 307);
01156 EXPECT_EQ (bool (cloud_out.is_dense), true);
01157 EXPECT_EQ (int (cloud_out.points.size ()), cloud_blob->width*cloud_blob->height-outrem2_.getRemovedIndices()->size());
01158
01159 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].x, -0.077893, 1e-4);
01160 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].y, 0.16039, 1e-4);
01161 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].z, -0.021299, 1e-4);
01162 }
01163
01165 TEST (CropBox, Filters)
01166 {
01167
01168
01169
01170
01171
01172 PointCloud<PointXYZ>::Ptr input (new PointCloud<PointXYZ> ());
01173
01174 input->push_back (PointXYZ (0.0f, 0.0f, 0.0f));
01175 input->push_back (PointXYZ (0.9f, 0.9f, 0.9f));
01176 input->push_back (PointXYZ (0.9f, 0.9f, -0.9f));
01177 input->push_back (PointXYZ (0.9f, -0.9f, 0.9f));
01178 input->push_back (PointXYZ (-0.9f, 0.9f, 0.9f));
01179 input->push_back (PointXYZ (0.9f, -0.9f, -0.9f));
01180 input->push_back (PointXYZ (-0.9f, -0.9f, 0.9f));
01181 input->push_back (PointXYZ (-0.9f, 0.9f, -0.9f));
01182 input->push_back (PointXYZ (-0.9f, -0.9f, -0.9f));
01183
01184
01185 CropBox<PointXYZ> cropBoxFilter (true);
01186 cropBoxFilter.setInputCloud (input);
01187 Eigen::Vector4f min_pt (-1.0f, -1.0f, -1.0f, 1.0f);
01188 Eigen::Vector4f max_pt (1.0f, 1.0f, 1.0f, 1.0f);
01189
01190
01191 cropBoxFilter.setMin (min_pt);
01192 cropBoxFilter.setMax (max_pt);
01193
01194
01195 vector<int> indices;
01196 cropBoxFilter.filter (indices);
01197
01198
01199 PointCloud<PointXYZ> cloud_out;
01200 cropBoxFilter.filter (cloud_out);
01201
01202
01203 EXPECT_EQ (int (indices.size ()), 9);
01204 EXPECT_EQ (int (cloud_out.size ()), 9);
01205 EXPECT_EQ (int (cloud_out.width), 9);
01206 EXPECT_EQ (int (cloud_out.height), 1);
01207
01208 IndicesConstPtr removed_indices;
01209 removed_indices = cropBoxFilter.getRemovedIndices ();
01210 EXPECT_EQ (int (removed_indices->size ()), 0);
01211
01212
01213 PointCloud<PointXYZ> cloud_out_negative;
01214 cropBoxFilter.setNegative (true);
01215 cropBoxFilter.filter (cloud_out_negative);
01216 EXPECT_EQ (int (cloud_out_negative.size ()), 0);
01217
01218 cropBoxFilter.filter (indices);
01219 EXPECT_EQ (int (indices.size ()), 0);
01220
01221 cropBoxFilter.setNegative (false);
01222 cropBoxFilter.filter (cloud_out);
01223
01224
01225 cropBoxFilter.setTranslation(Eigen::Vector3f(0, 1, 0));
01226 cropBoxFilter.filter (indices);
01227 cropBoxFilter.filter (cloud_out);
01228
01229 EXPECT_EQ (int (indices.size ()), 5);
01230 EXPECT_EQ (int (cloud_out.size ()), 5);
01231
01232 removed_indices = cropBoxFilter.getRemovedIndices ();
01233 EXPECT_EQ (int (removed_indices->size ()), 4);
01234
01235
01236 cropBoxFilter.setNegative (true);
01237 cropBoxFilter.filter (cloud_out_negative);
01238 EXPECT_EQ (int (cloud_out_negative.size ()), 4);
01239
01240 cropBoxFilter.filter (indices);
01241 EXPECT_EQ (int (indices.size ()), 4);
01242
01243 cropBoxFilter.setNegative (false);
01244 cropBoxFilter.filter (cloud_out);
01245
01246
01247 cropBoxFilter.setRotation (Eigen::Vector3f (0.0f, 45.0f * float (M_PI) / 180.0f, 0.0f));
01248 cropBoxFilter.filter (indices);
01249 cropBoxFilter.filter (cloud_out);
01250
01251 EXPECT_EQ (int (indices.size ()), 1);
01252 EXPECT_EQ (int (cloud_out.size ()), 1);
01253 EXPECT_EQ (int (cloud_out.width), 1);
01254 EXPECT_EQ (int (cloud_out.height), 1);
01255
01256 removed_indices = cropBoxFilter.getRemovedIndices ();
01257 EXPECT_EQ (int (removed_indices->size ()), 8);
01258
01259
01260 cropBoxFilter.setNegative (true);
01261 cropBoxFilter.filter (cloud_out_negative);
01262 EXPECT_EQ (int (cloud_out_negative.size ()), 8);
01263
01264 cropBoxFilter.filter (indices);
01265 EXPECT_EQ (int (indices.size ()), 8);
01266
01267 cropBoxFilter.setNegative (false);
01268 cropBoxFilter.filter (cloud_out);
01269
01270
01271 cropBoxFilter.setTransform (getTransformation (0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -45.0f * float (M_PI) / 180.0f));
01272 cropBoxFilter.filter (indices);
01273 cropBoxFilter.filter (cloud_out);
01274
01275 EXPECT_EQ (int (indices.size ()), 3);
01276 EXPECT_EQ (int (cloud_out.size ()), 3);
01277 EXPECT_EQ (int (cloud_out.width), 3);
01278 EXPECT_EQ (int (cloud_out.height), 1);
01279
01280 removed_indices = cropBoxFilter.getRemovedIndices ();
01281 EXPECT_EQ (int (removed_indices->size ()), 6);
01282
01283
01284 cropBoxFilter.setNegative (true);
01285 cropBoxFilter.filter (cloud_out_negative);
01286 EXPECT_EQ (int (cloud_out_negative.size ()), 6);
01287
01288 cropBoxFilter.filter (indices);
01289 EXPECT_EQ (int (indices.size ()), 6);
01290
01291 cropBoxFilter.setNegative (false);
01292 cropBoxFilter.filter (cloud_out);
01293
01294
01295 cropBoxFilter.setTransform (getTransformation(0, -1, 0, 0, 0, -45.0 * float (M_PI) / 180.0));
01296 cropBoxFilter.filter (indices);
01297 cropBoxFilter.filter (cloud_out);
01298
01299 EXPECT_EQ (int (indices.size ()), 2);
01300 EXPECT_EQ (int (cloud_out.size ()), 2);
01301 EXPECT_EQ (int (cloud_out.width), 2);
01302 EXPECT_EQ (int (cloud_out.height), 1);
01303
01304 removed_indices = cropBoxFilter.getRemovedIndices ();
01305 EXPECT_EQ (int (removed_indices->size ()), 7);
01306
01307
01308 cropBoxFilter.setNegative (true);
01309 cropBoxFilter.filter (cloud_out_negative);
01310 EXPECT_EQ (int (cloud_out_negative.size ()), 7);
01311
01312 cropBoxFilter.filter (indices);
01313 EXPECT_EQ (int (indices.size ()), 7);
01314
01315 cropBoxFilter.setNegative (false);
01316 cropBoxFilter.filter (cloud_out);
01317
01318
01319 cropBoxFilter.setTransform (getTransformation(0, -1, 0, 0, 0, 0));
01320 cropBoxFilter.filter (indices);
01321 cropBoxFilter.filter (cloud_out);
01322
01323 EXPECT_EQ (int (indices.size ()), 0);
01324 EXPECT_EQ (int (cloud_out.size ()), 0);
01325 EXPECT_EQ (int (cloud_out.width), 0);
01326 EXPECT_EQ (int (cloud_out.height), 1);
01327
01328 removed_indices = cropBoxFilter.getRemovedIndices ();
01329 EXPECT_EQ (int (removed_indices->size ()), 9);
01330
01331
01332 cropBoxFilter.setNegative (true);
01333 cropBoxFilter.filter (cloud_out_negative);
01334 EXPECT_EQ (int (cloud_out_negative.size ()), 9);
01335
01336 cropBoxFilter.filter (indices);
01337 EXPECT_EQ (int (indices.size ()), 9);
01338
01339
01340
01341
01342
01343 PCLPointCloud2::Ptr input2 (new PCLPointCloud2);
01344 pcl::toPCLPointCloud2 (*input, *input2);
01345
01346
01347 CropBox<PCLPointCloud2> cropBoxFilter2(true);
01348 cropBoxFilter2.setInputCloud (input2);
01349
01350
01351 cropBoxFilter2.setMin (min_pt);
01352 cropBoxFilter2.setMax (max_pt);
01353
01354
01355 vector<int> indices2;
01356 cropBoxFilter2.filter (indices2);
01357
01358
01359 PCLPointCloud2 cloud_out2;
01360 cropBoxFilter2.filter (cloud_out2);
01361
01362
01363 EXPECT_EQ (int (indices2.size ()), 9);
01364 EXPECT_EQ (int (indices2.size ()), int (cloud_out2.width * cloud_out2.height));
01365
01366 IndicesConstPtr removed_indices2;
01367 removed_indices2 = cropBoxFilter2.getRemovedIndices ();
01368 EXPECT_EQ (int (removed_indices2->size ()), 0);
01369
01370
01371 PCLPointCloud2 cloud_out2_negative;
01372 cropBoxFilter2.setNegative (true);
01373 cropBoxFilter2.filter (cloud_out2_negative);
01374 EXPECT_EQ (int (cloud_out2_negative.width), 0);
01375
01376 cropBoxFilter2.filter (indices2);
01377 EXPECT_EQ (int (indices2.size ()), 0);
01378
01379 cropBoxFilter2.setNegative (false);
01380 cropBoxFilter2.filter (cloud_out2);
01381
01382
01383 cropBoxFilter2.setTranslation (Eigen::Vector3f(0, 1, 0));
01384 cropBoxFilter2.filter (indices2);
01385 cropBoxFilter2.filter (cloud_out2);
01386
01387 EXPECT_EQ (int (indices2.size ()), 5);
01388 EXPECT_EQ (int (indices2.size ()), int (cloud_out2.width * cloud_out2.height));
01389
01390 removed_indices2 = cropBoxFilter2.getRemovedIndices ();
01391 EXPECT_EQ (int (removed_indices2->size ()), 4);
01392
01393
01394 cropBoxFilter2.setNegative (true);
01395 cropBoxFilter2.filter (cloud_out2_negative);
01396 EXPECT_EQ (int (cloud_out2_negative.width), 4);
01397
01398 cropBoxFilter2.filter (indices2);
01399 EXPECT_EQ (int (indices2.size ()), 4);
01400
01401 cropBoxFilter2.setNegative (false);
01402 cropBoxFilter2.filter (cloud_out2);
01403
01404
01405 cropBoxFilter2.setRotation (Eigen::Vector3f (0.0f, 45.0f * float (M_PI) / 180.0f, 0.0f));
01406 cropBoxFilter2.filter (indices2);
01407 cropBoxFilter2.filter (cloud_out2);
01408
01409 EXPECT_EQ (int (indices2.size ()), 1);
01410 EXPECT_EQ (int (indices2.size ()), int (cloud_out2.width * cloud_out2.height));
01411
01412
01413 cropBoxFilter2.setTransform (getTransformation (0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -45.0f * float (M_PI) / 180.0f));
01414 cropBoxFilter2.filter (indices2);
01415 cropBoxFilter2.filter (cloud_out2);
01416
01417 EXPECT_EQ (int (indices2.size ()), 3);
01418 EXPECT_EQ (int (cloud_out2.width * cloud_out2.height), 3);
01419
01420 removed_indices2 = cropBoxFilter2.getRemovedIndices ();
01421 EXPECT_EQ (int (removed_indices2->size ()), 6);
01422
01423
01424 cropBoxFilter2.setNegative (true);
01425 cropBoxFilter2.filter (cloud_out2_negative);
01426 EXPECT_EQ (int (cloud_out2_negative.width), 6);
01427
01428 cropBoxFilter2.filter (indices2);
01429 EXPECT_EQ (int (indices2.size ()), 6);
01430
01431 cropBoxFilter2.setNegative (false);
01432 cropBoxFilter2.filter (cloud_out2);
01433
01434
01435 cropBoxFilter2.setTransform (getTransformation (0.0f, -1.0f, 0.0f, 0.0f, 0.0f, -45.0f * float (M_PI) / 180.0f));
01436 cropBoxFilter2.filter (indices2);
01437 cropBoxFilter2.filter (cloud_out2);
01438
01439 EXPECT_EQ (int (indices2.size ()), 2);
01440 EXPECT_EQ (int (cloud_out2.width * cloud_out2.height), 2);
01441
01442 removed_indices2 = cropBoxFilter2.getRemovedIndices ();
01443 EXPECT_EQ (int (removed_indices2->size ()), 7);
01444
01445
01446 cropBoxFilter2.setNegative (true);
01447 cropBoxFilter2.filter (cloud_out2_negative);
01448 EXPECT_EQ (int (cloud_out2_negative.width), 7);
01449
01450 cropBoxFilter2.filter (indices2);
01451 EXPECT_EQ (int (indices2.size ()), 7);
01452
01453 cropBoxFilter2.setNegative (false);
01454 cropBoxFilter2.filter (cloud_out2);
01455
01456
01457 cropBoxFilter2.setTransform (getTransformation(0, -1, 0, 0, 0, 0));
01458 cropBoxFilter2.filter (indices2);
01459 cropBoxFilter2.filter (cloud_out2);
01460
01461 EXPECT_EQ (int (indices2.size ()), 0);
01462 EXPECT_EQ (int (cloud_out2.width * cloud_out2.height), 0);
01463
01464 removed_indices2 = cropBoxFilter2.getRemovedIndices ();
01465 EXPECT_EQ (int (removed_indices2->size ()), 9);
01466
01467
01468 cropBoxFilter2.setNegative (true);
01469 cropBoxFilter2.filter (cloud_out2_negative);
01470 EXPECT_EQ (int (cloud_out2_negative.width), 9);
01471
01472 cropBoxFilter2.filter (indices2);
01473 EXPECT_EQ (int (indices2.size ()), 9);
01474
01475 cropBoxFilter2.setNegative (false);
01476 cropBoxFilter2.filter (cloud_out2);
01477 }
01478
01480 TEST (StatisticalOutlierRemoval, Filters)
01481 {
01482
01483 PointCloud<PointXYZ> output;
01484
01485 StatisticalOutlierRemoval<PointXYZ> outrem;
01486 outrem.setInputCloud (cloud);
01487 outrem.setMeanK (50);
01488 outrem.setStddevMulThresh (1.0);
01489 outrem.filter (output);
01490
01491 EXPECT_EQ (int (output.points.size ()), 352);
01492 EXPECT_EQ (int (output.width), 352);
01493 EXPECT_EQ (bool (output.is_dense), true);
01494 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.034667, 1e-4);
01495 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.15131, 1e-4);
01496 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.00071029, 1e-4);
01497
01498 outrem.setNegative (true);
01499 outrem.filter (output);
01500
01501 EXPECT_EQ (int (output.points.size ()), int (cloud->points.size ()) - 352);
01502 EXPECT_EQ (int (output.width), int (cloud->width) - 352);
01503 EXPECT_EQ (bool (output.is_dense), true);
01504 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.07793, 1e-4);
01505 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.17516, 1e-4);
01506 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.0444, 1e-4);
01507
01508
01509 PCLPointCloud2 output2;
01510 StatisticalOutlierRemoval<PCLPointCloud2> outrem2;
01511 outrem2.setInputCloud (cloud_blob);
01512 outrem2.setMeanK (50);
01513 outrem2.setStddevMulThresh (1.0);
01514 outrem2.filter (output2);
01515
01516 fromPCLPointCloud2 (output2, output);
01517
01518 EXPECT_EQ (int (output.points.size ()), 352);
01519 EXPECT_EQ (int (output.width), 352);
01520 EXPECT_EQ (bool (output.is_dense), true);
01521 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.034667, 1e-4);
01522 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.15131, 1e-4);
01523 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.00071029, 1e-4);
01524
01525 outrem2.setNegative (true);
01526 outrem2.filter (output2);
01527
01528 fromPCLPointCloud2 (output2, output);
01529
01530 EXPECT_EQ (int (output.points.size ()), int (cloud->points.size ()) - 352);
01531 EXPECT_EQ (int (output.width), int (cloud->width) - 352);
01532 EXPECT_EQ (bool (output.is_dense), true);
01533 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.07793, 1e-4);
01534 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.17516, 1e-4);
01535 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.0444, 1e-4);
01536
01537
01538 StatisticalOutlierRemoval<PointXYZ> outrem_(true);
01539 outrem_.setInputCloud (cloud);
01540 outrem_.setMeanK (50);
01541 outrem_.setStddevMulThresh (1.0);
01542 outrem_.filter (output);
01543
01544 EXPECT_EQ (int (output.points.size ()), 352);
01545 EXPECT_EQ (int (output.width), 352);
01546 EXPECT_EQ (bool (output.is_dense), true);
01547 EXPECT_EQ (int (output.points.size ()), cloud->points.size ()-outrem_.getRemovedIndices()->size());
01548 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.034667, 1e-4);
01549 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.15131, 1e-4);
01550 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.00071029, 1e-4);
01551
01552 outrem_.setNegative (true);
01553 outrem_.filter (output);
01554
01555 EXPECT_EQ (int (output.points.size ()), int (cloud->points.size ()) - 352);
01556 EXPECT_EQ (int (output.width), int (cloud->width) - 352);
01557 EXPECT_EQ (bool (output.is_dense), true);
01558 EXPECT_EQ (int (output.points.size ()) ,cloud->points.size ()-outrem_.getRemovedIndices()->size());
01559 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.07793, 1e-4);
01560 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.17516, 1e-4);
01561 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.0444, 1e-4);
01562
01563
01564 StatisticalOutlierRemoval<PCLPointCloud2> outrem2_(true);
01565 outrem2_.setInputCloud (cloud_blob);
01566 outrem2_.setMeanK (50);
01567 outrem2_.setStddevMulThresh (1.0);
01568 outrem2_.filter (output2);
01569
01570 fromPCLPointCloud2 (output2, output);
01571
01572 EXPECT_EQ (int (output.points.size ()), 352);
01573 EXPECT_EQ (int (output.width), 352);
01574 EXPECT_EQ (bool (output.is_dense), true);
01575 EXPECT_EQ (int (output.points.size ()), cloud_blob->width*cloud_blob->height-outrem2_.getRemovedIndices()->size());
01576 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.034667, 1e-4);
01577 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.15131, 1e-4);
01578 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.00071029, 1e-4);
01579
01580 outrem2_.setNegative (true);
01581 outrem2_.filter (output2);
01582
01583 fromPCLPointCloud2 (output2, output);
01584
01585 EXPECT_EQ (int (output.points.size ()), int (cloud->points.size ()) - 352);
01586 EXPECT_EQ (int (output.width), int (cloud->width) - 352);
01587 EXPECT_EQ (bool (output.is_dense), true);
01588 EXPECT_EQ (int (output.points.size ()), cloud_blob->width*cloud_blob->height-outrem2_.getRemovedIndices()->size());
01589 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.07793, 1e-4);
01590 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.17516, 1e-4);
01591 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.0444, 1e-4);
01592 }
01593
01595 TEST (ConditionalRemoval, Filters)
01596 {
01597
01598 PointCloud<PointXYZ> output;
01599
01600
01601 ConditionAnd<PointXYZ>::Ptr range_cond (new ConditionAnd<PointXYZ> ());
01602 range_cond->addComparison (FieldComparison<PointXYZ>::ConstPtr (new FieldComparison<PointXYZ> ("z",
01603 ComparisonOps::GT,
01604 0.02)));
01605 range_cond->addComparison (FieldComparison<PointXYZ>::ConstPtr (new FieldComparison<PointXYZ> ("z",
01606 ComparisonOps::LT,
01607 0.04)));
01608 range_cond->addComparison (FieldComparison<PointXYZ>::ConstPtr (new FieldComparison<PointXYZ> ("y",
01609 ComparisonOps::GT,
01610 0.10)));
01611 range_cond->addComparison (FieldComparison<PointXYZ>::ConstPtr (new FieldComparison<PointXYZ> ("y",
01612 ComparisonOps::LT,
01613 0.12)));
01614
01615
01616 ConditionalRemoval<PointXYZ> condrem (range_cond);
01617 condrem.setInputCloud (cloud);
01618
01619
01620 condrem.setKeepOrganized (false);
01621 condrem.filter (output);
01622
01623 EXPECT_EQ (bool (output.isOrganized ()), false);
01624 EXPECT_EQ (int (output.points.size ()), 28);
01625 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.087292, 1e-4);
01626 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.103140, 1e-4);
01627 EXPECT_NEAR (output.points[output.points.size () - 1].z, 0.020825, 1e-4);
01628 EXPECT_EQ (bool (output.is_dense), true);
01629
01630
01631 condrem.setKeepOrganized (true);
01632 condrem.filter (output);
01633
01634 int num_not_nan = 0;
01635 for (size_t i = 0; i < output.points.size (); i++)
01636 {
01637 if (pcl_isfinite (output.points[i].x) &&
01638 pcl_isfinite (output.points[i].y) &&
01639 pcl_isfinite (output.points[i].z))
01640 num_not_nan++;
01641 }
01642
01643 EXPECT_EQ (bool (output.isOrganized ()), bool (cloud->isOrganized ()));
01644 EXPECT_EQ (int (output.points.size ()), int (cloud->points.size ()));
01645 EXPECT_EQ (int (output.width), int (cloud->width));
01646 EXPECT_EQ (int (output.height), int (cloud->height));
01647 EXPECT_EQ (num_not_nan, 28);
01648 EXPECT_EQ (bool (output.is_dense), false);
01649
01650
01651 ConditionalRemoval<PointXYZ> condrem_ (range_cond,true);
01652 condrem_.setInputCloud (cloud);
01653
01654
01655 condrem_.setKeepOrganized (false);
01656 condrem_.filter (output);
01657
01658 EXPECT_EQ (bool (output.isOrganized ()), false);
01659 EXPECT_EQ (int (output.points.size ()), 28);
01660 EXPECT_EQ (int (output.points.size ()), cloud->points.size()-condrem_.getRemovedIndices()->size());
01661 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.087292, 1e-4);
01662 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.103140, 1e-4);
01663 EXPECT_NEAR (output.points[output.points.size () - 1].z, 0.020825, 1e-4);
01664 EXPECT_EQ (bool (output.is_dense), true);
01665
01666
01667 condrem_.setKeepOrganized (true);
01668 condrem_.filter (output);
01669
01670 num_not_nan = 0;
01671 for (size_t i = 0; i < output.points.size (); i++)
01672 {
01673 if (pcl_isfinite (output.points[i].x) &&
01674 pcl_isfinite (output.points[i].y) &&
01675 pcl_isfinite (output.points[i].z))
01676 num_not_nan++;
01677 }
01678
01679 EXPECT_EQ (bool (output.isOrganized ()), bool (cloud->isOrganized ()));
01680 EXPECT_EQ (int (output.points.size ()), int (cloud->points.size ()));
01681 EXPECT_EQ (int (output.width), int (cloud->width));
01682 EXPECT_EQ (int (output.height), int (cloud->height));
01683 EXPECT_EQ (num_not_nan, 28);
01684 EXPECT_EQ (bool (output.is_dense), false);
01685 EXPECT_EQ (int (num_not_nan), cloud->points.size()-condrem_.getRemovedIndices()->size());
01686 }
01687
01689 TEST (ConditionalRemovalSetIndices, Filters)
01690 {
01691
01692 PointCloud<PointXYZ> output;
01693
01694
01695 boost::shared_ptr<vector<int> > indices (new vector<int> (2));
01696 (*indices)[0] = 0;
01697 (*indices)[1] = static_cast<int> (cloud->points.size ()) - 1;
01698
01699
01700 ConditionAnd<PointXYZ>::Ptr true_cond (new ConditionAnd<PointXYZ> ());
01701 true_cond->addComparison (TfQuadraticXYZComparison<PointXYZ>::ConstPtr (new TfQuadraticXYZComparison<PointXYZ> (ComparisonOps::EQ, Eigen::Matrix3f::Zero (),
01702 Eigen::Vector3f::Zero (), 0)));
01703
01704
01705 ConditionalRemoval<PointXYZ> condrem2 (true_cond);
01706 condrem2.setInputCloud (cloud);
01707 condrem2.setIndices (indices);
01708
01709
01710 condrem2.setKeepOrganized (false);
01711 condrem2.filter (output);
01712
01713 EXPECT_EQ (int (output.points.size ()), 2);
01714 EXPECT_EQ (int (output.width), 2);
01715 EXPECT_EQ (int (output.height), 1);
01716
01717 EXPECT_EQ (cloud->points[0].x, output.points[0].x);
01718 EXPECT_EQ (cloud->points[0].y, output.points[0].y);
01719 EXPECT_EQ (cloud->points[0].z, output.points[0].z);
01720
01721 EXPECT_EQ (cloud->points[cloud->points.size () - 1].x, output.points[1].x);
01722 EXPECT_EQ (cloud->points[cloud->points.size () - 1].y, output.points[1].y);
01723 EXPECT_EQ (cloud->points[cloud->points.size () - 1].z, output.points[1].z);
01724
01725
01726 condrem2.setKeepOrganized (true);
01727 condrem2.filter (output);
01728
01729 EXPECT_EQ (cloud->points[0].x, output.points[0].x);
01730 EXPECT_EQ (cloud->points[0].y, output.points[0].y);
01731 EXPECT_EQ (cloud->points[0].z, output.points[0].z);
01732
01733 EXPECT_EQ (cloud->points[cloud->points.size () - 1].x, output.points[output.points.size () - 1].x);
01734 EXPECT_EQ (cloud->points[cloud->points.size () - 1].y, output.points[output.points.size () - 1].y);
01735 EXPECT_EQ (cloud->points[cloud->points.size () - 1].z, output.points[output.points.size () - 1].z);
01736
01737 int num_not_nan = 0;
01738 for (size_t i = 0; i < output.points.size (); i++)
01739 {
01740 if (pcl_isfinite (output.points[i].x) &&
01741 pcl_isfinite (output.points[i].y) &&
01742 pcl_isfinite (output.points[i].z))
01743 num_not_nan++;
01744 }
01745
01746 EXPECT_EQ (int (output.points.size ()), int (cloud->points.size ()));
01747 EXPECT_EQ (int (output.width), int (cloud->width));
01748 EXPECT_EQ (int (output.height), int (cloud->height));
01749 EXPECT_EQ (num_not_nan, 2);
01750
01751
01752 ConditionalRemoval<PointXYZ> condrem2_ (true_cond, true);
01753 condrem2_.setIndices (indices);
01754 condrem2_.setInputCloud (cloud);
01755
01756
01757 condrem2_.setKeepOrganized (false);
01758 condrem2_.filter (output);
01759
01760 EXPECT_EQ (int (output.points.size ()), 2);
01761 EXPECT_EQ (int (output.width), 2);
01762 EXPECT_EQ (int (output.height), 1);
01763
01764 EXPECT_EQ (cloud->points[0].x, output.points[0].x);
01765 EXPECT_EQ (cloud->points[0].y, output.points[0].y);
01766 EXPECT_EQ (cloud->points[0].z, output.points[0].z);
01767
01768 EXPECT_EQ (cloud->points[cloud->points.size () - 1].x, output.points[1].x);
01769 EXPECT_EQ (cloud->points[cloud->points.size () - 1].y, output.points[1].y);
01770 EXPECT_EQ (cloud->points[cloud->points.size () - 1].z, output.points[1].z);
01771
01772 EXPECT_EQ (int (output.points.size ()), int (indices->size ()) - int (condrem2_.getRemovedIndices ()->size ()));
01773
01774
01775 condrem2_.setKeepOrganized (true);
01776 condrem2_.filter (output);
01777
01778 EXPECT_EQ (cloud->points[0].x, output.points[0].x);
01779 EXPECT_EQ (cloud->points[0].y, output.points[0].y);
01780 EXPECT_EQ (cloud->points[0].z, output.points[0].z);
01781
01782 EXPECT_EQ (cloud->points[cloud->points.size () - 1].x, output.points[output.points.size () - 1].x);
01783 EXPECT_EQ (cloud->points[cloud->points.size () - 1].y, output.points[output.points.size () - 1].y);
01784 EXPECT_EQ (cloud->points[cloud->points.size () - 1].z, output.points[output.points.size () - 1].z);
01785
01786 num_not_nan = 0;
01787 for (size_t i = 0; i < output.points.size (); i++)
01788 {
01789 if (pcl_isfinite (output.points[i].x) &&
01790 pcl_isfinite (output.points[i].y) &&
01791 pcl_isfinite (output.points[i].z))
01792 num_not_nan++;
01793 }
01794
01795 EXPECT_EQ (int (output.points.size ()), int (cloud->points.size ()));
01796 EXPECT_EQ (int (output.width), int (cloud->width));
01797 EXPECT_EQ (int (output.height), int (cloud->height));
01798 EXPECT_EQ (num_not_nan, 2);
01799
01800 EXPECT_EQ (num_not_nan, int (indices->size ()) - int (condrem2_.getRemovedIndices ()->size ()));
01801 }
01802
01804 TEST (SamplingSurfaceNormal, Filters)
01805 {
01806 PointCloud <PointNormal>::Ptr incloud (new PointCloud <PointNormal> ());
01807 PointCloud <PointNormal> outcloud;
01808
01809
01810 for (float i = 0.0f; i < 5.0f; i += 0.1f)
01811 {
01812 for (float j = 0.0f; j < 5.0f; j += 0.1f)
01813 {
01814 PointNormal pt;
01815 pt.x = i;
01816 pt.y = j;
01817 pt.z = 1;
01818 incloud->points.push_back (pt);
01819 }
01820 }
01821 incloud->width = 1;
01822 incloud->height = uint32_t (incloud->points.size ());
01823
01824 pcl::SamplingSurfaceNormal <pcl::PointNormal> ssn_filter;
01825 ssn_filter.setInputCloud (incloud);
01826 ssn_filter.setRatio (0.3f);
01827 ssn_filter.filter (outcloud);
01828
01829
01830 for (unsigned int i = 0; i < outcloud.points.size (); i++)
01831 {
01832 EXPECT_NEAR (outcloud.points[i].normal[0], 0, 1e-3);
01833 EXPECT_NEAR (outcloud.points[i].normal[1], 0, 1e-3);
01834 EXPECT_NEAR (outcloud.points[i].normal[2], 1, 1e-3);
01835 }
01836 }
01837
01839 TEST (ShadowPoints, Filters)
01840 {
01841
01842 PointCloud<PointXYZ>::Ptr input (new PointCloud<PointXYZ> ());
01843 for (float i = 0.0f; i < 10.0f; i+=0.1f)
01844 {
01845 for (float j = 0.0f; j < 10.0f; j+=0.1f)
01846 {
01847 input->push_back (PointXYZ (i, j, 1.0f));
01848 }
01849 }
01850
01851
01852 PointXYZ pt (.0f, .0f, .1f);
01853 input->points.push_back (pt);
01854
01855 input->width = 1;
01856 input->height = static_cast<uint32_t> (input->points.size ());
01857
01858 NormalEstimation<PointXYZ, PointNormal> ne;
01859 ne.setInputCloud (input);
01860
01861 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
01862 ne.setSearchMethod (tree);
01863
01864 pcl::PointCloud<PointNormal>::Ptr input_normals (new PointCloud<PointNormal>);
01865 ne.setKSearch (15);
01866 ne.compute (*input_normals);
01867
01868 PointCloud<PointXYZ> output;
01869 ShadowPoints <PointXYZ, PointNormal> spfilter (true);
01870 spfilter.setInputCloud (input);
01871 spfilter.setThreshold (0.1f);
01872 spfilter.setNormals (input_normals);
01873
01874 spfilter.filter (output);
01875
01876
01877 EXPECT_EQ (int (output.points.size ()), 10000);
01878 pcl::IndicesConstPtr removed = spfilter.getRemovedIndices ();
01879 EXPECT_EQ (int (removed->size ()), 1);
01880 EXPECT_EQ (removed->at (0), output.points.size ());
01881
01882 spfilter.setKeepOrganized (true);
01883 spfilter.filter (output);
01884 EXPECT_EQ (output.size (), input->size ());
01885 EXPECT_TRUE (pcl_isnan (output.at (input->size () - 1).x));
01886 removed = spfilter.getRemovedIndices ();
01887 EXPECT_EQ (int (removed->size ()), 1);
01888
01889
01890 spfilter.setKeepOrganized (false);
01891 spfilter.setNegative (true);
01892 spfilter.filter (output);
01893 EXPECT_EQ (int (output.points.size ()), 1);
01894 EXPECT_EQ (output.at (0).x, pt.x);
01895 EXPECT_EQ (output.at (0).y, pt.y);
01896 EXPECT_EQ (output.at (0).z, pt.z);
01897 removed = spfilter.getRemovedIndices ();
01898 EXPECT_EQ (int (removed->size ()), 10000);
01899 }
01900
01901
01903 TEST (FrustumCulling, Filters)
01904 {
01905
01906 PointCloud<PointXYZ>::Ptr input (new PointCloud<PointXYZ> ());
01907
01908 for (int i = 0; i < 5; i++)
01909 {
01910 for (int j = 0; j < 5; j++)
01911 {
01912 for (int k = 0; k < 5; k++)
01913 {
01914 pcl::PointXYZ pt;
01915 pt.x = float (i);
01916 pt.y = float (j);
01917 pt.z = float (k);
01918 input->points.push_back (pt);
01919 }
01920 }
01921 }
01922 input->width = 1;
01923 input->height = static_cast<uint32_t> (input->points.size ());
01924
01925 pcl::FrustumCulling<pcl::PointXYZ> fc (true);
01926 fc.setInputCloud (input);
01927 fc.setVerticalFOV (90);
01928 fc.setHorizontalFOV (90);
01929 fc.setNearPlaneDistance (0.0);
01930 fc.setFarPlaneDistance (10);
01931
01932 Eigen::Matrix4f camera_pose;
01933 camera_pose.setZero ();
01934
01935 Eigen::Matrix3f R;
01936 R = Eigen::AngleAxisf (0 * M_PI / 180, Eigen::Vector3f::UnitX ()) *
01937 Eigen::AngleAxisf (0 * M_PI / 180, Eigen::Vector3f::UnitY ()) *
01938 Eigen::AngleAxisf (0 * M_PI / 180, Eigen::Vector3f::UnitZ ());
01939
01940 camera_pose.block (0, 0, 3, 3) = R;
01941
01942 Eigen::Vector3f T;
01943 T (0) = -5; T (1) = 0; T (2) = 0;
01944 camera_pose.block (0, 3, 3, 1) = T;
01945 camera_pose (3, 3) = 1;
01946
01947 fc.setCameraPose (camera_pose);
01948
01949 pcl::PointCloud <pcl::PointXYZ>::Ptr output (new pcl::PointCloud <pcl::PointXYZ>);
01950 fc.filter (*output);
01951
01952
01953 EXPECT_EQ (output->points.size (), input->points.size ());
01954 pcl::IndicesConstPtr removed;
01955 removed = fc.getRemovedIndices ();
01956 EXPECT_EQ (int (removed->size ()), 0);
01957
01958 fc.setNegative (true);
01959 fc.filter (*output);
01960 EXPECT_EQ (int (output->size ()), 0);
01961 removed = fc.getRemovedIndices ();
01962 EXPECT_EQ (removed->size (), input->size ());
01963
01964 fc.setKeepOrganized (true);
01965 fc.filter (*output);
01966 EXPECT_EQ (output->size (), input->size ());
01967 for (size_t i = 0; i < output->size (); i++)
01968 {
01969 EXPECT_TRUE (pcl_isnan (output->at (i).x));
01970 EXPECT_TRUE (pcl_isnan (output->at (i).y));
01971 EXPECT_TRUE (pcl_isnan (output->at (i).z));
01972 }
01973 removed = fc.getRemovedIndices ();
01974 EXPECT_EQ (removed->size (), input->size ());
01975
01976
01977 }
01978
01980 TEST (ConditionalRemovalTfQuadraticXYZComparison, Filters)
01981 {
01982
01983 PointCloud<PointXYZ> output;
01984
01985
01986 PointCloud<PointXYZ>::Ptr input (new PointCloud<PointXYZ> ());
01987
01988 input->push_back (PointXYZ (-1.0, 0.0, 0.0));
01989 input->push_back (PointXYZ (0.0, 0.0, 0.0));
01990 input->push_back (PointXYZ (1.0, 0.0, 0.0));
01991 input->push_back (PointXYZ (2.0, 0.0, 0.0));
01992 input->push_back (PointXYZ (3.0, 0.0, 0.0));
01993 input->push_back (PointXYZ (4.0, 0.0, 0.0));
01994 input->push_back (PointXYZ (5.0, 0.0, 0.0));
01995 input->push_back (PointXYZ (6.0, 0.0, 0.0));
01996 input->push_back (PointXYZ (7.0, 0.0, 0.0));
01997 input->push_back (PointXYZ (8.0, 0.0, 0.0));
01998
01999
02000 ConditionAnd<PointXYZ>::Ptr cyl_cond (new ConditionAnd<PointXYZ> ());
02001 Eigen::Matrix3f cylinderMatrix;
02002 cylinderMatrix << 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
02003 float cylinderScalar = -4;
02004 TfQuadraticXYZComparison<PointXYZ>::Ptr cyl_comp (new TfQuadraticXYZComparison<PointXYZ> (ComparisonOps::LE, cylinderMatrix,
02005 Eigen::Vector3f::Zero (), cylinderScalar));
02006 cyl_cond->addComparison (cyl_comp);
02007
02008
02009 ConditionalRemoval<PointXYZ> condrem (cyl_cond);
02010 condrem.setInputCloud (input);
02011 condrem.setKeepOrganized (false);
02012
02013
02014 condrem.filter (output);
02015
02016 EXPECT_EQ (10, int (output.points.size ()));
02017
02018 EXPECT_EQ (input->points[0].x, output.points[0].x);
02019 EXPECT_EQ (input->points[0].y, output.points[0].y);
02020 EXPECT_EQ (input->points[0].z, output.points[0].z);
02021
02022 EXPECT_EQ (input->points[9].x, output.points[9].x);
02023 EXPECT_EQ (input->points[9].y, output.points[9].y);
02024 EXPECT_EQ (input->points[9].z, output.points[9].z);
02025
02026
02027 cyl_comp->transformComparison (getTransformation (0.0f, 0.0f, 0.0f, 0.0f, 0.0f, float (M_PI) / 2.0f).inverse ());
02028
02029 condrem.filter (output);
02030
02031 EXPECT_EQ (4, int (output.points.size ()));
02032
02033 EXPECT_EQ (input->points[0].x, output.points[0].x);
02034 EXPECT_EQ (input->points[0].y, output.points[0].y);
02035 EXPECT_EQ (input->points[0].z, output.points[0].z);
02036
02037 EXPECT_EQ (input->points[3].x, output.points[3].x);
02038 EXPECT_EQ (input->points[3].y, output.points[3].y);
02039 EXPECT_EQ (input->points[3].z, output.points[3].z);
02040
02041
02042 Eigen::Vector3f planeVector;
02043 planeVector << 1.0, 0.0, 0.0;
02044 Eigen::Matrix3f planeMatrix = Eigen::Matrix3f::Zero ();
02045 cyl_comp->setComparisonMatrix (planeMatrix);
02046 cyl_comp->setComparisonVector (planeVector);
02047 cyl_comp->setComparisonScalar (-2 * 5.0);
02048 cyl_comp->setComparisonOperator (ComparisonOps::LT);
02049
02050 condrem.filter (output);
02051
02052 EXPECT_EQ (6, int (output.points.size ()));
02053
02054 EXPECT_EQ (input->points[0].x, output.points[0].x);
02055 EXPECT_EQ (input->points[0].y, output.points[0].y);
02056 EXPECT_EQ (input->points[0].z, output.points[0].z);
02057
02058 EXPECT_EQ (input->points[5].x, output.points[5].x);
02059 EXPECT_EQ (input->points[5].y, output.points[5].y);
02060 EXPECT_EQ (input->points[5].z, output.points[5].z);
02061 }
02062
02063
02065 TEST (MedianFilter, Filters)
02066 {
02067
02068
02069
02070
02071
02072
02073
02074 PointCloud<PointXYZ> cloud_manual;
02075 cloud_manual.height = 5;
02076 cloud_manual.width = 5;
02077 cloud_manual.is_dense = false;
02078 cloud_manual.resize (5 * 5);
02079
02080 for (size_t i = 0; i < 5; ++i)
02081 {
02082 cloud_manual (i, 0).z = static_cast<float> (i + 1);
02083 cloud_manual (i, 1).z = static_cast<float> (i + 6);
02084 cloud_manual (i, 2).z = static_cast<float> (10 - i);
02085 cloud_manual (i, 3).z = static_cast<float> (5 - i);
02086 cloud_manual (i, 4).z = static_cast<float> (100);
02087 }
02088 cloud_manual (2, 4).z = 500;
02089
02090
02091 MedianFilter<PointXYZ> median_filter;
02092 median_filter.setInputCloud (cloud_manual.makeShared ());
02093 median_filter.setWindowSize (3);
02094
02095 PointCloud<PointXYZ> out_1;
02096 median_filter.filter (out_1);
02097
02098
02099
02100
02101
02102
02103
02104
02105 PointCloud<PointXYZ> out_1_correct;
02106 out_1_correct.height = 5;
02107 out_1_correct.width = 5;
02108 out_1_correct.is_dense = false;
02109 out_1_correct.resize (5 * 5);
02110 out_1_correct (0, 0).z = 6.f;
02111 out_1_correct (1, 0).z = 6.f;
02112 out_1_correct (2, 0).z = 7.f;
02113 out_1_correct (3, 0).z = 8.f;
02114 out_1_correct (4, 0).z = 9.f;
02115
02116 out_1_correct (0, 1).z = 7.f;
02117 out_1_correct (1, 1).z = 7.f;
02118 out_1_correct (2, 1).z = 7.f;
02119 out_1_correct (3, 1).z = 7.f;
02120 out_1_correct (4, 1).z = 7.f;
02121
02122 out_1_correct (0, 2).z = 7.f;
02123 out_1_correct (1, 2).z = 7.f;
02124 out_1_correct (2, 2).z = 7.f;
02125 out_1_correct (3, 2).z = 7.f;
02126 out_1_correct (4, 2).z = 7.f;
02127
02128 out_1_correct (0, 3).z = 10.f;
02129 out_1_correct (1, 3).z = 9.f;
02130 out_1_correct (2, 3).z = 8.f;
02131 out_1_correct (3, 3).z = 7.f;
02132 out_1_correct (4, 3).z = 7.f;
02133
02134 out_1_correct (0, 4).z = 100.f;
02135 out_1_correct (1, 4).z = 100.f;
02136 out_1_correct (2, 4).z = 100.f;
02137 out_1_correct (3, 4).z = 100.f;
02138 out_1_correct (4, 4).z = 100.f;
02139
02140 for (size_t i = 0; i < 5 * 5; ++i)
02141 EXPECT_NEAR (out_1_correct[i].z, out_1[i].z, 1e-5);
02142
02143
02144
02145 PointCloud<PointXYZ> out_2;
02146 median_filter.setMaxAllowedMovement (50.f);
02147 median_filter.filter (out_2);
02148
02149
02150
02151
02152
02153
02154
02155
02156 PointCloud<PointXYZ> out_2_correct;
02157 out_2_correct = out_1_correct;
02158 out_2_correct (2, 4).z = 450.f;
02159
02160 for (size_t i = 0; i < 5 * 5; ++i)
02161 EXPECT_NEAR (out_2_correct[i].z, out_2[i].z, 1e-5);
02162
02163
02164
02165 MedianFilter<PointXYZRGB> median_filter_xyzrgb;
02166 median_filter_xyzrgb.setInputCloud (cloud_organized);
02167 median_filter_xyzrgb.setWindowSize (7);
02168 median_filter_xyzrgb.setMaxAllowedMovement (0.01f);
02169 PointCloud<PointXYZRGB> out_3;
02170 median_filter_xyzrgb.filter (out_3);
02171
02172
02173 EXPECT_NEAR (1.300999999f, out_3(30, 100).z, 1e-5);
02174 EXPECT_NEAR (1.300999999f, out_3(50, 100).z, 1e-5);
02175 EXPECT_NEAR (1.305999994f, out_3(90, 100).z, 1e-5);
02176 EXPECT_NEAR (0.908000111f, out_3(50, 200).z, 1e-5);
02177 EXPECT_NEAR (0.695000112f, out_3(100, 300).z, 1e-5);
02178 EXPECT_NEAR (1.177000045f, out_3(128, 128).z, 1e-5);
02179 EXPECT_NEAR (0.778999984f, out_3(256, 256).z, 1e-5);
02180 EXPECT_NEAR (0.703000009f, out_3(428, 300).z, 1e-5);
02181 }
02182
02183
02185 #include <pcl/common/time.h>
02186 TEST (NormalRefinement, Filters)
02187 {
02188
02189
02190
02191
02192
02193 pcl::PointCloud<pcl::PointXYZRGB> cloud_organized_nonan;
02194 std::vector<int> dummy;
02195 pcl::removeNaNFromPointCloud<pcl::PointXYZRGB> (*cloud_organized, cloud_organized_nonan, dummy);
02196
02197
02198 const float vp_x = cloud_organized_nonan.sensor_origin_[0];
02199 const float vp_y = cloud_organized_nonan.sensor_origin_[1];
02200 const float vp_z = cloud_organized_nonan.sensor_origin_[2];
02201
02202
02203 const int k = 5;
02204 std::vector<std::vector<int> > k_indices;
02205 std::vector<std::vector<float> > k_sqr_distances;
02206
02207
02208 pcl::PointCloud<pcl::PointXYZRGBNormal> cloud_organized_normal;
02209 pcl::PointCloud<pcl::PointXYZRGBNormal> cloud_organized_normal_refined;
02210
02211
02212
02213
02214
02215
02216 pcl::search::KdTree<pcl::PointXYZRGB> kdtree;
02217 kdtree.setInputCloud (cloud_organized_nonan.makeShared ());
02218 kdtree.nearestKSearch (cloud_organized_nonan, std::vector<int> (), k, k_indices, k_sqr_distances);
02219
02220
02221
02222
02223
02224
02225 pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> ne;
02226 cloud_organized_normal.reserve (cloud_organized_nonan.size ());
02227 for (unsigned int i = 0; i < cloud_organized_nonan.size (); ++i)
02228 {
02229
02230 pcl::PointXYZRGBNormal normali;
02231
02232 std::memcpy (normali.data, cloud_organized_nonan[i].data, 3*sizeof (float));
02233 normali.rgba = cloud_organized_nonan[i].rgba;
02234
02235 ne.computePointNormal (cloud_organized_nonan, k_indices[i], normali.normal_x, normali.normal_y, normali.normal_z, normali.curvature);
02236 pcl::flipNormalTowardsViewpoint (cloud_organized_nonan[i], vp_x, vp_y, vp_z, normali.normal_x, normali.normal_y, normali.normal_z);
02237
02238 cloud_organized_normal.push_back (normali);
02239 }
02240
02241
02242
02243
02244
02245
02246 pcl::NormalRefinement<pcl::PointXYZRGBNormal> nr (k_indices, k_sqr_distances);
02247 nr.setInputCloud (cloud_organized_normal.makeShared());
02248 nr.setMaxIterations (15);
02249 nr.setConvergenceThreshold (0.00001f);
02250 nr.filter (cloud_organized_normal_refined);
02251
02252
02253
02254
02255
02256
02257 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
02258 pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
02259 pcl::SACSegmentation<pcl::PointXYZRGBNormal> seg;
02260 seg.setOptimizeCoefficients (true);
02261 seg.setModelType (pcl::SACMODEL_PLANE);
02262 seg.setMethodType (pcl::SAC_RANSAC);
02263 seg.setDistanceThreshold (0.005);
02264 seg.setInputCloud (cloud_organized_normal.makeShared ());
02265 seg.segment (*inliers, *coefficients);
02266
02267
02268 const std::vector<int>& idx_table = inliers->indices;
02269 float a = coefficients->values[0];
02270 float b = coefficients->values[1];
02271 float c = coefficients->values[2];
02272 const float d = coefficients->values[3];
02273
02274
02275 pcl::PointXYZ p_table;
02276 p_table.x = 0.0f;
02277 p_table.y = 0.0f;
02278 p_table.z = -d / c;
02279
02280
02281 pcl::flipNormalTowardsViewpoint (p_table, vp_x, vp_y, vp_z, a, b, c);
02282
02283
02284
02285
02286
02287
02288 std::vector<float> errs_est;
02289 float err_est_mean = 0.0f;
02290 std::vector<float> errs_refined;
02291 float err_refined_mean = 0.0f;
02292
02293
02294 int num_zeros = 0;
02295 int num_nans = 0;
02296
02297
02298 for (unsigned int i = 0; i < idx_table.size (); ++i)
02299 {
02300 float tmp;
02301
02302
02303 const pcl::PointXYZRGBNormal& calci = cloud_organized_normal[idx_table[i]];
02304 if ((fabsf (calci.normal_x) + fabsf (calci.normal_y) + fabsf (calci.normal_z)) > 0.0f)
02305 {
02306 tmp = 1.0f - (calci.normal_x * a + calci.normal_y * b + calci.normal_z * c);
02307 if (pcl_isfinite (tmp))
02308 {
02309 errs_est.push_back (tmp);
02310 err_est_mean += tmp;
02311 }
02312 }
02313
02314
02315 const pcl::PointXYZRGBNormal& refinedi = cloud_organized_normal_refined[idx_table[i]];
02316 if ((fabsf (refinedi.normal_x) + fabsf (refinedi.normal_y) + fabsf (refinedi.normal_z)) > 0.0f)
02317 {
02318 tmp = 1.0f - (refinedi.normal_x * a + refinedi.normal_y * b + refinedi.normal_z * c);
02319 if (pcl_isfinite(tmp))
02320 {
02321 errs_refined.push_back (tmp);
02322 err_refined_mean += tmp;
02323 }
02324 else
02325 {
02326
02327 ++num_nans;
02328 }
02329 } else
02330 {
02331
02332 ++num_zeros;
02333 }
02334 }
02335
02336
02337 err_est_mean /= static_cast<float> (errs_est.size ());
02338 err_refined_mean /= static_cast<float> (errs_refined.size ());
02339
02340
02341 float err_est_var = 0.0f;
02342 for (unsigned int i = 0; i < errs_est.size (); ++i)
02343 err_est_var = (errs_est[i] - err_est_mean) * (errs_est[i] - err_est_mean);
02344 err_est_var /= static_cast<float> (errs_est.size () - 1);
02345
02346
02347 float err_refined_var = 0.0f;
02348 for (unsigned int i = 0; i < errs_refined.size (); ++i)
02349 err_refined_var = (errs_refined[i] - err_refined_mean) * (errs_refined[i] - err_refined_mean);
02350 err_refined_var /= static_cast<float> (errs_refined.size () - 1);
02351
02352
02353 EXPECT_EQ(num_zeros, 0);
02354 EXPECT_EQ(num_nans, 0);
02355
02356
02357 EXPECT_LT(err_refined_mean, err_est_mean);
02358 EXPECT_LT(err_refined_var, err_est_var);
02359 }
02360
02361
02362 int
02363 main (int argc, char** argv)
02364 {
02365
02366 if (argc < 3)
02367 {
02368 std::cerr << "No test file given. Please download `bun0.pcd` and `milk_cartoon_all_small_clorox.pcd` and pass their paths to the test." << std::endl;
02369 return (-1);
02370 }
02371
02372 char* file_name = argv[1];
02373
02374 loadPCDFile (file_name, *cloud_blob);
02375 fromPCLPointCloud2 (*cloud_blob, *cloud);
02376
02377 indices_.resize (cloud->points.size ());
02378 for (int i = 0; i < static_cast<int> (indices_.size ()); ++i)
02379 indices_[i] = i;
02380
02381
02382 loadPCDFile (argv[2], *cloud_organized);
02383
02384
02385 testing::InitGoogleTest (&argc, argv);
02386 return (RUN_ALL_TESTS ());
02387 }
02388