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 #include <gtest/gtest.h>
00041 #include <pcl/point_types.h>
00042 #include <pcl/io/pcd_io.h>
00043 #include <pcl/filters/filter.h>
00044 #include <pcl/filters/passthrough.h>
00045 #include <pcl/filters/voxel_grid.h>
00046 #include <pcl/filters/extract_indices.h>
00047 #include <pcl/filters/project_inliers.h>
00048 #include <pcl/filters/radius_outlier_removal.h>
00049 #include <pcl/filters/statistical_outlier_removal.h>
00050 #include <pcl/filters/conditional_removal.h>
00051 #include <pcl/filters/random_sample.h>
00052 #include <pcl/filters/crop_box.h>
00053
00054 #include <pcl/common/transforms.h>
00055 #include <pcl/common/eigen.h>
00056
00057 using namespace pcl;
00058 using namespace pcl::io;
00059 using namespace std;
00060 using namespace sensor_msgs;
00061 using namespace Eigen;
00062
00063 const float PI = 3.141592f;
00064
00065 PointCloud2::Ptr cloud_blob (new PointCloud2);
00066 PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
00067 vector<int> indices_;
00068
00069
00070
00072 TEST (ExtractIndicesSelf, Filters)
00073 {
00074
00075 ExtractIndices<PointXYZ> ei;
00076 boost::shared_ptr<vector<int> > indices (new vector<int> (2));
00077 (*indices)[0] = 0;
00078 (*indices)[1] = static_cast<int> (cloud->points.size ()) - 1;
00079
00080 PointCloud<PointXYZ>::Ptr output (new PointCloud<PointXYZ>);
00081 ei.setInputCloud (cloud);
00082 ei.setIndices (indices);
00083 ei.filter (*output);
00084
00085 EXPECT_EQ (int (output->points.size ()), 2);
00086 EXPECT_EQ (int (output->width), 2);
00087 EXPECT_EQ (int (output->height), 1);
00088
00089 EXPECT_EQ (cloud->points[0].x, output->points[0].x);
00090 EXPECT_EQ (cloud->points[0].y, output->points[0].y);
00091 EXPECT_EQ (cloud->points[0].z, output->points[0].z);
00092
00093 EXPECT_EQ (cloud->points[cloud->points.size () - 1].x, output->points[1].x);
00094 EXPECT_EQ (cloud->points[cloud->points.size () - 1].y, output->points[1].y);
00095 EXPECT_EQ (cloud->points[cloud->points.size () - 1].z, output->points[1].z);
00096 }
00097
00099 TEST (ExtractIndices, Filters)
00100 {
00101
00102 ExtractIndices<PointXYZ> ei;
00103 boost::shared_ptr<vector<int> > indices (new vector<int> (2));
00104 (*indices)[0] = 0;
00105 (*indices)[1] = static_cast<int> (cloud->points.size ()) - 1;
00106
00107 PointCloud<PointXYZ> output;
00108 ei.setInputCloud (cloud);
00109 ei.setIndices (indices);
00110 ei.filter (output);
00111
00112 EXPECT_EQ (int (output.points.size ()), 2);
00113 EXPECT_EQ (int (output.width), 2);
00114 EXPECT_EQ (int (output.height), 1);
00115
00116 EXPECT_EQ (cloud->points[0].x, output.points[0].x);
00117 EXPECT_EQ (cloud->points[0].y, output.points[0].y);
00118 EXPECT_EQ (cloud->points[0].z, output.points[0].z);
00119
00120 EXPECT_EQ (cloud->points[cloud->points.size () - 1].x, output.points[1].x);
00121 EXPECT_EQ (cloud->points[cloud->points.size () - 1].y, output.points[1].y);
00122 EXPECT_EQ (cloud->points[cloud->points.size () - 1].z, output.points[1].z);
00123
00124 ei.setNegative (true);
00125 ei.filter (output);
00126
00127 EXPECT_EQ (output.points.size (), cloud->points.size () - 2);
00128 EXPECT_EQ (output.width, cloud->points.size () - 2);
00129 EXPECT_EQ (int (output.height), 1);
00130
00131 EXPECT_EQ (cloud->points[1].x, output.points[0].x);
00132 EXPECT_EQ (cloud->points[1].y, output.points[0].y);
00133 EXPECT_EQ (cloud->points[1].z, output.points[0].z);
00134
00135 EXPECT_EQ (cloud->points[cloud->points.size () - 2].x, output.points[output.points.size () - 1].x);
00136 EXPECT_EQ (cloud->points[cloud->points.size () - 2].y, output.points[output.points.size () - 1].y);
00137 EXPECT_EQ (cloud->points[cloud->points.size () - 2].z, output.points[output.points.size () - 1].z);
00138
00139
00140 ExtractIndices<PointCloud2> ei2;
00141
00142 PointCloud2 output_blob;
00143 ei2.setInputCloud (cloud_blob);
00144 ei2.setIndices (indices);
00145 ei2.filter (output_blob);
00146
00147 fromROSMsg (output_blob, output);
00148
00149 EXPECT_EQ (int (output.points.size ()), 2);
00150 EXPECT_EQ (int (output.width), 2);
00151 EXPECT_EQ (int (output.height), 1);
00152
00153 EXPECT_EQ (cloud->points[0].x, output.points[0].x);
00154 EXPECT_EQ (cloud->points[0].y, output.points[0].y);
00155 EXPECT_EQ (cloud->points[0].z, output.points[0].z);
00156
00157 EXPECT_EQ (cloud->points[cloud->points.size () - 1].x, output.points[1].x);
00158 EXPECT_EQ (cloud->points[cloud->points.size () - 1].y, output.points[1].y);
00159 EXPECT_EQ (cloud->points[cloud->points.size () - 1].z, output.points[1].z);
00160
00161 ei2.setNegative (true);
00162 ei2.filter (output_blob);
00163
00164 fromROSMsg (output_blob, output);
00165
00166 EXPECT_EQ (output.points.size (), cloud->points.size () - 2);
00167 EXPECT_EQ (output.width, cloud->points.size () - 2);
00168 EXPECT_EQ (int (output.height), 1);
00169
00170 EXPECT_EQ (cloud->points[1].x, output.points[0].x);
00171 EXPECT_EQ (cloud->points[1].y, output.points[0].y);
00172 EXPECT_EQ (cloud->points[1].z, output.points[0].z);
00173
00174 EXPECT_EQ (cloud->points[cloud->points.size () - 2].x, output.points[output.points.size () - 1].x);
00175 EXPECT_EQ (cloud->points[cloud->points.size () - 2].y, output.points[output.points.size () - 1].y);
00176 EXPECT_EQ (cloud->points[cloud->points.size () - 2].z, output.points[output.points.size () - 1].z);
00177
00178
00179 PointCloud<PointXYZ> empty, result;
00180 ExtractIndices<PointXYZ> eie;
00181 eie.setInputCloud (empty.makeShared ());
00182 eie.setNegative (false);
00183 eie.filter (result);
00184
00185 EXPECT_EQ (int (result.points.size ()), 0);
00186 eie.setNegative (true);
00187 eie.filter (result);
00188 EXPECT_EQ (int (result.points.size ()), 0);
00189
00190 boost::shared_ptr<vector<int> > idx (new vector<int> (10));
00191 eie.setIndices (idx);
00192 eie.setNegative (false);
00193 eie.filter (result);
00194 EXPECT_EQ (int (result.points.size ()), 0);
00195 eie.setNegative (true);
00196 eie.filter (result);
00197 EXPECT_EQ (int (result.points.size ()), 0);
00198
00199 empty.points.resize (10);
00200 empty.width = 10; empty.height = 1;
00201 eie.setInputCloud (empty.makeShared ());
00202 for (int i = 0; i < 10; ++i)
00203 (*idx)[i] = i;
00204 eie.setIndices (idx);
00205 eie.setNegative (false);
00206 eie.filter (result);
00207 EXPECT_EQ (int (result.points.size ()), 10);
00208 eie.setNegative (true);
00209 eie.filter (result);
00210 EXPECT_EQ (int (result.points.size ()), 0);
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233 }
00234
00236 TEST (PassThrough, Filters)
00237 {
00238
00239 PointCloud<PointXYZ> output;
00240 PassThrough<PointXYZ> pt;
00241
00242 pt.setInputCloud (cloud);
00243 pt.filter (output);
00244
00245 EXPECT_EQ (output.points.size (), cloud->points.size ());
00246 EXPECT_EQ (output.width, cloud->width);
00247 EXPECT_EQ (output.height, cloud->height);
00248
00249 pt.setFilterFieldName ("z");
00250 pt.setFilterLimits (0.05, 0.1);
00251 pt.filter (output);
00252
00253 EXPECT_EQ (int (output.points.size ()), 42);
00254 EXPECT_EQ (int (output.width), 42);
00255 EXPECT_EQ (int (output.height), 1);
00256 EXPECT_EQ (bool (output.is_dense), true);
00257
00258 EXPECT_NEAR (output.points[0].x, -0.074556, 1e-5);
00259 EXPECT_NEAR (output.points[0].y, 0.13415, 1e-5);
00260 EXPECT_NEAR (output.points[0].z, 0.051046, 1e-5);
00261
00262 EXPECT_NEAR (output.points[41].x, -0.030331, 1e-5);
00263 EXPECT_NEAR (output.points[41].y, 0.039749, 1e-5);
00264 EXPECT_NEAR (output.points[41].z, 0.052133, 1e-5);
00265
00266 pt.setFilterLimitsNegative (true);
00267 pt.filter (output);
00268
00269 EXPECT_EQ (int (output.points.size ()), 355);
00270 EXPECT_EQ (int (output.width), 355);
00271 EXPECT_EQ (int (output.height), 1);
00272 EXPECT_EQ (bool (output.is_dense), true);
00273
00274 EXPECT_NEAR (output.points[0].x, 0.0054216, 1e-5);
00275 EXPECT_NEAR (output.points[0].y, 0.11349, 1e-5);
00276 EXPECT_NEAR (output.points[0].z, 0.040749, 1e-5);
00277
00278 EXPECT_NEAR (output.points[354].x, -0.07793, 1e-5);
00279 EXPECT_NEAR (output.points[354].y, 0.17516, 1e-5);
00280 EXPECT_NEAR (output.points[354].z, -0.0444, 1e-5);
00281
00282 PassThrough<PointXYZ> pt_(true);
00283
00284 pt_.setInputCloud (cloud);
00285 pt_.filter (output);
00286
00287 EXPECT_EQ (pt_.getRemovedIndices()->size(), 0);
00288 EXPECT_EQ (output.points.size (), cloud->points.size ());
00289 EXPECT_EQ (output.width, cloud->width);
00290 EXPECT_EQ (output.height, cloud->height);
00291
00292 pt_.setFilterFieldName ("z");
00293 pt_.setFilterLimits (0.05, 0.1);
00294 pt_.filter (output);
00295
00296 EXPECT_EQ (int (output.points.size ()), 42);
00297 EXPECT_EQ (int (output.width), 42);
00298 EXPECT_EQ (int (output.height), 1);
00299 EXPECT_EQ (bool (output.is_dense), true);
00300 EXPECT_EQ (int (output.points.size ()), cloud->points.size ()-pt_.getRemovedIndices()->size());
00301
00302 EXPECT_NEAR (output.points[0].x, -0.074556, 1e-5);
00303 EXPECT_NEAR (output.points[0].y, 0.13415, 1e-5);
00304 EXPECT_NEAR (output.points[0].z, 0.051046, 1e-5);
00305
00306 EXPECT_NEAR (output.points[41].x, -0.030331, 1e-5);
00307 EXPECT_NEAR (output.points[41].y, 0.039749, 1e-5);
00308 EXPECT_NEAR (output.points[41].z, 0.052133, 1e-5);
00309
00310 pt_.setFilterLimitsNegative (true);
00311 pt_.filter (output);
00312
00313 EXPECT_EQ (int (output.points.size ()), 355);
00314 EXPECT_EQ (int (output.width), 355);
00315 EXPECT_EQ (int (output.height), 1);
00316 EXPECT_EQ (bool (output.is_dense), true);
00317 EXPECT_EQ (int (output.points.size ()), cloud->points.size ()-pt_.getRemovedIndices()->size());
00318
00319 EXPECT_NEAR (output.points[0].x, 0.0054216, 1e-5);
00320 EXPECT_NEAR (output.points[0].y, 0.11349, 1e-5);
00321 EXPECT_NEAR (output.points[0].z, 0.040749, 1e-5);
00322
00323 EXPECT_NEAR (output.points[354].x, -0.07793, 1e-5);
00324 EXPECT_NEAR (output.points[354].y, 0.17516, 1e-5);
00325 EXPECT_NEAR (output.points[354].z, -0.0444, 1e-5);
00326
00327
00328 pt.setUserFilterValue (std::numeric_limits<float>::quiet_NaN ());
00329 pt.setFilterFieldName ("");
00330 pt.filter (output);
00331
00332 EXPECT_EQ (output.points.size (), cloud->points.size ());
00333 EXPECT_EQ (output.width, cloud->width);
00334 EXPECT_EQ (output.height, cloud->height);
00335 EXPECT_EQ (output.is_dense, cloud->is_dense);
00336 EXPECT_NEAR (output.points[0].x, cloud->points[0].x, 1e-5);
00337 EXPECT_NEAR (output.points[output.points.size () - 1].x, cloud->points[cloud->points.size () - 1].x, 1e-5);
00338
00339 pt.setFilterFieldName ("z");
00340 pt.setFilterLimitsNegative (false);
00341 pt.setKeepOrganized (true);
00342 pt.filter (output);
00343
00344 EXPECT_EQ (output.points.size (), cloud->points.size ());
00345 EXPECT_EQ (output.width, cloud->width);
00346 EXPECT_EQ (output.height, cloud->height);
00347 EXPECT_EQ (bool (output.is_dense), false);
00348
00349 if (!pcl_isnan (output.points[0].x)) EXPECT_EQ (1, 0);
00350 if (!pcl_isnan (output.points[0].y)) EXPECT_EQ (1, 0);
00351 if (!pcl_isnan (output.points[0].z)) EXPECT_EQ (1, 0);
00352
00353 if (!pcl_isnan (output.points[41].x)) EXPECT_EQ (1, 0);
00354 if (!pcl_isnan (output.points[41].y)) EXPECT_EQ (1, 0);
00355 if (!pcl_isnan (output.points[41].z)) EXPECT_EQ (1, 0);
00356
00357 pt.setFilterLimitsNegative (true);
00358 pt.filter (output);
00359
00360 EXPECT_EQ (output.points.size (), cloud->points.size ());
00361 EXPECT_EQ (output.width, cloud->width);
00362 EXPECT_EQ (output.height, cloud->height);
00363 EXPECT_EQ (bool (output.is_dense), false);
00364
00365 EXPECT_NEAR (output.points[0].x, cloud->points[0].x, 1e-5);
00366 EXPECT_NEAR (output.points[0].y, cloud->points[0].y, 1e-5);
00367 EXPECT_NEAR (output.points[0].z, cloud->points[0].z, 1e-5);
00368
00369 EXPECT_NEAR (output.points[41].x, cloud->points[41].x, 1e-5);
00370 EXPECT_NEAR (output.points[41].y, cloud->points[41].y, 1e-5);
00371 EXPECT_NEAR (output.points[41].z, cloud->points[41].z, 1e-5);
00372
00373
00374 PassThrough<PointCloud2> pt2;
00375
00376 PointCloud2 output_blob;
00377 pt2.setInputCloud (cloud_blob);
00378 pt2.filter (output_blob);
00379
00380 fromROSMsg (output_blob, output);
00381
00382 EXPECT_EQ (output.points.size (), cloud->points.size ());
00383 EXPECT_EQ (output.width, cloud->width);
00384 EXPECT_EQ (output.height, cloud->height);
00385
00386 pt2.setFilterFieldName ("z");
00387 pt2.setFilterLimits (0.05, 0.1);
00388 pt2.filter (output_blob);
00389
00390 fromROSMsg (output_blob, output);
00391
00392 EXPECT_EQ (int (output.points.size ()), 42);
00393 EXPECT_EQ (int (output.width), 42);
00394 EXPECT_EQ (int (output.height), 1);
00395 EXPECT_EQ (bool (output.is_dense), true);
00396
00397 EXPECT_NEAR (output.points[0].x, -0.074556, 1e-5);
00398 EXPECT_NEAR (output.points[0].y, 0.13415, 1e-5);
00399 EXPECT_NEAR (output.points[0].z, 0.051046, 1e-5);
00400
00401 EXPECT_NEAR (output.points[41].x, -0.030331, 1e-5);
00402 EXPECT_NEAR (output.points[41].y, 0.039749, 1e-5);
00403 EXPECT_NEAR (output.points[41].z, 0.052133, 1e-5);
00404
00405 pt2.setFilterLimitsNegative (true);
00406 pt2.filter (output_blob);
00407
00408 fromROSMsg (output_blob, output);
00409
00410 EXPECT_EQ (int (output.points.size ()), 355);
00411 EXPECT_EQ (int (output.width), 355);
00412 EXPECT_EQ (int (output.height), 1);
00413 EXPECT_EQ (bool (output.is_dense), true);
00414
00415 EXPECT_NEAR (output.points[0].x, 0.0054216, 1e-5);
00416 EXPECT_NEAR (output.points[0].y, 0.11349, 1e-5);
00417 EXPECT_NEAR (output.points[0].z, 0.040749, 1e-5);
00418
00419 EXPECT_NEAR (output.points[354].x, -0.07793, 1e-5);
00420 EXPECT_NEAR (output.points[354].y, 0.17516, 1e-5);
00421 EXPECT_NEAR (output.points[354].z, -0.0444, 1e-5);
00422
00423 PassThrough<PointCloud2> pt2_(true);
00424 pt2_.setInputCloud (cloud_blob);
00425 pt2_.filter (output_blob);
00426
00427 fromROSMsg (output_blob, output);
00428
00429 EXPECT_EQ (pt2_.getRemovedIndices()->size(), 0);
00430 EXPECT_EQ (output.points.size (), cloud->points.size ());
00431 EXPECT_EQ (output.width, cloud->width);
00432 EXPECT_EQ (output.height, cloud->height);
00433
00434 pt2_.setFilterFieldName ("z");
00435 pt2_.setFilterLimits (0.05, 0.1);
00436 pt2_.filter (output_blob);
00437
00438 fromROSMsg (output_blob, output);
00439
00440 EXPECT_EQ (int (output.points.size ()), 42);
00441 EXPECT_EQ (int (output.width), 42);
00442 EXPECT_EQ (int (output.height), 1);
00443 EXPECT_EQ (bool (output.is_dense), true);
00444 EXPECT_EQ (int (output.points.size ()), cloud->points.size ()-pt2_.getRemovedIndices()->size());
00445
00446 EXPECT_NEAR (output.points[0].x, -0.074556, 1e-5);
00447 EXPECT_NEAR (output.points[0].y, 0.13415, 1e-5);
00448 EXPECT_NEAR (output.points[0].z, 0.051046, 1e-5);
00449
00450 EXPECT_NEAR (output.points[41].x, -0.030331, 1e-5);
00451 EXPECT_NEAR (output.points[41].y, 0.039749, 1e-5);
00452 EXPECT_NEAR (output.points[41].z, 0.052133, 1e-5);
00453
00454 pt2_.setFilterLimitsNegative (true);
00455 pt2_.filter (output_blob);
00456
00457 fromROSMsg (output_blob, output);
00458
00459 EXPECT_EQ (int (output.points.size ()), 355);
00460 EXPECT_EQ (int (output.width), 355);
00461 EXPECT_EQ (int (output.height), 1);
00462 EXPECT_EQ (bool (output.is_dense), true);
00463 EXPECT_EQ (int (output.points.size ()), cloud->points.size ()-pt2_.getRemovedIndices()->size());
00464
00465 EXPECT_NEAR (output.points[0].x, 0.0054216, 1e-5);
00466 EXPECT_NEAR (output.points[0].y, 0.11349, 1e-5);
00467 EXPECT_NEAR (output.points[0].z, 0.040749, 1e-5);
00468
00469 EXPECT_NEAR (output.points[354].x, -0.07793, 1e-5);
00470 EXPECT_NEAR (output.points[354].y, 0.17516, 1e-5);
00471 EXPECT_NEAR (output.points[354].z, -0.0444, 1e-5);
00472
00473
00474 pt2.setUserFilterValue (std::numeric_limits<float>::quiet_NaN ());
00475 pt2.setFilterFieldName ("");
00476 pt2.filter (output_blob);
00477 fromROSMsg (output_blob, output);
00478
00479 EXPECT_EQ (output.points.size (), cloud->points.size ());
00480 EXPECT_EQ (output.width, cloud->width);
00481 EXPECT_EQ (output.height, cloud->height);
00482 EXPECT_EQ (output.is_dense, cloud->is_dense);
00483 EXPECT_NEAR (output.points[0].x, cloud->points[0].x, 1e-5);
00484 EXPECT_NEAR (output.points[output.points.size () - 1].x, cloud->points[cloud->points.size () - 1].x, 1e-5);
00485
00486 pt2.setFilterFieldName ("z");
00487 pt2.setFilterLimitsNegative (false);
00488 pt2.setKeepOrganized (true);
00489 pt2.filter (output_blob);
00490 fromROSMsg (output_blob, output);
00491
00492 EXPECT_EQ (output.points.size (), cloud->points.size ());
00493 EXPECT_EQ (output.width, cloud->width);
00494 EXPECT_EQ (output.height, cloud->height);
00495 EXPECT_EQ (bool (output.is_dense), false);
00496
00497 if (!pcl_isnan (output.points[0].x)) EXPECT_EQ (1, 0);
00498 if (!pcl_isnan (output.points[0].y)) EXPECT_EQ (1, 0);
00499 if (!pcl_isnan (output.points[0].z)) EXPECT_EQ (1, 0);
00500
00501 if (!pcl_isnan (output.points[41].x)) EXPECT_EQ (1, 0);
00502 if (!pcl_isnan (output.points[41].y)) EXPECT_EQ (1, 0);
00503 if (!pcl_isnan (output.points[41].z)) EXPECT_EQ (1, 0);
00504
00505 pt2.setFilterLimitsNegative (true);
00506 pt2.filter (output_blob);
00507 fromROSMsg (output_blob, output);
00508
00509 EXPECT_EQ (output.points.size (), cloud->points.size ());
00510 EXPECT_EQ (output.width, cloud->width);
00511 EXPECT_EQ (output.height, cloud->height);
00512 EXPECT_EQ (bool (output.is_dense), false);
00513
00514 EXPECT_NEAR (output.points[0].x, cloud->points[0].x, 1e-5);
00515 EXPECT_NEAR (output.points[0].y, cloud->points[0].y, 1e-5);
00516 EXPECT_NEAR (output.points[0].z, cloud->points[0].z, 1e-5);
00517
00518 EXPECT_NEAR (output.points[41].x, cloud->points[41].x, 1e-5);
00519 EXPECT_NEAR (output.points[41].y, cloud->points[41].y, 1e-5);
00520 EXPECT_NEAR (output.points[41].z, cloud->points[41].z, 1e-5);
00521 }
00522
00524 TEST (VoxelGrid, Filters)
00525 {
00526
00527 PointCloud<PointXYZ> output;
00528 VoxelGrid<PointXYZ> grid;
00529
00530 grid.setLeafSize (0.02f, 0.02f, 0.02f);
00531 grid.setInputCloud (cloud);
00532 grid.filter (output);
00533
00534 EXPECT_EQ (int (output.points.size ()), 103);
00535 EXPECT_EQ (int (output.width), 103);
00536 EXPECT_EQ (int (output.height), 1);
00537 EXPECT_EQ (bool (output.is_dense), true);
00538
00539 grid.setFilterFieldName ("z");
00540 grid.setFilterLimits (0.05, 0.1);
00541 grid.filter (output);
00542
00543 EXPECT_EQ (int (output.points.size ()), 14);
00544 EXPECT_EQ (int (output.width), 14);
00545 EXPECT_EQ (int (output.height), 1);
00546 EXPECT_EQ (bool (output.is_dense), true);
00547
00548 EXPECT_NEAR (output.points[0].x, -0.026125, 1e-4);
00549 EXPECT_NEAR (output.points[0].y, 0.039788, 1e-4);
00550 EXPECT_NEAR (output.points[0].z, 0.052827, 1e-4);
00551
00552 EXPECT_NEAR (output.points[13].x, -0.073202, 1e-4);
00553 EXPECT_NEAR (output.points[13].y, 0.1296, 1e-4);
00554 EXPECT_NEAR (output.points[13].z, 0.051333, 1e-4);
00555
00556 grid.setFilterLimitsNegative (true);
00557 grid.setSaveLeafLayout(true);
00558 grid.filter (output);
00559
00560 EXPECT_EQ (int (output.points.size ()), 100);
00561 EXPECT_EQ (int (output.width), 100);
00562 EXPECT_EQ (int (output.height), 1);
00563 EXPECT_EQ (bool (output.is_dense), true);
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574 EXPECT_EQ (grid.getCentroidIndex (output.points[0]), 0);
00575 EXPECT_EQ (grid.getCentroidIndex (output.points[99]), 99);
00576 EXPECT_EQ (grid.getCentroidIndexAt (grid.getGridCoordinates (-1,-1,-1)), -1);
00577
00578
00579
00580 int centroidIdx = grid.getCentroidIndex (cloud->points[195]);
00581
00582
00583 EXPECT_LE (fabs (output.points[centroidIdx].x - cloud->points[195].x), 0.02);
00584 EXPECT_LE (fabs (output.points[centroidIdx].y - cloud->points[195].y), 0.02);
00585 EXPECT_LE (fabs (output.points[centroidIdx].z - cloud->points[195].z), 0.02);
00586
00587
00588 EXPECT_EQ (grid.getNeighborCentroidIndices (output.points[0], Eigen::MatrixXi::Zero(3,1))[0], 0);
00589 EXPECT_EQ (grid.getNeighborCentroidIndices (output.points[99], Eigen::MatrixXi::Zero(3,1))[0], 99);
00590
00591
00592 Eigen::MatrixXi directions = Eigen::Vector3i (0, 0, 1);
00593 vector<int> neighbors = grid.getNeighborCentroidIndices (cloud->points[195], directions);
00594 EXPECT_EQ (neighbors.size (), size_t (directions.cols ()));
00595 EXPECT_NE (neighbors.at (0), -1);
00596 EXPECT_LE (fabs (output.points[neighbors.at (0)].x - output.points[centroidIdx].x), 0.02);
00597 EXPECT_LE (fabs (output.points[neighbors.at (0)].y - output.points[centroidIdx].y), 0.02);
00598 EXPECT_LE ( output.points[neighbors.at (0)].z - output.points[centroidIdx].z, 0.02 * 2);
00599
00600
00601 VoxelGrid<PointCloud2> grid2;
00602
00603 PointCloud2 output_blob;
00604
00605 grid2.setLeafSize (0.02f, 0.02f, 0.02f);
00606 grid2.setInputCloud (cloud_blob);
00607 grid2.filter (output_blob);
00608
00609 fromROSMsg (output_blob, output);
00610
00611 EXPECT_EQ (int (output.points.size ()), 103);
00612 EXPECT_EQ (int (output.width), 103);
00613 EXPECT_EQ (int (output.height), 1);
00614 EXPECT_EQ (bool (output.is_dense), true);
00615
00616 grid2.setFilterFieldName ("z");
00617 grid2.setFilterLimits (0.05, 0.1);
00618 grid2.filter (output_blob);
00619
00620 fromROSMsg (output_blob, output);
00621
00622 EXPECT_EQ (int (output.points.size ()), 14);
00623 EXPECT_EQ (int (output.width), 14);
00624 EXPECT_EQ (int (output.height), 1);
00625 EXPECT_EQ (bool (output.is_dense), true);
00626
00627 EXPECT_NEAR (output.points[0].x, -0.026125, 1e-4);
00628 EXPECT_NEAR (output.points[0].y, 0.039788, 1e-4);
00629 EXPECT_NEAR (output.points[0].z, 0.052827, 1e-4);
00630
00631 EXPECT_NEAR (output.points[13].x, -0.073202, 1e-4);
00632 EXPECT_NEAR (output.points[13].y, 0.1296, 1e-4);
00633 EXPECT_NEAR (output.points[13].z, 0.051333, 1e-4);
00634
00635 grid2.setFilterLimitsNegative (true);
00636 grid2.setSaveLeafLayout(true);
00637 grid2.filter (output_blob);
00638
00639 fromROSMsg (output_blob, output);
00640
00641 EXPECT_EQ (int (output.points.size ()), 100);
00642 EXPECT_EQ (int (output.width), 100);
00643 EXPECT_EQ (int (output.height), 1);
00644 EXPECT_EQ (bool (output.is_dense), true);
00645
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655 EXPECT_EQ (grid2.getCentroidIndex (output.points[0].x, output.points[0].y, output.points[0].z), 0);
00656 EXPECT_EQ (grid2.getCentroidIndex (output.points[99].x, output.points[99].y, output.points[99].z), 99);
00657 EXPECT_EQ (grid2.getCentroidIndexAt (grid2.getGridCoordinates (-1,-1,-1)), -1);
00658
00659
00660
00661 int centroidIdx2 = grid2.getCentroidIndex (0.048722f, 0.073760f, 0.017434f);
00662 EXPECT_NE (centroidIdx2, -1);
00663
00664
00665 EXPECT_LE (fabs (output.points[centroidIdx2].x - 0.048722), 0.02);
00666 EXPECT_LE (fabs (output.points[centroidIdx2].y - 0.073760), 0.02);
00667 EXPECT_LE (fabs (output.points[centroidIdx2].z - 0.017434), 0.02);
00668
00669
00670 EXPECT_EQ (grid2.getNeighborCentroidIndices (output.points[0].x, output.points[0].y, output.points[0].z, Eigen::MatrixXi::Zero(3,1))[0], 0);
00671 EXPECT_EQ (grid2.getNeighborCentroidIndices (output.points[99].x, output.points[99].y, output.points[99].z, Eigen::MatrixXi::Zero(3,1))[0], 99);
00672
00673
00674 Eigen::MatrixXi directions2 = Eigen::Vector3i (0, 0, 1);
00675 vector<int> neighbors2 = grid2.getNeighborCentroidIndices (0.048722f, 0.073760f, 0.017434f, directions2);
00676 EXPECT_EQ (neighbors2.size (), size_t (directions2.cols ()));
00677 EXPECT_NE (neighbors2.at (0), -1);
00678 EXPECT_LE (fabs (output.points[neighbors2.at (0)].x - output.points[centroidIdx2].x), 0.02);
00679 EXPECT_LE (fabs (output.points[neighbors2.at (0)].y - output.points[centroidIdx2].y), 0.02);
00680 EXPECT_LE (output.points[neighbors2.at (0)].z - output.points[centroidIdx2].z, 0.02 * 2);
00681 }
00682
00684 TEST (VoxelGrid_RGB, Filters)
00685 {
00686 PointCloud2 cloud_rgb_blob_;
00687 PointCloud2::Ptr cloud_rgb_blob_ptr_;
00688 PointCloud<PointXYZRGB> cloud_rgb_;
00689 PointCloud<PointXYZRGB>::Ptr cloud_rgb_ptr_;
00690
00691 int col_r[] = {214, 193, 180, 164, 133, 119, 158, 179, 178, 212};
00692 int col_g[] = {10, 39, 219, 231, 142, 169, 84, 158, 139, 214};
00693 int col_b[] = {101, 26, 46, 189, 211, 154, 246, 16, 139, 153};
00694 float ave_r = 0.0f;
00695 float ave_b = 0.0f;
00696 float ave_g = 0.0f;
00697 for (int i = 0; i < 10; ++i)
00698 {
00699 ave_r += static_cast<float> (col_r[i]);
00700 ave_g += static_cast<float> (col_g[i]);
00701 ave_b += static_cast<float> (col_b[i]);
00702 }
00703 ave_r /= 10.0f;
00704 ave_g /= 10.0f;
00705 ave_b /= 10.0f;
00706
00707 for (int i = 0; i < 10; ++i)
00708 {
00709 PointXYZRGB pt;
00710 int rgb = (col_r[i] << 16) | (col_g[i] << 8) | col_b[i];
00711 pt.x = 0.0f;
00712 pt.y = 0.0f;
00713 pt.z = 0.0f;
00714 pt.rgb = *reinterpret_cast<float*> (&rgb);
00715 cloud_rgb_.points.push_back (pt);
00716 }
00717
00718 toROSMsg (cloud_rgb_, cloud_rgb_blob_);
00719 cloud_rgb_blob_ptr_.reset (new PointCloud2 (cloud_rgb_blob_));
00720 cloud_rgb_ptr_.reset (new PointCloud<PointXYZRGB> (cloud_rgb_));
00721
00722 PointCloud<PointXYZRGB> output_rgb;
00723 VoxelGrid<PointXYZRGB> grid_rgb;
00724
00725 grid_rgb.setLeafSize (0.03f, 0.03f, 0.03f);
00726 grid_rgb.setInputCloud (cloud_rgb_ptr_);
00727 grid_rgb.filter (output_rgb);
00728
00729 EXPECT_EQ (int (output_rgb.points.size ()), 1);
00730 EXPECT_EQ (int (output_rgb.width), 1);
00731 EXPECT_EQ (int (output_rgb.height), 1);
00732 EXPECT_EQ (bool (output_rgb.is_dense), true);
00733 {
00734 int rgb;
00735 int r,g,b;
00736 memcpy (&rgb, &(output_rgb.points[0].rgb), sizeof(int));
00737 r = (rgb >> 16) & 0xFF; g = (rgb >> 8 ) & 0xFF; b = (rgb >> 0 ) & 0xFF;
00738 EXPECT_NEAR (output_rgb.points[0].x, 0.0, 1e-4);
00739 EXPECT_NEAR (output_rgb.points[0].y, 0.0, 1e-4);
00740 EXPECT_NEAR (output_rgb.points[0].z, 0.0, 1e-4);
00741 EXPECT_NEAR (r, ave_r, 1.0);
00742 EXPECT_NEAR (g, ave_g, 1.0);
00743 EXPECT_NEAR (b, ave_b, 1.0);
00744 }
00745
00746 VoxelGrid<PointCloud2> grid2;
00747 PointCloud2 output_rgb_blob;
00748
00749 grid2.setLeafSize (0.03f, 0.03f, 0.03f);
00750 grid2.setInputCloud (cloud_rgb_blob_ptr_);
00751 grid2.filter (output_rgb_blob);
00752
00753 fromROSMsg (output_rgb_blob, output_rgb);
00754
00755 EXPECT_EQ (int (output_rgb.points.size ()), 1);
00756 EXPECT_EQ (int (output_rgb.width), 1);
00757 EXPECT_EQ (int (output_rgb.height), 1);
00758 EXPECT_EQ (bool (output_rgb.is_dense), true);
00759 {
00760 int rgb;
00761 int r,g,b;
00762 memcpy (&rgb, &(output_rgb.points[0].rgb), sizeof(int));
00763 r = (rgb >> 16) & 0xFF; g = (rgb >> 8 ) & 0xFF; b = (rgb >> 0 ) & 0xFF;
00764 EXPECT_NEAR (output_rgb.points[0].x, 0.0, 1e-4);
00765 EXPECT_NEAR (output_rgb.points[0].y, 0.0, 1e-4);
00766 EXPECT_NEAR (output_rgb.points[0].z, 0.0, 1e-4);
00767 EXPECT_NEAR (r, ave_r, 1.0);
00768 EXPECT_NEAR (g, ave_g, 1.0);
00769 EXPECT_NEAR (b, ave_b, 1.0);
00770 }
00771 }
00772
00774 TEST (ProjectInliers, Filters)
00775 {
00776
00777 ProjectInliers<PointXYZ> proj;
00778 PointCloud<PointXYZ> output;
00779
00780 ModelCoefficients::Ptr coefficients (new ModelCoefficients ());
00781 coefficients->values.resize (4);
00782 coefficients->values[0] = coefficients->values[1] = 0;
00783 coefficients->values[2] = 1.0;
00784 coefficients->values[3] = 0;
00785
00786 proj.setModelType (SACMODEL_PLANE);
00787 proj.setInputCloud (cloud);
00788 proj.setModelCoefficients (coefficients);
00789 proj.filter (output);
00790
00791 for (size_t i = 0; i < output.points.size (); ++i)
00792 EXPECT_NEAR (output.points[i].z, 0.0, 1e-4);
00793
00794
00795 ProjectInliers<PointCloud2> proj2;
00796
00797 PointCloud2 output_blob;
00798
00799 proj2.setModelType (SACMODEL_PLANE);
00800 proj2.setInputCloud (cloud_blob);
00801 proj2.setModelCoefficients (coefficients);
00802 proj2.filter (output_blob);
00803
00804 fromROSMsg (output_blob, output);
00805
00806 for (size_t i = 0; i < output.points.size (); ++i)
00807 EXPECT_NEAR (output.points[i].z, 0.0, 1e-4);
00808 }
00809
00811 TEST (RadiusOutlierRemoval, Filters)
00812 {
00813
00814 PointCloud<PointXYZ> cloud_out;
00815
00816 RadiusOutlierRemoval<PointXYZ> outrem;
00817 outrem.setInputCloud (cloud);
00818 outrem.setRadiusSearch (0.02);
00819 outrem.setMinNeighborsInRadius (14);
00820 outrem.filter (cloud_out);
00821
00822 EXPECT_EQ (int (cloud_out.points.size ()), 307);
00823 EXPECT_EQ (int (cloud_out.width), 307);
00824 EXPECT_EQ (bool (cloud_out.is_dense), true);
00825 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].x, -0.077893, 1e-4);
00826 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].y, 0.16039, 1e-4);
00827 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].z, -0.021299, 1e-4);
00828
00829
00830 PointCloud2 cloud_out2;
00831 RadiusOutlierRemoval<PointCloud2> outrem2;
00832 outrem2.setInputCloud (cloud_blob);
00833 outrem2.setRadiusSearch (0.02);
00834 outrem2.setMinNeighborsInRadius (15);
00835 outrem2.filter (cloud_out2);
00836
00837 fromROSMsg (cloud_out2, cloud_out);
00838 EXPECT_EQ (int (cloud_out.points.size ()), 307);
00839 EXPECT_EQ (int (cloud_out.width), 307);
00840 EXPECT_EQ (bool (cloud_out.is_dense), true);
00841 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].x, -0.077893, 1e-4);
00842 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].y, 0.16039, 1e-4);
00843 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].z, -0.021299, 1e-4);
00844
00845
00846 RadiusOutlierRemoval<PointXYZ> outrem_(true);
00847 outrem_.setInputCloud (cloud);
00848 outrem_.setRadiusSearch (0.02);
00849 outrem_.setMinNeighborsInRadius (14);
00850 outrem_.filter (cloud_out);
00851
00852 EXPECT_EQ (int (cloud_out.points.size ()), 307);
00853 EXPECT_EQ (int (cloud_out.width), 307);
00854 EXPECT_EQ (bool (cloud_out.is_dense), true);
00855 EXPECT_EQ (int (cloud_out.points.size ()), cloud->points.size ()-outrem_.getRemovedIndices()->size());
00856
00857 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].x, -0.077893, 1e-4);
00858 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].y, 0.16039, 1e-4);
00859 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].z, -0.021299, 1e-4);
00860
00861
00862 RadiusOutlierRemoval<PointCloud2> outrem2_(true);
00863 outrem2_.setInputCloud (cloud_blob);
00864 outrem2_.setRadiusSearch (0.02);
00865 outrem2_.setMinNeighborsInRadius (15);
00866 outrem2_.filter (cloud_out2);
00867
00868 fromROSMsg (cloud_out2, cloud_out);
00869 EXPECT_EQ (int (cloud_out.points.size ()), 307);
00870 EXPECT_EQ (int (cloud_out.width), 307);
00871 EXPECT_EQ (bool (cloud_out.is_dense), true);
00872 EXPECT_EQ (int (cloud_out.points.size ()), cloud_blob->width*cloud_blob->height-outrem2_.getRemovedIndices()->size());
00873
00874 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].x, -0.077893, 1e-4);
00875 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].y, 0.16039, 1e-4);
00876 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].z, -0.021299, 1e-4);
00877 }
00878
00880 TEST (RandomSample, Filters)
00881 {
00882
00883
00884 RandomSample<PointXYZ> sample;
00885 sample.setInputCloud (cloud);
00886 sample.setSample (10);
00887
00888
00889 vector<int> indices;
00890 sample.filter (indices);
00891
00892 EXPECT_EQ (int (indices.size ()), 10);
00893
00894
00895 PointCloud<PointXYZ> cloud_out;
00896 sample.filter(cloud_out);
00897
00898 EXPECT_EQ (int (cloud_out.width), 10);
00899 EXPECT_EQ (int (indices.size ()), int (cloud_out.size ()));
00900
00901 for (size_t i = 0; i < indices.size () - 1; ++i)
00902 {
00903
00904 EXPECT_LT (indices[i], indices[i+1]);
00905
00906 EXPECT_NEAR (cloud->points[indices[i]].x, cloud_out.points[i].x, 1e-4);
00907 EXPECT_NEAR (cloud->points[indices[i]].y, cloud_out.points[i].y, 1e-4);
00908 EXPECT_NEAR (cloud->points[indices[i]].z, cloud_out.points[i].z, 1e-4);
00909 }
00910
00911
00912
00913
00914 RandomSample<PointCloud2> sample2;
00915 sample2.setInputCloud (cloud_blob);
00916 sample2.setSample (10);
00917
00918
00919 vector<int> indices2;
00920 sample2.filter (indices2);
00921
00922 EXPECT_EQ (int (indices2.size ()), 10);
00923
00924
00925 PointCloud2 output_blob;
00926 sample2.filter (output_blob);
00927
00928 fromROSMsg (output_blob, cloud_out);
00929
00930 EXPECT_EQ (int (cloud_out.width), 10);
00931 EXPECT_EQ (int (indices2.size ()), int (cloud_out.size ()));
00932
00933 for (size_t i = 0; i < indices2.size () - 1; ++i)
00934 {
00935
00936 EXPECT_LT (indices2[i], indices2[i+1]);
00937
00938 EXPECT_NEAR (cloud->points[indices2[i]].x, cloud_out.points[i].x, 1e-4);
00939 EXPECT_NEAR (cloud->points[indices2[i]].y, cloud_out.points[i].y, 1e-4);
00940 EXPECT_NEAR (cloud->points[indices2[i]].z, cloud_out.points[i].z, 1e-4);
00941 }
00942 }
00943
00945 TEST (CropBox, Filters)
00946 {
00947
00948
00949
00950
00951
00952 PointCloud<PointXYZ>::Ptr input (new PointCloud<PointXYZ> ());
00953
00954 input->push_back (PointXYZ (0.0f, 0.0f, 0.0f));
00955 input->push_back (PointXYZ (0.9f, 0.9f, 0.9f));
00956 input->push_back (PointXYZ (0.9f, 0.9f, -0.9f));
00957 input->push_back (PointXYZ (0.9f, -0.9f, 0.9f));
00958 input->push_back (PointXYZ (-0.9f, 0.9f, 0.9f));
00959 input->push_back (PointXYZ (0.9f, -0.9f, -0.9f));
00960 input->push_back (PointXYZ (-0.9f, -0.9f, 0.9f));
00961 input->push_back (PointXYZ (-0.9f, 0.9f, -0.9f));
00962 input->push_back (PointXYZ (-0.9f, -0.9f, -0.9f));
00963
00964
00965 CropBox<PointXYZ> cropBoxFilter;
00966 cropBoxFilter.setInputCloud (input);
00967 Eigen::Vector4f min_pt (-1.0f, -1.0f, -1.0f, 1.0f);
00968 Eigen::Vector4f max_pt (1.0f, 1.0f, 1.0f, 1.0f);
00969
00970
00971 cropBoxFilter.setMin (min_pt);
00972 cropBoxFilter.setMax (max_pt);
00973
00974
00975 vector<int> indices;
00976 cropBoxFilter.filter (indices);
00977
00978
00979 PointCloud<PointXYZ> cloud_out;
00980 cropBoxFilter.filter (cloud_out);
00981
00982
00983 EXPECT_EQ (int (indices.size ()), 9);
00984 EXPECT_EQ (int (cloud_out.size ()), 9);
00985 EXPECT_EQ (int (cloud_out.width), 9);
00986 EXPECT_EQ (int (cloud_out.height), 1);
00987
00988
00989 cropBoxFilter.setTranslation(Eigen::Vector3f(0, 1, 0));
00990 cropBoxFilter.filter (indices);
00991 cropBoxFilter.filter (cloud_out);
00992
00993 EXPECT_EQ (int (indices.size ()), 5);
00994 EXPECT_EQ (int (cloud_out.size ()), 5);
00995
00996
00997 cropBoxFilter.setRotation(Eigen::Vector3f(0, 45*PI/180, 0));
00998 cropBoxFilter.filter (indices);
00999 cropBoxFilter.filter (cloud_out);
01000
01001 EXPECT_EQ (int (indices.size ()), 1);
01002 EXPECT_EQ (int (cloud_out.size ()), 1);
01003 EXPECT_EQ (int (cloud_out.width), 1);
01004 EXPECT_EQ (int (cloud_out.height), 1);
01005
01006
01007 cropBoxFilter.setTransform(getTransformation(0, 0, 0, 0, 0, -45*PI/180));
01008 cropBoxFilter.filter (indices);
01009 cropBoxFilter.filter (cloud_out);
01010
01011 EXPECT_EQ (int (indices.size ()), 3);
01012 EXPECT_EQ (int (cloud_out.size ()), 3);
01013 EXPECT_EQ (int (cloud_out.width), 3);
01014 EXPECT_EQ (int (cloud_out.height), 1);
01015
01016
01017 cropBoxFilter.setTransform (getTransformation(0, -1, 0, 0, 0, -45*PI/180));
01018 cropBoxFilter.filter (indices);
01019 cropBoxFilter.filter (cloud_out);
01020
01021 EXPECT_EQ (int (indices.size ()), 2);
01022 EXPECT_EQ (int (cloud_out.size ()), 2);
01023 EXPECT_EQ (int (cloud_out.width), 2);
01024 EXPECT_EQ (int (cloud_out.height), 1);
01025
01026
01027 cropBoxFilter.setTransform (getTransformation(0, -1, 0, 0, 0, 0));
01028 cropBoxFilter.filter (indices);
01029 cropBoxFilter.filter (cloud_out);
01030
01031 EXPECT_EQ (int (indices.size ()), 0);
01032 EXPECT_EQ (int (cloud_out.size ()), 0);
01033 EXPECT_EQ (int (cloud_out.width), 0);
01034 EXPECT_EQ (int (cloud_out.height), 1);
01035
01036
01037
01038
01039
01040 PointCloud2::Ptr input2 (new PointCloud2);
01041 pcl::toROSMsg (*input, *input2);
01042
01043
01044 CropBox<PointCloud2> cropBoxFilter2;
01045 cropBoxFilter2.setInputCloud (input2);
01046
01047
01048 cropBoxFilter2.setMin (min_pt);
01049 cropBoxFilter2.setMax (max_pt);
01050
01051
01052 vector<int> indices2;
01053 cropBoxFilter2.filter (indices2);
01054
01055
01056 PointCloud2 cloud_out2;
01057 cropBoxFilter2.filter (cloud_out2);
01058
01059
01060 EXPECT_EQ (int (indices2.size ()), 9);
01061 EXPECT_EQ (int (indices2.size ()), int (cloud_out2.width * cloud_out2.height));
01062
01063
01064 cropBoxFilter2.setTranslation (Eigen::Vector3f(0, 1, 0));
01065 cropBoxFilter2.filter (indices2);
01066 cropBoxFilter2.filter (cloud_out2);
01067
01068 EXPECT_EQ (int (indices2.size ()), 5);
01069 EXPECT_EQ (int (indices2.size ()), int (cloud_out2.width * cloud_out2.height));
01070
01071
01072 cropBoxFilter2.setRotation (Eigen::Vector3f(0, 45*PI/180, 0));
01073 cropBoxFilter2.filter (indices2);
01074 cropBoxFilter2.filter (cloud_out2);
01075
01076 EXPECT_EQ (int (indices2.size ()), 1);
01077 EXPECT_EQ (int (indices2.size ()), int (cloud_out2.width * cloud_out2.height));
01078
01079
01080 cropBoxFilter2.setTransform (getTransformation(0, 0, 0, 0, 0, -45*PI/180));
01081 cropBoxFilter2.filter (indices2);
01082 cropBoxFilter2.filter (cloud_out2);
01083
01084 EXPECT_EQ (int (indices2.size ()), 3);
01085 EXPECT_EQ (int (cloud_out2.width * cloud_out2.height), 3);
01086
01087
01088 cropBoxFilter2.setTransform (getTransformation(0, -1, 0, 0, 0, -45*PI/180));
01089 cropBoxFilter2.filter (indices2);
01090 cropBoxFilter2.filter (cloud_out2);
01091
01092 EXPECT_EQ (int (indices2.size ()), 2);
01093 EXPECT_EQ (int (cloud_out2.width * cloud_out2.height), 2);
01094
01095
01096 cropBoxFilter2.setTransform (getTransformation(0, -1, 0, 0, 0, 0));
01097 cropBoxFilter2.filter (indices2);
01098 cropBoxFilter2.filter (cloud_out2);
01099
01100 EXPECT_EQ (int (indices2.size ()), 0);
01101 EXPECT_EQ (int (cloud_out2.width * cloud_out2.height), 0);
01102 }
01103
01105 TEST (StatisticalOutlierRemoval, Filters)
01106 {
01107
01108 PointCloud<PointXYZ> output;
01109
01110 StatisticalOutlierRemoval<PointXYZ> outrem;
01111 outrem.setInputCloud (cloud);
01112 outrem.setMeanK (50);
01113 outrem.setStddevMulThresh (1.0);
01114 outrem.filter (output);
01115
01116 EXPECT_EQ (int (output.points.size ()), 352);
01117 EXPECT_EQ (int (output.width), 352);
01118 EXPECT_EQ (bool (output.is_dense), true);
01119 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.034667, 1e-4);
01120 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.15131, 1e-4);
01121 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.00071029, 1e-4);
01122
01123 outrem.setNegative (true);
01124 outrem.filter (output);
01125
01126 EXPECT_EQ (int (output.points.size ()), int (cloud->points.size ()) - 352);
01127 EXPECT_EQ (int (output.width), int (cloud->width) - 352);
01128 EXPECT_EQ (bool (output.is_dense), true);
01129 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.07793, 1e-4);
01130 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.17516, 1e-4);
01131 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.0444, 1e-4);
01132
01133
01134 PointCloud2 output2;
01135 StatisticalOutlierRemoval<PointCloud2> outrem2;
01136 outrem2.setInputCloud (cloud_blob);
01137 outrem2.setMeanK (50);
01138 outrem2.setStddevMulThresh (1.0);
01139 outrem2.filter (output2);
01140
01141 fromROSMsg (output2, output);
01142
01143 EXPECT_EQ (int (output.points.size ()), 352);
01144 EXPECT_EQ (int (output.width), 352);
01145 EXPECT_EQ (bool (output.is_dense), true);
01146 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.034667, 1e-4);
01147 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.15131, 1e-4);
01148 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.00071029, 1e-4);
01149
01150 outrem2.setNegative (true);
01151 outrem2.filter (output2);
01152
01153 fromROSMsg (output2, output);
01154
01155 EXPECT_EQ (int (output.points.size ()), int (cloud->points.size ()) - 352);
01156 EXPECT_EQ (int (output.width), int (cloud->width) - 352);
01157 EXPECT_EQ (bool (output.is_dense), true);
01158 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.07793, 1e-4);
01159 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.17516, 1e-4);
01160 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.0444, 1e-4);
01161
01162
01163 StatisticalOutlierRemoval<PointXYZ> outrem_(true);
01164 outrem_.setInputCloud (cloud);
01165 outrem_.setMeanK (50);
01166 outrem_.setStddevMulThresh (1.0);
01167 outrem_.filter (output);
01168
01169 EXPECT_EQ (int (output.points.size ()), 352);
01170 EXPECT_EQ (int (output.width), 352);
01171 EXPECT_EQ (bool (output.is_dense), true);
01172 EXPECT_EQ (int (output.points.size ()), cloud->points.size ()-outrem_.getRemovedIndices()->size());
01173 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.034667, 1e-4);
01174 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.15131, 1e-4);
01175 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.00071029, 1e-4);
01176
01177 outrem_.setNegative (true);
01178 outrem_.filter (output);
01179
01180 EXPECT_EQ (int (output.points.size ()), int (cloud->points.size ()) - 352);
01181 EXPECT_EQ (int (output.width), int (cloud->width) - 352);
01182 EXPECT_EQ (bool (output.is_dense), true);
01183 EXPECT_EQ (int (output.points.size ()) ,cloud->points.size ()-outrem_.getRemovedIndices()->size());
01184 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.07793, 1e-4);
01185 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.17516, 1e-4);
01186 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.0444, 1e-4);
01187
01188
01189 StatisticalOutlierRemoval<PointCloud2> outrem2_(true);
01190 outrem2_.setInputCloud (cloud_blob);
01191 outrem2_.setMeanK (50);
01192 outrem2_.setStddevMulThresh (1.0);
01193 outrem2_.filter (output2);
01194
01195 fromROSMsg (output2, output);
01196
01197 EXPECT_EQ (int (output.points.size ()), 352);
01198 EXPECT_EQ (int (output.width), 352);
01199 EXPECT_EQ (bool (output.is_dense), true);
01200 EXPECT_EQ (int (output.points.size ()), cloud_blob->width*cloud_blob->height-outrem2_.getRemovedIndices()->size());
01201 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.034667, 1e-4);
01202 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.15131, 1e-4);
01203 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.00071029, 1e-4);
01204
01205 outrem2_.setNegative (true);
01206 outrem2_.filter (output2);
01207
01208 fromROSMsg (output2, output);
01209
01210 EXPECT_EQ (int (output.points.size ()), int (cloud->points.size ()) - 352);
01211 EXPECT_EQ (int (output.width), int (cloud->width) - 352);
01212 EXPECT_EQ (bool (output.is_dense), true);
01213 EXPECT_EQ (int (output.points.size ()), cloud_blob->width*cloud_blob->height-outrem2_.getRemovedIndices()->size());
01214 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.07793, 1e-4);
01215 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.17516, 1e-4);
01216 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.0444, 1e-4);
01217 }
01218
01220 TEST (ConditionalRemoval, Filters)
01221 {
01222
01223 PointCloud<PointXYZ> output;
01224
01225
01226 ConditionAnd<PointXYZ>::Ptr range_cond (new ConditionAnd<PointXYZ> ());
01227 range_cond->addComparison (FieldComparison<PointXYZ>::ConstPtr (new FieldComparison<PointXYZ> ("z",
01228 ComparisonOps::GT,
01229 0.02)));
01230 range_cond->addComparison (FieldComparison<PointXYZ>::ConstPtr (new FieldComparison<PointXYZ> ("z",
01231 ComparisonOps::LT,
01232 0.04)));
01233 range_cond->addComparison (FieldComparison<PointXYZ>::ConstPtr (new FieldComparison<PointXYZ> ("y",
01234 ComparisonOps::GT,
01235 0.10)));
01236 range_cond->addComparison (FieldComparison<PointXYZ>::ConstPtr (new FieldComparison<PointXYZ> ("y",
01237 ComparisonOps::LT,
01238 0.12)));
01239
01240
01241 ConditionalRemoval<PointXYZ> condrem (range_cond);
01242 condrem.setInputCloud (cloud);
01243
01244
01245 condrem.setKeepOrganized (false);
01246 condrem.filter (output);
01247
01248 EXPECT_EQ (int (output.points.size ()), 28);
01249 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.087292, 1e-4);
01250 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.103140, 1e-4);
01251 EXPECT_NEAR (output.points[output.points.size () - 1].z, 0.020825, 1e-4);
01252
01253
01254 condrem.setKeepOrganized (true);
01255 condrem.filter (output);
01256
01257 int num_not_nan = 0;
01258 for (size_t i = 0; i < output.points.size (); i++)
01259 {
01260 if (pcl_isfinite (output.points[i].x) &&
01261 pcl_isfinite (output.points[i].y) &&
01262 pcl_isfinite (output.points[i].z))
01263 num_not_nan++;
01264 }
01265
01266 EXPECT_EQ (int (output.points.size ()), int (cloud->points.size ()));
01267 EXPECT_EQ (int (output.width), int (cloud->width));
01268 EXPECT_EQ (int (output.height), int (cloud->height));
01269 EXPECT_EQ (num_not_nan, 28);
01270
01271
01272 ConditionalRemoval<PointXYZ> condrem_ (range_cond,true);
01273 condrem_.setInputCloud (cloud);
01274
01275
01276 condrem_.setKeepOrganized (false);
01277 condrem_.filter (output);
01278
01279 EXPECT_EQ (int (output.points.size ()), 28);
01280 EXPECT_EQ (int (output.points.size ()), cloud->points.size()-condrem_.getRemovedIndices()->size());
01281 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.087292, 1e-4);
01282 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.103140, 1e-4);
01283 EXPECT_NEAR (output.points[output.points.size () - 1].z, 0.020825, 1e-4);
01284
01285
01286 condrem_.setKeepOrganized (true);
01287 condrem_.filter (output);
01288
01289 num_not_nan = 0;
01290 for (size_t i = 0; i < output.points.size (); i++)
01291 {
01292 if (pcl_isfinite (output.points[i].x) &&
01293 pcl_isfinite (output.points[i].y) &&
01294 pcl_isfinite (output.points[i].z))
01295 num_not_nan++;
01296 }
01297
01298 EXPECT_EQ (int (output.points.size ()), int (cloud->points.size ()));
01299 EXPECT_EQ (int (output.width), int (cloud->width));
01300 EXPECT_EQ (int (output.height), int (cloud->height));
01301 EXPECT_EQ (num_not_nan, 28);
01302 EXPECT_EQ (int (num_not_nan), cloud->points.size()-condrem_.getRemovedIndices()->size());
01303 }
01304
01306 TEST (ConditionalRemovalSetIndices, Filters)
01307 {
01308
01309 PointCloud<PointXYZ> output;
01310
01311
01312 boost::shared_ptr<vector<int> > indices (new vector<int> (2));
01313 (*indices)[0] = 0;
01314 (*indices)[1] = static_cast<int> (cloud->points.size ()) - 1;
01315
01316
01317 ConditionAnd<PointXYZ>::Ptr true_cond (new ConditionAnd<PointXYZ> ());
01318 true_cond->addComparison (TfQuadraticXYZComparison<PointXYZ>::ConstPtr (new TfQuadraticXYZComparison<PointXYZ> (ComparisonOps::EQ, Eigen::Matrix3f::Zero (),
01319 Eigen::Vector3f::Zero (), 0)));
01320
01321
01322 ConditionalRemoval<PointXYZ> condrem2 (true_cond);
01323 condrem2.setInputCloud (cloud);
01324 condrem2.setIndices (indices);
01325
01326
01327 condrem2.setKeepOrganized (false);
01328 condrem2.filter (output);
01329
01330 EXPECT_EQ (int (output.points.size ()), 2);
01331 EXPECT_EQ (int (output.width), 2);
01332 EXPECT_EQ (int (output.height), 1);
01333
01334 EXPECT_EQ (cloud->points[0].x, output.points[0].x);
01335 EXPECT_EQ (cloud->points[0].y, output.points[0].y);
01336 EXPECT_EQ (cloud->points[0].z, output.points[0].z);
01337
01338 EXPECT_EQ (cloud->points[cloud->points.size () - 1].x, output.points[1].x);
01339 EXPECT_EQ (cloud->points[cloud->points.size () - 1].y, output.points[1].y);
01340 EXPECT_EQ (cloud->points[cloud->points.size () - 1].z, output.points[1].z);
01341
01342
01343 condrem2.setKeepOrganized (true);
01344 condrem2.filter (output);
01345
01346 EXPECT_EQ (cloud->points[0].x, output.points[0].x);
01347 EXPECT_EQ (cloud->points[0].y, output.points[0].y);
01348 EXPECT_EQ (cloud->points[0].z, output.points[0].z);
01349
01350 EXPECT_EQ (cloud->points[cloud->points.size () - 1].x, output.points[output.points.size () - 1].x);
01351 EXPECT_EQ (cloud->points[cloud->points.size () - 1].y, output.points[output.points.size () - 1].y);
01352 EXPECT_EQ (cloud->points[cloud->points.size () - 1].z, output.points[output.points.size () - 1].z);
01353
01354 int num_not_nan = 0;
01355 for (size_t i = 0; i < output.points.size (); i++)
01356 {
01357 if (pcl_isfinite (output.points[i].x) &&
01358 pcl_isfinite (output.points[i].y) &&
01359 pcl_isfinite (output.points[i].z))
01360 num_not_nan++;
01361 }
01362
01363 EXPECT_EQ (int (output.points.size ()), int (cloud->points.size ()));
01364 EXPECT_EQ (int (output.width), int (cloud->width));
01365 EXPECT_EQ (int (output.height), int (cloud->height));
01366 EXPECT_EQ (num_not_nan, 2);
01367
01368
01369 ConditionalRemoval<PointXYZ> condrem2_ (true_cond, true);
01370 condrem2_.setIndices (indices);
01371 condrem2_.setInputCloud (cloud);
01372
01373
01374 condrem2_.setKeepOrganized (false);
01375 condrem2_.filter (output);
01376
01377 EXPECT_EQ (int (output.points.size ()), 2);
01378 EXPECT_EQ (int (output.width), 2);
01379 EXPECT_EQ (int (output.height), 1);
01380
01381 EXPECT_EQ (cloud->points[0].x, output.points[0].x);
01382 EXPECT_EQ (cloud->points[0].y, output.points[0].y);
01383 EXPECT_EQ (cloud->points[0].z, output.points[0].z);
01384
01385 EXPECT_EQ (cloud->points[cloud->points.size () - 1].x, output.points[1].x);
01386 EXPECT_EQ (cloud->points[cloud->points.size () - 1].y, output.points[1].y);
01387 EXPECT_EQ (cloud->points[cloud->points.size () - 1].z, output.points[1].z);
01388
01389 EXPECT_EQ (int (output.points.size ()), int (indices->size ()) - int (condrem2_.getRemovedIndices ()->size ()));
01390
01391
01392 condrem2_.setKeepOrganized (true);
01393 condrem2_.filter (output);
01394
01395 EXPECT_EQ (cloud->points[0].x, output.points[0].x);
01396 EXPECT_EQ (cloud->points[0].y, output.points[0].y);
01397 EXPECT_EQ (cloud->points[0].z, output.points[0].z);
01398
01399 EXPECT_EQ (cloud->points[cloud->points.size () - 1].x, output.points[output.points.size () - 1].x);
01400 EXPECT_EQ (cloud->points[cloud->points.size () - 1].y, output.points[output.points.size () - 1].y);
01401 EXPECT_EQ (cloud->points[cloud->points.size () - 1].z, output.points[output.points.size () - 1].z);
01402
01403 num_not_nan = 0;
01404 for (size_t i = 0; i < output.points.size (); i++)
01405 {
01406 if (pcl_isfinite (output.points[i].x) &&
01407 pcl_isfinite (output.points[i].y) &&
01408 pcl_isfinite (output.points[i].z))
01409 num_not_nan++;
01410 }
01411
01412 EXPECT_EQ (int (output.points.size ()), int (cloud->points.size ()));
01413 EXPECT_EQ (int (output.width), int (cloud->width));
01414 EXPECT_EQ (int (output.height), int (cloud->height));
01415 EXPECT_EQ (num_not_nan, 2);
01416
01417 EXPECT_EQ (num_not_nan, int (indices->size ()) - int (condrem2_.getRemovedIndices ()->size ()));
01418 }
01419
01420 TEST (ConditionalRemovalTfQuadraticXYZComparison, Filters)
01421 {
01422
01423 PointCloud<PointXYZ> output;
01424
01425
01426 PointCloud<PointXYZ>::Ptr input (new PointCloud<PointXYZ> ());
01427
01428 input->push_back (PointXYZ (-1.0, 0.0, 0.0));
01429 input->push_back (PointXYZ (0.0, 0.0, 0.0));
01430 input->push_back (PointXYZ (1.0, 0.0, 0.0));
01431 input->push_back (PointXYZ (2.0, 0.0, 0.0));
01432 input->push_back (PointXYZ (3.0, 0.0, 0.0));
01433 input->push_back (PointXYZ (4.0, 0.0, 0.0));
01434 input->push_back (PointXYZ (5.0, 0.0, 0.0));
01435 input->push_back (PointXYZ (6.0, 0.0, 0.0));
01436 input->push_back (PointXYZ (7.0, 0.0, 0.0));
01437 input->push_back (PointXYZ (8.0, 0.0, 0.0));
01438
01439
01440 ConditionAnd<PointXYZ>::Ptr cyl_cond (new ConditionAnd<PointXYZ> ());
01441 Eigen::Matrix3f cylinderMatrix;
01442 cylinderMatrix << 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
01443 float cylinderScalar = -4;
01444 TfQuadraticXYZComparison<PointXYZ>::Ptr cyl_comp (new TfQuadraticXYZComparison<PointXYZ> (ComparisonOps::LE, cylinderMatrix,
01445 Eigen::Vector3f::Zero (), cylinderScalar));
01446 cyl_cond->addComparison (cyl_comp);
01447
01448
01449 ConditionalRemoval<PointXYZ> condrem (cyl_cond);
01450 condrem.setInputCloud (input);
01451 condrem.setKeepOrganized (false);
01452
01453
01454 condrem.filter (output);
01455
01456 EXPECT_EQ (10, int (output.points.size ()));
01457
01458 EXPECT_EQ (input->points[0].x, output.points[0].x);
01459 EXPECT_EQ (input->points[0].y, output.points[0].y);
01460 EXPECT_EQ (input->points[0].z, output.points[0].z);
01461
01462 EXPECT_EQ (input->points[9].x, output.points[9].x);
01463 EXPECT_EQ (input->points[9].y, output.points[9].y);
01464 EXPECT_EQ (input->points[9].z, output.points[9].z);
01465
01466
01467 cyl_comp->transformComparison (getTransformation (0, 0, 0, 0, 0, PI / 2).inverse ());
01468
01469 condrem.filter (output);
01470
01471 EXPECT_EQ (4, int (output.points.size ()));
01472
01473 EXPECT_EQ (input->points[0].x, output.points[0].x);
01474 EXPECT_EQ (input->points[0].y, output.points[0].y);
01475 EXPECT_EQ (input->points[0].z, output.points[0].z);
01476
01477 EXPECT_EQ (input->points[3].x, output.points[3].x);
01478 EXPECT_EQ (input->points[3].y, output.points[3].y);
01479 EXPECT_EQ (input->points[3].z, output.points[3].z);
01480
01481
01482 Eigen::Vector3f planeVector;
01483 planeVector << 1.0, 0.0, 0.0;
01484 Eigen::Matrix3f planeMatrix = Eigen::Matrix3f::Zero ();
01485 cyl_comp->setComparisonMatrix (planeMatrix);
01486 cyl_comp->setComparisonVector (planeVector);
01487 cyl_comp->setComparisonScalar (-2 * 5.0);
01488 cyl_comp->setComparisonOperator (ComparisonOps::LT);
01489
01490 condrem.filter (output);
01491
01492 EXPECT_EQ (6, int (output.points.size ()));
01493
01494 EXPECT_EQ (input->points[0].x, output.points[0].x);
01495 EXPECT_EQ (input->points[0].y, output.points[0].y);
01496 EXPECT_EQ (input->points[0].z, output.points[0].z);
01497
01498 EXPECT_EQ (input->points[5].x, output.points[5].x);
01499 EXPECT_EQ (input->points[5].y, output.points[5].y);
01500 EXPECT_EQ (input->points[5].z, output.points[5].z);
01501 }
01502
01503
01504 int
01505 main (int argc, char** argv)
01506 {
01507
01508 if (argc < 2)
01509 {
01510 std::cerr << "No test file given. Please download `bun0.pcd` and pass its path to the test." << std::endl;
01511 return (-1);
01512 }
01513
01514 char* file_name = argv[1];
01515
01516 loadPCDFile (file_name, *cloud_blob);
01517 fromROSMsg (*cloud_blob, *cloud);
01518
01519 indices_.resize (cloud->points.size ());
01520 for (int i = 0; i < static_cast<int> (indices_.size ()); ++i)
01521 indices_[i] = i;
01522
01523 testing::InitGoogleTest (&argc, argv);
01524 return (RUN_ALL_TESTS ());
01525 }
01526