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
00039 #include <gtest/gtest.h>
00040 #include <pcl/point_types.h>
00041 #include <pcl/io/pcd_io.h>
00042 #include <pcl/filters/filter.h>
00043 #include <pcl/filters/passthrough.h>
00044 #include <pcl/filters/voxel_grid.h>
00045 #include <pcl/filters/extract_indices.h>
00046 #include <pcl/filters/project_inliers.h>
00047 #include <pcl/filters/radius_outlier_removal.h>
00048 #include <pcl/filters/statistical_outlier_removal.h>
00049 #include <pcl/filters/conditional_removal.h>
00050
00051 using namespace pcl;
00052 using namespace pcl::io;
00053 using namespace std;
00054
00055 sensor_msgs::PointCloud2 cloud_blob_;
00056 sensor_msgs::PointCloud2::Ptr cloud_blob_ptr_;
00057 PointCloud<PointXYZ> cloud_;
00058 PointCloud<PointXYZ>::Ptr cloud_ptr_;
00059 vector<int> indices_;
00060
00062 TEST (ExtractIndices, Filters)
00063 {
00064
00065 ExtractIndices<PointXYZ> ei;
00066 vector<int> indices (2);
00067 indices[0] = 0; indices[1] = cloud_.points.size () - 1;
00068
00069 PointCloud<PointXYZ> output;
00070 ei.setInputCloud (cloud_ptr_);
00071 ei.setIndices (boost::make_shared<std::vector<int> > (indices));
00072 ei.filter (output);
00073
00074 EXPECT_EQ ((int)output.points.size (), 2);
00075 EXPECT_EQ ((int)output.width, 2);
00076 EXPECT_EQ ((int)output.height, 1);
00077
00078 EXPECT_EQ (cloud_.points[0].x, output.points[0].x);
00079 EXPECT_EQ (cloud_.points[0].y, output.points[0].y);
00080 EXPECT_EQ (cloud_.points[0].z, output.points[0].z);
00081
00082 EXPECT_EQ (cloud_.points[cloud_.points.size () - 1].x, output.points[1].x);
00083 EXPECT_EQ (cloud_.points[cloud_.points.size () - 1].y, output.points[1].y);
00084 EXPECT_EQ (cloud_.points[cloud_.points.size () - 1].z, output.points[1].z);
00085
00086 ei.setNegative (true);
00087 ei.filter (output);
00088
00089 EXPECT_EQ (output.points.size (), cloud_.points.size () - 2);
00090 EXPECT_EQ (output.width, cloud_.points.size () - 2);
00091 EXPECT_EQ ((int)output.height, 1);
00092
00093 EXPECT_EQ (cloud_.points[1].x, output.points[0].x);
00094 EXPECT_EQ (cloud_.points[1].y, output.points[0].y);
00095 EXPECT_EQ (cloud_.points[1].z, output.points[0].z);
00096
00097 EXPECT_EQ (cloud_.points[cloud_.points.size () - 2].x, output.points[output.points.size () - 1].x);
00098 EXPECT_EQ (cloud_.points[cloud_.points.size () - 2].y, output.points[output.points.size () - 1].y);
00099 EXPECT_EQ (cloud_.points[cloud_.points.size () - 2].z, output.points[output.points.size () - 1].z);
00100
00101
00102 ExtractIndices<sensor_msgs::PointCloud2> ei2;
00103
00104 sensor_msgs::PointCloud2 output_blob;
00105 ei2.setInputCloud (cloud_blob_ptr_);
00106 ei2.setIndices (boost::make_shared<std::vector<int> > (indices));
00107 ei2.filter (output_blob);
00108
00109 fromROSMsg (output_blob, output);
00110
00111 EXPECT_EQ ((int)output.points.size (), 2);
00112 EXPECT_EQ ((int)output.width, 2);
00113 EXPECT_EQ ((int)output.height, 1);
00114
00115 EXPECT_EQ (cloud_.points[0].x, output.points[0].x);
00116 EXPECT_EQ (cloud_.points[0].y, output.points[0].y);
00117 EXPECT_EQ (cloud_.points[0].z, output.points[0].z);
00118
00119 EXPECT_EQ (cloud_.points[cloud_.points.size () - 1].x, output.points[1].x);
00120 EXPECT_EQ (cloud_.points[cloud_.points.size () - 1].y, output.points[1].y);
00121 EXPECT_EQ (cloud_.points[cloud_.points.size () - 1].z, output.points[1].z);
00122
00123 ei2.setNegative (true);
00124 ei2.filter (output_blob);
00125
00126 fromROSMsg (output_blob, output);
00127
00128 EXPECT_EQ (output.points.size (), cloud_.points.size () - 2);
00129 EXPECT_EQ (output.width, cloud_.points.size () - 2);
00130 EXPECT_EQ ((int)output.height, 1);
00131
00132 EXPECT_EQ (cloud_.points[1].x, output.points[0].x);
00133 EXPECT_EQ (cloud_.points[1].y, output.points[0].y);
00134 EXPECT_EQ (cloud_.points[1].z, output.points[0].z);
00135
00136 EXPECT_EQ (cloud_.points[cloud_.points.size () - 2].x, output.points[output.points.size () - 1].x);
00137 EXPECT_EQ (cloud_.points[cloud_.points.size () - 2].y, output.points[output.points.size () - 1].y);
00138 EXPECT_EQ (cloud_.points[cloud_.points.size () - 2].z, output.points[output.points.size () - 1].z);
00139 }
00140
00142 TEST (PassThrough, Filters)
00143 {
00144
00145 PointCloud<PointXYZ> output;
00146 PassThrough<PointXYZ> pt;
00147
00148 pt.setInputCloud (cloud_ptr_);
00149 pt.filter (output);
00150
00151 EXPECT_EQ (output.points.size (), cloud_.points.size ());
00152 EXPECT_EQ (output.width, cloud_.width);
00153 EXPECT_EQ (output.height, cloud_.height);
00154
00155 pt.setFilterFieldName ("z");
00156 pt.setFilterLimits (0.05, 0.1);
00157 pt.filter (output);
00158
00159 EXPECT_EQ ((int)output.points.size (), 42);
00160 EXPECT_EQ ((int)output.width, 42);
00161 EXPECT_EQ ((int)output.height, 1);
00162 EXPECT_EQ ((bool)output.is_dense, true);
00163
00164 EXPECT_NEAR (output.points[0].x, -0.074556, 1e-5);
00165 EXPECT_NEAR (output.points[0].y, 0.13415, 1e-5);
00166 EXPECT_NEAR (output.points[0].z, 0.051046, 1e-5);
00167
00168 EXPECT_NEAR (output.points[41].x, -0.030331, 1e-5);
00169 EXPECT_NEAR (output.points[41].y, 0.039749, 1e-5);
00170 EXPECT_NEAR (output.points[41].z, 0.052133, 1e-5);
00171
00172 pt.setFilterLimitsNegative (true);
00173 pt.filter (output);
00174
00175 EXPECT_EQ ((int)output.points.size (), 355);
00176 EXPECT_EQ ((int)output.width, 355);
00177 EXPECT_EQ ((int)output.height, 1);
00178 EXPECT_EQ ((bool)output.is_dense, true);
00179
00180 EXPECT_NEAR (output.points[0].x, 0.0054216, 1e-5);
00181 EXPECT_NEAR (output.points[0].y, 0.11349, 1e-5);
00182 EXPECT_NEAR (output.points[0].z, 0.040749, 1e-5);
00183
00184 EXPECT_NEAR (output.points[354].x, -0.07793, 1e-5);
00185 EXPECT_NEAR (output.points[354].y, 0.17516, 1e-5);
00186 EXPECT_NEAR (output.points[354].z, -0.0444, 1e-5);
00187
00188
00189 pt.setUserFilterValue (std::numeric_limits<float>::quiet_NaN ());
00190 pt.setFilterFieldName ("");
00191 pt.filter (output);
00192
00193 EXPECT_EQ (output.points.size (), cloud_.points.size ());
00194 EXPECT_EQ (output.width, cloud_.width);
00195 EXPECT_EQ (output.height, cloud_.height);
00196 EXPECT_EQ (output.is_dense, cloud_.is_dense);
00197 EXPECT_NEAR (output.points[0].x, cloud_.points[0].x, 1e-5);
00198 EXPECT_NEAR (output.points[output.points.size () - 1].x, cloud_.points[cloud_.points.size () - 1].x, 1e-5);
00199
00200 pt.setFilterFieldName ("z");
00201 pt.setFilterLimitsNegative (false);
00202 pt.setKeepOrganized (true);
00203 pt.filter (output);
00204
00205 EXPECT_EQ (output.points.size (), cloud_.points.size ());
00206 EXPECT_EQ (output.width, cloud_.width);
00207 EXPECT_EQ (output.height, cloud_.height);
00208 EXPECT_EQ ((bool)output.is_dense, false);
00209
00210 if (!pcl_isnan (output.points[0].x)) EXPECT_EQ (1, 0);
00211 if (!pcl_isnan (output.points[0].y)) EXPECT_EQ (1, 0);
00212 if (!pcl_isnan (output.points[0].z)) EXPECT_EQ (1, 0);
00213
00214 if (!pcl_isnan (output.points[41].x)) EXPECT_EQ (1, 0);
00215 if (!pcl_isnan (output.points[41].y)) EXPECT_EQ (1, 0);
00216 if (!pcl_isnan (output.points[41].z)) EXPECT_EQ (1, 0);
00217
00218 pt.setFilterLimitsNegative (true);
00219 pt.filter (output);
00220
00221 EXPECT_EQ (output.points.size (), cloud_.points.size ());
00222 EXPECT_EQ (output.width, cloud_.width);
00223 EXPECT_EQ (output.height, cloud_.height);
00224 EXPECT_EQ ((bool)output.is_dense, false);
00225
00226 EXPECT_NEAR (output.points[0].x, cloud_.points[0].x, 1e-5);
00227 EXPECT_NEAR (output.points[0].y, cloud_.points[0].y, 1e-5);
00228 EXPECT_NEAR (output.points[0].z, cloud_.points[0].z, 1e-5);
00229
00230 EXPECT_NEAR (output.points[41].x, cloud_.points[41].x, 1e-5);
00231 EXPECT_NEAR (output.points[41].y, cloud_.points[41].y, 1e-5);
00232 EXPECT_NEAR (output.points[41].z, cloud_.points[41].z, 1e-5);
00233
00234
00235
00236 PassThrough<sensor_msgs::PointCloud2> pt2;
00237
00238 sensor_msgs::PointCloud2 output_blob;
00239 pt2.setInputCloud (cloud_blob_ptr_);
00240 pt2.filter (output_blob);
00241
00242 fromROSMsg (output_blob, output);
00243
00244 EXPECT_EQ (output.points.size (), cloud_.points.size ());
00245 EXPECT_EQ (output.width, cloud_.width);
00246 EXPECT_EQ (output.height, cloud_.height);
00247
00248 pt2.setFilterFieldName ("z");
00249 pt2.setFilterLimits (0.05, 0.1);
00250 pt2.filter (output_blob);
00251
00252 fromROSMsg (output_blob, output);
00253
00254 EXPECT_EQ ((int)output.points.size (), 42);
00255 EXPECT_EQ ((int)output.width, 42);
00256 EXPECT_EQ ((int)output.height, 1);
00257 EXPECT_EQ ((bool)output.is_dense, true);
00258
00259 EXPECT_NEAR (output.points[0].x, -0.074556, 1e-5);
00260 EXPECT_NEAR (output.points[0].y, 0.13415, 1e-5);
00261 EXPECT_NEAR (output.points[0].z, 0.051046, 1e-5);
00262
00263 EXPECT_NEAR (output.points[41].x, -0.030331, 1e-5);
00264 EXPECT_NEAR (output.points[41].y, 0.039749, 1e-5);
00265 EXPECT_NEAR (output.points[41].z, 0.052133, 1e-5);
00266
00267 pt2.setFilterLimitsNegative (true);
00268 pt2.filter (output_blob);
00269
00270 fromROSMsg (output_blob, output);
00271
00272 EXPECT_EQ ((int)output.points.size (), 355);
00273 EXPECT_EQ ((int)output.width, 355);
00274 EXPECT_EQ ((int)output.height, 1);
00275 EXPECT_EQ ((bool)output.is_dense, true);
00276
00277 EXPECT_NEAR (output.points[0].x, 0.0054216, 1e-5);
00278 EXPECT_NEAR (output.points[0].y, 0.11349, 1e-5);
00279 EXPECT_NEAR (output.points[0].z, 0.040749, 1e-5);
00280
00281 EXPECT_NEAR (output.points[354].x, -0.07793, 1e-5);
00282 EXPECT_NEAR (output.points[354].y, 0.17516, 1e-5);
00283 EXPECT_NEAR (output.points[354].z, -0.0444, 1e-5);
00284
00285
00286 pt2.setUserFilterValue (std::numeric_limits<float>::quiet_NaN ());
00287 pt2.setFilterFieldName ("");
00288 pt2.filter (output_blob);
00289 fromROSMsg (output_blob, output);
00290
00291 EXPECT_EQ (output.points.size (), cloud_.points.size ());
00292 EXPECT_EQ (output.width, cloud_.width);
00293 EXPECT_EQ (output.height, cloud_.height);
00294 EXPECT_EQ (output.is_dense, cloud_.is_dense);
00295 EXPECT_NEAR (output.points[0].x, cloud_.points[0].x, 1e-5);
00296 EXPECT_NEAR (output.points[output.points.size () - 1].x, cloud_.points[cloud_.points.size () - 1].x, 1e-5);
00297
00298 pt2.setFilterFieldName ("z");
00299 pt2.setFilterLimitsNegative (false);
00300 pt2.setKeepOrganized (true);
00301 pt2.filter (output_blob);
00302 fromROSMsg (output_blob, output);
00303
00304 EXPECT_EQ (output.points.size (), cloud_.points.size ());
00305 EXPECT_EQ (output.width, cloud_.width);
00306 EXPECT_EQ (output.height, cloud_.height);
00307 EXPECT_EQ ((bool)output.is_dense, false);
00308
00309 if (!pcl_isnan (output.points[0].x)) EXPECT_EQ (1, 0);
00310 if (!pcl_isnan (output.points[0].y)) EXPECT_EQ (1, 0);
00311 if (!pcl_isnan (output.points[0].z)) EXPECT_EQ (1, 0);
00312
00313 if (!pcl_isnan (output.points[41].x)) EXPECT_EQ (1, 0);
00314 if (!pcl_isnan (output.points[41].y)) EXPECT_EQ (1, 0);
00315 if (!pcl_isnan (output.points[41].z)) EXPECT_EQ (1, 0);
00316
00317 pt2.setFilterLimitsNegative (true);
00318 pt2.filter (output_blob);
00319 fromROSMsg (output_blob, output);
00320
00321 EXPECT_EQ (output.points.size (), cloud_.points.size ());
00322 EXPECT_EQ (output.width, cloud_.width);
00323 EXPECT_EQ (output.height, cloud_.height);
00324 EXPECT_EQ ((bool)output.is_dense, false);
00325
00326 EXPECT_NEAR (output.points[0].x, cloud_.points[0].x, 1e-5);
00327 EXPECT_NEAR (output.points[0].y, cloud_.points[0].y, 1e-5);
00328 EXPECT_NEAR (output.points[0].z, cloud_.points[0].z, 1e-5);
00329
00330 EXPECT_NEAR (output.points[41].x, cloud_.points[41].x, 1e-5);
00331 EXPECT_NEAR (output.points[41].y, cloud_.points[41].y, 1e-5);
00332 EXPECT_NEAR (output.points[41].z, cloud_.points[41].z, 1e-5);
00333 }
00334
00336 TEST (VoxelGrid, Filters)
00337 {
00338
00339 PointCloud<PointXYZ> output;
00340 VoxelGrid<PointXYZ> grid;
00341
00342 grid.setLeafSize (0.02, 0.02, 0.02);
00343 grid.setInputCloud (cloud_ptr_);
00344 grid.filter (output);
00345
00346 EXPECT_EQ ((int)output.points.size (), 103);
00347 EXPECT_EQ ((int)output.width, 103);
00348 EXPECT_EQ ((int)output.height, 1);
00349 EXPECT_EQ ((bool)output.is_dense, true);
00350
00351 grid.setFilterFieldName ("z");
00352 grid.setFilterLimits (0.05, 0.1);
00353 grid.filter (output);
00354
00355 EXPECT_EQ ((int)output.points.size (), 14);
00356 EXPECT_EQ ((int)output.width, 14);
00357 EXPECT_EQ ((int)output.height, 1);
00358 EXPECT_EQ ((bool)output.is_dense, true);
00359
00360 EXPECT_NEAR (output.points[0].x, -0.026125, 1e-4);
00361 EXPECT_NEAR (output.points[0].y, 0.039788, 1e-4);
00362 EXPECT_NEAR (output.points[0].z, 0.052827, 1e-4);
00363
00364 EXPECT_NEAR (output.points[13].x, -0.073202, 1e-4);
00365 EXPECT_NEAR (output.points[13].y, 0.1296, 1e-4);
00366 EXPECT_NEAR (output.points[13].z, 0.051333, 1e-4);
00367
00368 grid.setFilterLimitsNegative (true);
00369 grid.setSaveLeafLayout(true);
00370 grid.filter (output);
00371
00372 EXPECT_EQ ((int)output.points.size (), 100);
00373 EXPECT_EQ ((int)output.width, 100);
00374 EXPECT_EQ ((int)output.height, 1);
00375 EXPECT_EQ ((bool)output.is_dense, true);
00376
00377 EXPECT_NEAR (output.points[0].x, -0.070192, 1e-4);
00378 EXPECT_NEAR (output.points[0].y, 0.17653, 1e-4);
00379 EXPECT_NEAR (output.points[0].z, -0.048774, 1e-4);
00380
00381 EXPECT_NEAR (output.points[99].x, -0.068948, 1e-4);
00382 EXPECT_NEAR (output.points[99].y, 0.1447, 1e-4);
00383 EXPECT_NEAR (output.points[99].z, 0.042178, 1e-4);
00384
00385
00386 EXPECT_EQ (grid.getCentroidIndex (output.points[0]), 0);
00387 EXPECT_EQ (grid.getCentroidIndex (output.points[99]), 99);
00388 EXPECT_EQ (grid.getCentroidIndexAt (grid.getGridCoordinates (-1,-1,-1), false), -1);
00389
00390
00391
00392 int centroidIdx = grid.getCentroidIndex (cloud_ptr_->points[195]);
00393
00394
00395 EXPECT_LE (fabs (output.points[centroidIdx].x - cloud_ptr_->points[195].x), 0.02);
00396 EXPECT_LE (fabs (output.points[centroidIdx].y - cloud_ptr_->points[195].y), 0.02);
00397 EXPECT_LE (fabs (output.points[centroidIdx].z - cloud_ptr_->points[195].z), 0.02);
00398
00399
00400 EXPECT_EQ (grid.getNeighborCentroidIndices (output.points[0], Eigen::MatrixXi::Zero(3,1))[0], 0);
00401 EXPECT_EQ (grid.getNeighborCentroidIndices (output.points[99], Eigen::MatrixXi::Zero(3,1))[0], 99);
00402
00403
00404 Eigen::MatrixXi directions = Eigen::Vector3i (0, 0, 1);
00405 vector<int> neighbors = grid.getNeighborCentroidIndices (cloud_ptr_->points[195], directions);
00406 EXPECT_EQ (neighbors.size (), (size_t)directions.cols ());
00407 EXPECT_NE (neighbors.at (0), -1);
00408 EXPECT_LE (fabs (output.points[neighbors.at (0)].x - output.points[centroidIdx].x), 0.02);
00409 EXPECT_LE (fabs (output.points[neighbors.at (0)].y - output.points[centroidIdx].y), 0.02);
00410 EXPECT_LE ( output.points[neighbors.at (0)].z - output.points[centroidIdx].z, 0.02 * 2);
00411
00412
00413 VoxelGrid<sensor_msgs::PointCloud2> grid2;
00414
00415 sensor_msgs::PointCloud2 output_blob;
00416
00417 grid2.setLeafSize (0.02, 0.02, 0.02);
00418 grid2.setInputCloud (cloud_blob_ptr_);
00419 grid2.filter (output_blob);
00420
00421 fromROSMsg (output_blob, output);
00422
00423 EXPECT_EQ ((int)output.points.size (), 103);
00424 EXPECT_EQ ((int)output.width, 103);
00425 EXPECT_EQ ((int)output.height, 1);
00426 EXPECT_EQ ((bool)output.is_dense, true);
00427
00428 grid2.setFilterFieldName ("z");
00429 grid2.setFilterLimits (0.05, 0.1);
00430 grid2.filter (output_blob);
00431
00432 fromROSMsg (output_blob, output);
00433
00434 EXPECT_EQ ((int)output.points.size (), 14);
00435 EXPECT_EQ ((int)output.width, 14);
00436 EXPECT_EQ ((int)output.height, 1);
00437 EXPECT_EQ ((bool)output.is_dense, true);
00438
00439 EXPECT_NEAR (output.points[0].x, -0.026125, 1e-4);
00440 EXPECT_NEAR (output.points[0].y, 0.039788, 1e-4);
00441 EXPECT_NEAR (output.points[0].z, 0.052827, 1e-4);
00442
00443 EXPECT_NEAR (output.points[13].x, -0.073202, 1e-4);
00444 EXPECT_NEAR (output.points[13].y, 0.1296, 1e-4);
00445 EXPECT_NEAR (output.points[13].z, 0.051333, 1e-4);
00446
00447 grid2.setFilterLimitsNegative (true);
00448 grid2.setSaveLeafLayout(true);
00449 grid2.filter (output_blob);
00450
00451 fromROSMsg (output_blob, output);
00452
00453 EXPECT_EQ ((int)output.points.size (), 100);
00454 EXPECT_EQ ((int)output.width, 100);
00455 EXPECT_EQ ((int)output.height, 1);
00456 EXPECT_EQ ((bool)output.is_dense, true);
00457
00458 EXPECT_NEAR (output.points[0].x, -0.070192, 1e-4);
00459 EXPECT_NEAR (output.points[0].y, 0.17653, 1e-4);
00460 EXPECT_NEAR (output.points[0].z, -0.048774, 1e-4);
00461
00462 EXPECT_NEAR (output.points[99].x, -0.068948, 1e-4);
00463 EXPECT_NEAR (output.points[99].y, 0.1447, 1e-4);
00464 EXPECT_NEAR (output.points[99].z, 0.042178, 1e-4);
00465
00466
00467 EXPECT_EQ (grid2.getCentroidIndex (output.points[0].x, output.points[0].y, output.points[0].z), 0);
00468 EXPECT_EQ (grid2.getCentroidIndex (output.points[99].x, output.points[99].y, output.points[99].z), 99);
00469 EXPECT_EQ (grid2.getCentroidIndexAt (grid2.getGridCoordinates (-1,-1,-1), false), -1);
00470
00471
00472
00473 int centroidIdx2 = grid2.getCentroidIndex (0.048722, 0.073760, 0.017434);
00474 EXPECT_NE (centroidIdx2, -1);
00475
00476
00477 EXPECT_LE (fabs (output.points[centroidIdx2].x - 0.048722), 0.02);
00478 EXPECT_LE (fabs (output.points[centroidIdx2].y - 0.073760), 0.02);
00479 EXPECT_LE (fabs (output.points[centroidIdx2].z - 0.017434), 0.02);
00480
00481
00482 EXPECT_EQ (grid2.getNeighborCentroidIndices (output.points[0].x, output.points[0].y, output.points[0].z, Eigen::MatrixXi::Zero(3,1))[0], 0);
00483 EXPECT_EQ (grid2.getNeighborCentroidIndices (output.points[99].x, output.points[99].y, output.points[99].z, Eigen::MatrixXi::Zero(3,1))[0], 99);
00484
00485
00486 Eigen::MatrixXi directions2 = Eigen::Vector3i (0, 0, 1);
00487 vector<int> neighbors2 = grid2.getNeighborCentroidIndices (0.048722, 0.073760, 0.017434, directions2);
00488 EXPECT_EQ (neighbors2.size (), (size_t)directions2.cols ());
00489 EXPECT_NE (neighbors2.at (0), -1);
00490 EXPECT_LE (fabs (output.points[neighbors2.at (0)].x - output.points[centroidIdx2].x), 0.02);
00491 EXPECT_LE (fabs (output.points[neighbors2.at (0)].y - output.points[centroidIdx2].y), 0.02);
00492 EXPECT_LE ( output.points[neighbors2.at (0)].z - output.points[centroidIdx2].z, 0.02 * 2);
00493 }
00494
00496 TEST (VoxelGrid_RGB, Filters)
00497 {
00498 sensor_msgs::PointCloud2 cloud_rgb_blob_;
00499 sensor_msgs::PointCloud2::Ptr cloud_rgb_blob_ptr_;
00500 PointCloud<PointXYZRGB> cloud_rgb_;
00501 PointCloud<PointXYZRGB>::Ptr cloud_rgb_ptr_;
00502
00503 int col_r[] = {214,193, 180, 164, 133, 119, 158, 179, 178, 212};
00504 int col_g[] = { 10, 39, 219, 231, 142, 169, 84, 158, 139, 214};
00505 int col_b[] = {101, 26, 46, 189, 211, 154, 246, 16, 139, 153};
00506 float ave_r = 0.0; float ave_b = 0.0; float ave_g = 0.0;
00507 for (int i = 0; i < 10; ++i)
00508 {
00509 ave_r += col_r[i]; ave_g += col_g[i]; ave_b += col_b[i];
00510 }
00511 ave_r /= 10; ave_g /= 10; ave_b /= 10;
00512
00513 for (int i=0; i < 10; ++i)
00514 {
00515 PointXYZRGB pt;
00516 int rgb = (col_r[i] << 16) | (col_g[i] << 8) | col_b[i];
00517 pt.x = 0.0; pt.y = 0.0; pt.z = 0.0;
00518 pt.rgb = *reinterpret_cast<float*>(&rgb);
00519 cloud_rgb_.points.push_back(pt);
00520 }
00521
00522 toROSMsg (cloud_rgb_, cloud_rgb_blob_);
00523 cloud_rgb_blob_ptr_.reset (new sensor_msgs::PointCloud2 (cloud_rgb_blob_));
00524 cloud_rgb_ptr_.reset (new PointCloud<PointXYZRGB> (cloud_rgb_));
00525
00526 PointCloud<PointXYZRGB> output_rgb;
00527 VoxelGrid<PointXYZRGB> grid_rgb;
00528
00529 grid_rgb.setLeafSize (0.03, 0.03, 0.03);
00530 grid_rgb.setInputCloud (cloud_rgb_ptr_);
00531 grid_rgb.filter (output_rgb);
00532
00533 EXPECT_EQ ((int)output_rgb.points.size (), 1);
00534 EXPECT_EQ ((int)output_rgb.width, 1);
00535 EXPECT_EQ ((int)output_rgb.height, 1);
00536 EXPECT_EQ ((bool)output_rgb.is_dense, true);
00537 {
00538 int rgb;
00539 int r,g,b;
00540 memcpy (&rgb, &(output_rgb.points[0].rgb), sizeof(int));
00541 r = (rgb >> 16) & 0xFF; g = (rgb >> 8 ) & 0xFF; b = (rgb >> 0 ) & 0xFF;
00542 EXPECT_NEAR (output_rgb.points[0].x, 0.0, 1e-4);
00543 EXPECT_NEAR (output_rgb.points[0].y, 0.0, 1e-4);
00544 EXPECT_NEAR (output_rgb.points[0].z, 0.0, 1e-4);
00545 EXPECT_NEAR (r, ave_r, 1.0);
00546 EXPECT_NEAR (g, ave_g, 1.0);
00547 EXPECT_NEAR (b, ave_b, 1.0);
00548 }
00549
00550 VoxelGrid<sensor_msgs::PointCloud2> grid2;
00551 sensor_msgs::PointCloud2 output_rgb_blob;
00552
00553 grid2.setLeafSize (0.03, 0.03, 0.03);
00554 grid2.setInputCloud (cloud_rgb_blob_ptr_);
00555 grid2.filter (output_rgb_blob);
00556
00557 fromROSMsg (output_rgb_blob, output_rgb);
00558
00559 EXPECT_EQ ((int)output_rgb.points.size (), 1);
00560 EXPECT_EQ ((int)output_rgb.width, 1);
00561 EXPECT_EQ ((int)output_rgb.height, 1);
00562 EXPECT_EQ ((bool)output_rgb.is_dense, true);
00563 {
00564 int rgb;
00565 int r,g,b;
00566 memcpy (&rgb, &(output_rgb.points[0].rgb), sizeof(int));
00567 r = (rgb >> 16) & 0xFF; g = (rgb >> 8 ) & 0xFF; b = (rgb >> 0 ) & 0xFF;
00568 EXPECT_NEAR (output_rgb.points[0].x, 0.0, 1e-4);
00569 EXPECT_NEAR (output_rgb.points[0].y, 0.0, 1e-4);
00570 EXPECT_NEAR (output_rgb.points[0].z, 0.0, 1e-4);
00571 EXPECT_NEAR (r, ave_r, 1.0);
00572 EXPECT_NEAR (g, ave_g, 1.0);
00573 EXPECT_NEAR (b, ave_b, 1.0);
00574 }
00575 }
00576
00578 TEST (ProjectInliers, Filters)
00579 {
00580
00581 ProjectInliers<PointXYZ> proj;
00582 PointCloud<PointXYZ> output;
00583
00584 ModelCoefficients::Ptr coefficients (new ModelCoefficients ());
00585 coefficients->values.resize (4);
00586 coefficients->values[0] = coefficients->values[1] = 0;
00587 coefficients->values[2] = 1.0;
00588 coefficients->values[3] = 0;
00589
00590 proj.setModelType (SACMODEL_PLANE);
00591 proj.setInputCloud (cloud_ptr_);
00592 proj.setModelCoefficients (coefficients);
00593 proj.filter (output);
00594
00595 for (size_t i = 0; i < output.points.size (); ++i)
00596 EXPECT_NEAR (output.points[i].z, 0.0, 1e-4);
00597
00598
00599 ProjectInliers<sensor_msgs::PointCloud2> proj2;
00600
00601 sensor_msgs::PointCloud2 output_blob;
00602
00603 proj2.setModelType (SACMODEL_PLANE);
00604 proj2.setInputCloud (cloud_blob_ptr_);
00605 proj2.setModelCoefficients (coefficients);
00606 proj2.filter (output_blob);
00607
00608 fromROSMsg (output_blob, output);
00609
00610 for (size_t i = 0; i < output.points.size (); ++i)
00611 EXPECT_NEAR (output.points[i].z, 0.0, 1e-4);
00612 }
00613
00615 TEST (RadiusOutlierRemoval, Filters)
00616 {
00617
00618 PointCloud<PointXYZ> cloud_out;
00619
00620 RadiusOutlierRemoval<PointXYZ> outrem;
00621 outrem.setInputCloud (cloud_ptr_);
00622 outrem.setRadiusSearch (0.02);
00623 outrem.setMinNeighborsInRadius (15);
00624 outrem.filter (cloud_out);
00625
00626 EXPECT_EQ ((int)cloud_out.points.size (), 307);
00627 EXPECT_EQ ((int)cloud_out.width, 307);
00628 EXPECT_EQ ((bool)cloud_out.is_dense, true);
00629 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].x, -0.077893, 1e-4);
00630 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].y, 0.16039, 1e-4);
00631 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].z, -0.021299, 1e-4);
00632
00633
00634 sensor_msgs::PointCloud2 cloud_out2;
00635 RadiusOutlierRemoval<sensor_msgs::PointCloud2> outrem2;
00636 outrem2.setInputCloud (cloud_blob_ptr_);
00637 outrem2.setRadiusSearch (0.02);
00638 outrem2.setMinNeighborsInRadius (15);
00639 outrem2.filter (cloud_out2);
00640
00641 fromROSMsg (cloud_out2, cloud_out);
00642 EXPECT_EQ ((int)cloud_out.points.size (), 307);
00643 EXPECT_EQ ((int)cloud_out.width, 307);
00644 EXPECT_EQ ((bool)cloud_out.is_dense, true);
00645 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].x, -0.077893, 1e-4);
00646 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].y, 0.16039, 1e-4);
00647 EXPECT_NEAR (cloud_out.points[cloud_out.points.size () - 1].z, -0.021299, 1e-4);
00648 }
00649
00651 TEST (StatisticalOutlierRemoval, Filters)
00652 {
00653
00654 PointCloud<PointXYZ> output;
00655
00656 StatisticalOutlierRemoval<PointXYZ> outrem;
00657 outrem.setInputCloud (cloud_ptr_);
00658 outrem.setMeanK (50);
00659 outrem.setStddevMulThresh (1.0);
00660 outrem.filter (output);
00661
00662 EXPECT_EQ ((int)output.points.size (), 352);
00663 EXPECT_EQ ((int)output.width, 352);
00664 EXPECT_EQ ((bool)output.is_dense, true);
00665 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.034667, 1e-4);
00666 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.15131, 1e-4);
00667 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.00071029, 1e-4);
00668
00669 outrem.setNegative (true);
00670 outrem.filter (output);
00671
00672 EXPECT_EQ ((int)output.points.size (), (int)cloud_.points.size () - 352);
00673 EXPECT_EQ ((int)output.width, (int)cloud_.width - 352);
00674 EXPECT_EQ ((bool)output.is_dense, true);
00675 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.07793, 1e-4);
00676 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.17516, 1e-4);
00677 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.0444, 1e-4);
00678
00679
00680 sensor_msgs::PointCloud2 output2;
00681 StatisticalOutlierRemoval<sensor_msgs::PointCloud2> outrem2;
00682 outrem2.setInputCloud (cloud_blob_ptr_);
00683 outrem2.setMeanK (50);
00684 outrem2.setStddevMulThresh (1.0);
00685 outrem2.filter (output2);
00686
00687 fromROSMsg (output2, output);
00688
00689 EXPECT_EQ ((int)output.points.size (), 352);
00690 EXPECT_EQ ((int)output.width, 352);
00691 EXPECT_EQ ((bool)output.is_dense, true);
00692 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.034667, 1e-4);
00693 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.15131, 1e-4);
00694 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.00071029, 1e-4);
00695
00696 outrem2.setNegative (true);
00697 outrem2.filter (output2);
00698
00699 fromROSMsg (output2, output);
00700
00701 EXPECT_EQ ((int)output.points.size (), (int)cloud_.points.size () - 352);
00702 EXPECT_EQ ((int)output.width, (int)cloud_.width - 352);
00703 EXPECT_EQ ((bool)output.is_dense, true);
00704 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.07793, 1e-4);
00705 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.17516, 1e-4);
00706 EXPECT_NEAR (output.points[output.points.size () - 1].z, -0.0444, 1e-4);
00707 }
00708
00710 TEST (ConditionalRemoval, Filters)
00711 {
00712
00713 PointCloud<PointXYZ> output;
00714
00715
00716 ConditionAnd<PointXYZ>::Ptr range_cond (new ConditionAnd<PointXYZ> ());
00717 range_cond->addComparison (FieldComparison<PointXYZ>::ConstPtr (new FieldComparison<PointXYZ>("z", ComparisonOps::GT, 0.02)) );
00718 range_cond->addComparison (FieldComparison<PointXYZ>::ConstPtr (new FieldComparison<PointXYZ>("z", ComparisonOps::LT, 0.04)) );
00719 range_cond->addComparison (FieldComparison<PointXYZ>::ConstPtr (new FieldComparison<PointXYZ>("y", ComparisonOps::GT, 0.10)) );
00720 range_cond->addComparison (FieldComparison<PointXYZ>::ConstPtr (new FieldComparison<PointXYZ>("y", ComparisonOps::LT, 0.12)) );
00721
00722
00723 ConditionalRemoval<PointXYZ> condrem (range_cond);
00724 condrem.setInputCloud (cloud_ptr_);
00725
00726
00727 condrem.setKeepOrganized (true);
00728 condrem.filter (output);
00729
00730 EXPECT_EQ ((int)output.points.size (), 28);
00731 EXPECT_NEAR (output.points[output.points.size () - 1].x, -0.087292, 1e-4);
00732 EXPECT_NEAR (output.points[output.points.size () - 1].y, 0.103140, 1e-4);
00733 EXPECT_NEAR (output.points[output.points.size () - 1].z, 0.020825, 1e-4);
00734
00735
00736 condrem.setKeepOrganized (false);
00737 condrem.filter (output);
00738
00739 int num_not_nan = 0;
00740 for (size_t i = 0; i < output.points.size (); i++)
00741 {
00742 if (pcl_isfinite (output.points[i].x) &&
00743 pcl_isfinite (output.points[i].y) &&
00744 pcl_isfinite (output.points[i].z))
00745 num_not_nan++;
00746 }
00747
00748 EXPECT_EQ ((int)output.points.size (), (int)cloud_ptr_->points.size ());
00749 EXPECT_EQ ((int)output.width, (int)cloud_ptr_->width);
00750 EXPECT_EQ ((int)output.height, (int)cloud_ptr_->height);
00751 EXPECT_EQ (num_not_nan, 28);
00752 }
00753
00754
00755 int
00756 main (int argc, char** argv)
00757 {
00758
00759 loadPCDFile ("./test/bun0.pcd", cloud_blob_);
00760 cloud_blob_ptr_.reset (new sensor_msgs::PointCloud2 (cloud_blob_));
00761 fromROSMsg (cloud_blob_, cloud_);
00762 cloud_ptr_.reset (new PointCloud<PointXYZ> (cloud_));
00763
00764 indices_.resize (cloud_.points.size ());
00765 for (size_t i = 0; i < indices_.size (); ++i) { indices_[i] = i; }
00766
00767 testing::InitGoogleTest (&argc, argv);
00768 return (RUN_ALL_TESTS ());
00769 }
00770