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 #include "rtabmap/core/util3d_filtering.h"
00029
00030 #include <pcl/filters/extract_indices.h>
00031 #include <pcl/filters/voxel_grid.h>
00032 #include <pcl/filters/frustum_culling.h>
00033 #include <pcl/filters/random_sample.h>
00034 #include <pcl/filters/passthrough.h>
00035 #include <pcl/filters/crop_box.h>
00036
00037 #include <pcl/features/normal_3d_omp.h>
00038
00039 #include <pcl/search/kdtree.h>
00040
00041 #include <pcl/common/common.h>
00042
00043 #include <pcl/segmentation/extract_clusters.h>
00044 #include <pcl/segmentation/sac_segmentation.h>
00045
00046 #include <rtabmap/core/util3d.h>
00047 #include <rtabmap/core/util3d_surface.h>
00048
00049 #include <rtabmap/utilite/ULogger.h>
00050 #include <rtabmap/utilite/UMath.h>
00051 #include <rtabmap/utilite/UConversion.h>
00052
00053 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
00054 #include <pcl/impl/instantiate.hpp>
00055 #include <pcl/point_types.h>
00056 #include <pcl/segmentation/impl/extract_clusters.hpp>
00057 #include <pcl/segmentation/extract_labeled_clusters.h>
00058 #include <pcl/segmentation/impl/extract_labeled_clusters.hpp>
00059 #include <pcl/filters/impl/extract_indices.hpp>
00060
00061 PCL_INSTANTIATE(EuclideanClusterExtraction, (pcl::PointXYZRGBNormal))
00062 PCL_INSTANTIATE(extractEuclideanClusters, (pcl::PointXYZRGBNormal))
00063 PCL_INSTANTIATE(extractEuclideanClusters_indices, (pcl::PointXYZRGBNormal))
00064 PCL_INSTANTIATE(ExtractIndices, (pcl::PointNormal))
00065
00066 #endif
00067
00068 namespace rtabmap
00069 {
00070
00071 namespace util3d
00072 {
00073
00074 LaserScan commonFiltering(
00075 const LaserScan & scanIn,
00076 int downsamplingStep,
00077 float rangeMin,
00078 float rangeMax,
00079 float voxelSize,
00080 int normalK,
00081 float normalRadius,
00082 bool forceGroundNormalsUp)
00083 {
00084 LaserScan scan = scanIn;
00085 UDEBUG("scan size=%d format=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f",
00086 scan.size(), (int)scan.format(), downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius);
00087 if(!scan.isEmpty())
00088 {
00089
00090 if(downsamplingStep<=1 || scan.size() <= downsamplingStep)
00091 {
00092 downsamplingStep = 1;
00093 }
00094
00095 if(downsamplingStep > 1 || rangeMin > 0.0f || rangeMax > 0.0f)
00096 {
00097 cv::Mat tmp = cv::Mat(1, scan.size()/downsamplingStep, scan.dataType());
00098 bool is2d = scan.is2d();
00099 int oi = 0;
00100 float rangeMinSqrd = rangeMin * rangeMin;
00101 float rangeMaxSqrd = rangeMax * rangeMax;
00102 for(int i=0; i<scan.size()-downsamplingStep+1; i+=downsamplingStep)
00103 {
00104 const float * ptr = scan.data().ptr<float>(0, i);
00105
00106 if(rangeMin>0.0f || rangeMax>0.0f)
00107 {
00108 float r;
00109 if(is2d)
00110 {
00111 r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
00112 }
00113 else
00114 {
00115 r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
00116 }
00117
00118 if(rangeMin > 0.0f && r < rangeMinSqrd)
00119 {
00120 continue;
00121 }
00122 if(rangeMax > 0.0f && r > rangeMaxSqrd)
00123 {
00124 continue;
00125 }
00126 }
00127
00128 cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(tmp, cv::Range::all(), cv::Range(oi,oi+1)));
00129 ++oi;
00130 }
00131 int previousSize = scan.size();
00132 int scanMaxPtsTmp = scan.maxPoints();
00133 scan = LaserScan(cv::Mat(tmp, cv::Range::all(), cv::Range(0, oi)), scanMaxPtsTmp/downsamplingStep, rangeMax>0.0f&&rangeMax<scan.maxRange()?rangeMax:scan.maxRange(), scan.format(), scan.localTransform());
00134 UDEBUG("Downsampling scan (step=%d): %d -> %d (scanMaxPts=%d->%d)", downsamplingStep, previousSize, scan.size(), scanMaxPtsTmp, scan.maxPoints());
00135 }
00136
00137 if(scan.size() && (voxelSize > 0.0f || ((normalK > 0 || normalRadius>0.0f) && !scan.hasNormals())))
00138 {
00139
00140 if(scan.hasRGB())
00141 {
00142 UASSERT(!scan.is2d());
00143 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = laserScanToPointCloudRGB(scan);
00144 if(cloud->size())
00145 {
00146 int scanMaxPts = scan.maxPoints();
00147 if(voxelSize > 0.0f)
00148 {
00149 cloud = voxelize(cloud, voxelSize);
00150 float ratio = float(cloud->size()) / scan.size();
00151 scanMaxPts = int(float(scanMaxPts) * ratio);
00152 UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), cloud->size(), scan.maxPoints(), scanMaxPts);
00153 }
00154 if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
00155 {
00156 pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, normalK, normalRadius);
00157 scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYZRGBNormal, scan.localTransform());
00158 UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
00159 }
00160 else
00161 {
00162 if(scan.hasNormals())
00163 {
00164 UWARN("Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
00165 }
00166 scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYZRGB, scan.localTransform());
00167 }
00168 }
00169 }
00170 else if(scan.hasIntensity())
00171 {
00172 pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = laserScanToPointCloudI(scan);
00173 if(cloud->size())
00174 {
00175 int scanMaxPts = scan.maxPoints();
00176 if(voxelSize > 0.0f)
00177 {
00178 cloud = voxelize(cloud, voxelSize);
00179 float ratio = float(cloud->size()) / scan.size();
00180 scanMaxPts = int(float(scanMaxPts) * ratio);
00181 UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), cloud->size(), scan.maxPoints(), scanMaxPts);
00182 }
00183 if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
00184 {
00185 pcl::PointCloud<pcl::Normal>::Ptr normals;
00186 if(scan.is2d())
00187 {
00188 normals = util3d::computeNormals2D(cloud, normalK, normalRadius);
00189 scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYINormal, scan.localTransform());
00190 }
00191 else
00192 {
00193 normals = util3d::computeNormals(cloud, normalK, normalRadius);
00194 scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYZINormal, scan.localTransform());
00195 }
00196 UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
00197 }
00198 else
00199 {
00200 if(scan.hasNormals())
00201 {
00202 UWARN("Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
00203 }
00204 if(scan.is2d())
00205 {
00206 scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYI, scan.localTransform());
00207 }
00208 else
00209 {
00210 scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYZI, scan.localTransform());
00211 }
00212 }
00213 }
00214 }
00215 else
00216 {
00217 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = laserScanToPointCloud(scan);
00218 if(cloud->size())
00219 {
00220 int scanMaxPts = scan.maxPoints();
00221 if(voxelSize > 0.0f)
00222 {
00223 cloud = voxelize(cloud, voxelSize);
00224 float ratio = float(cloud->size()) / scan.size();
00225 scanMaxPts = int(float(scanMaxPts) * ratio);
00226 UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), cloud->size(), scan.maxPoints(), scanMaxPts);
00227 }
00228 if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
00229 {
00230 pcl::PointCloud<pcl::Normal>::Ptr normals;
00231 if(scan.is2d())
00232 {
00233 normals = util3d::computeNormals2D(cloud, normalK, normalRadius);
00234 scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYNormal, scan.localTransform());
00235 }
00236 else
00237 {
00238 normals = util3d::computeNormals(cloud, normalK, normalRadius);
00239 scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYZNormal, scan.localTransform());
00240 }
00241 UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
00242 }
00243 else
00244 {
00245 if(scan.hasNormals())
00246 {
00247 UWARN("Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
00248 }
00249 if(scan.is2d())
00250 {
00251 scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXY, scan.localTransform());
00252 }
00253 else
00254 {
00255 scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYZ, scan.localTransform());
00256 }
00257 }
00258 }
00259 }
00260 }
00261
00262 if(scan.size() && !scan.is2d() && scan.hasNormals() && forceGroundNormalsUp)
00263 {
00264 scan = util3d::adjustNormalsToViewPoint(scan, Eigen::Vector3f(0,0,0), forceGroundNormalsUp);
00265 }
00266 }
00267 return scan;
00268 }
00269
00270 LaserScan rangeFiltering(
00271 const LaserScan & scan,
00272 float rangeMin,
00273 float rangeMax)
00274 {
00275 UASSERT(rangeMin >=0.0f && rangeMax>=0.0f);
00276 if(!scan.isEmpty())
00277 {
00278 if(rangeMin > 0.0f || rangeMax > 0.0f)
00279 {
00280 cv::Mat output = cv::Mat(1, scan.size(), scan.dataType());
00281 bool is2d = scan.is2d();
00282 int oi = 0;
00283 float rangeMinSqrd = rangeMin * rangeMin;
00284 float rangeMaxSqrd = rangeMax * rangeMax;
00285 for(int i=0; i<scan.size(); ++i)
00286 {
00287 const float * ptr = scan.data().ptr<float>(0, i);
00288 float r;
00289 if(is2d)
00290 {
00291 r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
00292 }
00293 else
00294 {
00295 r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
00296 }
00297
00298 if(rangeMin > 0.0f && r < rangeMinSqrd)
00299 {
00300 continue;
00301 }
00302 if(rangeMax > 0.0f && r > rangeMaxSqrd)
00303 {
00304 continue;
00305 }
00306
00307 cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
00308 ++oi;
00309 }
00310 return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0, oi)), scan.maxPoints(), scan.maxRange(), scan.format(), scan.localTransform());
00311 }
00312 }
00313
00314 return scan;
00315 }
00316
00317 LaserScan downsample(
00318 const LaserScan & scan,
00319 int step)
00320 {
00321 UASSERT(step > 0);
00322 if(step <= 1 || scan.size() <= step)
00323 {
00324
00325 return scan;
00326 }
00327 else
00328 {
00329 int finalSize = scan.size()/step;
00330 cv::Mat output = cv::Mat(1, finalSize, scan.dataType());
00331 int oi = 0;
00332 for(int i=0; i<scan.size()-step+1; i+=step)
00333 {
00334 cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
00335 ++oi;
00336 }
00337 return LaserScan(output, scan.maxPoints()/step, scan.maxRange(), scan.format(), scan.localTransform());
00338 }
00339 }
00340 template<typename PointT>
00341 typename pcl::PointCloud<PointT>::Ptr downsampleImpl(
00342 const typename pcl::PointCloud<PointT>::Ptr & cloud,
00343 int step)
00344 {
00345 UASSERT(step > 0);
00346 typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
00347 if(step <= 1 || (int)cloud->size() <= step)
00348 {
00349
00350 *output = *cloud;
00351 }
00352 else
00353 {
00354 int finalSize = int(cloud->size())/step;
00355 output->resize(finalSize);
00356 int oi = 0;
00357 for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
00358 {
00359 (*output)[oi++] = cloud->at(i);
00360 }
00361 }
00362 return output;
00363 }
00364 pcl::PointCloud<pcl::PointXYZ>::Ptr downsample(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int step)
00365 {
00366 return downsampleImpl<pcl::PointXYZ>(cloud, step);
00367 }
00368 pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int step)
00369 {
00370 return downsampleImpl<pcl::PointXYZRGB>(cloud, step);
00371 }
00372 pcl::PointCloud<pcl::PointNormal>::Ptr downsample(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, int step)
00373 {
00374 return downsampleImpl<pcl::PointNormal>(cloud, step);
00375 }
00376 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr downsample(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, int step)
00377 {
00378 return downsampleImpl<pcl::PointXYZRGBNormal>(cloud, step);
00379 }
00380
00381 template<typename PointT>
00382 typename pcl::PointCloud<PointT>::Ptr voxelizeImpl(
00383 const typename pcl::PointCloud<PointT>::Ptr & cloud,
00384 const pcl::IndicesPtr & indices,
00385 float voxelSize)
00386 {
00387 UASSERT(voxelSize > 0.0f);
00388 typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
00389 if((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()))
00390 {
00391 pcl::VoxelGrid<PointT> filter;
00392 filter.setLeafSize(voxelSize, voxelSize, voxelSize);
00393 filter.setInputCloud(cloud);
00394 if(indices->size())
00395 {
00396 filter.setIndices(indices);
00397 }
00398 filter.filter(*output);
00399 }
00400 else if(cloud->size() && !cloud->is_dense && indices->size() == 0)
00401 {
00402 UWARN("Cannot voxelize a not dense (organized) cloud with empty indices! (input=%d pts). Returning empty cloud!", (int)cloud->size());
00403 }
00404 return output;
00405 }
00406
00407 pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
00408 {
00409 return voxelizeImpl<pcl::PointXYZ>(cloud, indices, voxelSize);
00410 }
00411 pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
00412 {
00413 return voxelizeImpl<pcl::PointNormal>(cloud, indices, voxelSize);
00414 }
00415 pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
00416 {
00417 return voxelizeImpl<pcl::PointXYZRGB>(cloud, indices, voxelSize);
00418 }
00419 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
00420 {
00421 return voxelizeImpl<pcl::PointXYZRGBNormal>(cloud, indices, voxelSize);
00422 }
00423 pcl::PointCloud<pcl::PointXYZI>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
00424 {
00425 return voxelizeImpl<pcl::PointXYZI>(cloud, indices, voxelSize);
00426 }
00427 pcl::PointCloud<pcl::PointXYZINormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
00428 {
00429 return voxelizeImpl<pcl::PointXYZINormal>(cloud, indices, voxelSize);
00430 }
00431
00432 pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, float voxelSize)
00433 {
00434 pcl::IndicesPtr indices(new std::vector<int>);
00435 return voxelize(cloud, indices, voxelSize);
00436 }
00437 pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, float voxelSize)
00438 {
00439 pcl::IndicesPtr indices(new std::vector<int>);
00440 return voxelize(cloud, indices, voxelSize);
00441 }
00442 pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, float voxelSize)
00443 {
00444 pcl::IndicesPtr indices(new std::vector<int>);
00445 return voxelize(cloud, indices, voxelSize);
00446 }
00447 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, float voxelSize)
00448 {
00449 pcl::IndicesPtr indices(new std::vector<int>);
00450 return voxelize(cloud, indices, voxelSize);
00451 }
00452 pcl::PointCloud<pcl::PointXYZI>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, float voxelSize)
00453 {
00454 pcl::IndicesPtr indices(new std::vector<int>);
00455 return voxelize(cloud, indices, voxelSize);
00456 }
00457 pcl::PointCloud<pcl::PointXYZINormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, float voxelSize)
00458 {
00459 pcl::IndicesPtr indices(new std::vector<int>);
00460 return voxelize(cloud, indices, voxelSize);
00461 }
00462
00463 template<typename PointT>
00464 typename pcl::PointCloud<PointT>::Ptr randomSamplingImpl(
00465 const typename pcl::PointCloud<PointT>::Ptr & cloud, int samples)
00466 {
00467 UASSERT(samples > 0);
00468 typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
00469 pcl::RandomSample<PointT> filter;
00470 filter.setSample(samples);
00471 filter.setInputCloud(cloud);
00472 filter.filter(*output);
00473 return output;
00474 }
00475 pcl::PointCloud<pcl::PointXYZ>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int samples)
00476 {
00477 return randomSamplingImpl<pcl::PointXYZ>(cloud, samples);
00478 }
00479 pcl::PointCloud<pcl::PointXYZRGB>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int samples)
00480 {
00481 return randomSamplingImpl<pcl::PointXYZRGB>(cloud, samples);
00482 }
00483
00484 template<typename PointT>
00485 pcl::IndicesPtr passThroughImpl(
00486 const typename pcl::PointCloud<PointT>::Ptr & cloud,
00487 const pcl::IndicesPtr & indices,
00488 const std::string & axis,
00489 float min,
00490 float max,
00491 bool negative)
00492 {
00493 UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
00494 UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
00495
00496 pcl::IndicesPtr output(new std::vector<int>);
00497 pcl::PassThrough<PointT> filter;
00498 filter.setNegative(negative);
00499 filter.setFilterFieldName(axis);
00500 filter.setFilterLimits(min, max);
00501 filter.setInputCloud(cloud);
00502 filter.setIndices(indices);
00503 filter.filter(*output);
00504 return output;
00505 }
00506
00507 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
00508 {
00509 return passThroughImpl<pcl::PointXYZ>(cloud, indices, axis, min, max, negative);
00510 }
00511 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
00512 {
00513 return passThroughImpl<pcl::PointXYZRGB>(cloud, indices, axis, min, max, negative);
00514 }
00515 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
00516 {
00517 return passThroughImpl<pcl::PointXYZI>(cloud, indices, axis, min, max, negative);
00518 }
00519 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
00520 {
00521 return passThroughImpl<pcl::PointNormal>(cloud, indices, axis, min, max, negative);
00522 }
00523 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
00524 {
00525 return passThroughImpl<pcl::PointXYZRGBNormal>(cloud, indices, axis, min, max, negative);
00526 }
00527 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
00528 {
00529 return passThroughImpl<pcl::PointXYZINormal>(cloud, indices, axis, min, max, negative);
00530 }
00531
00532 template<typename PointT>
00533 typename pcl::PointCloud<PointT>::Ptr passThroughImpl(
00534 const typename pcl::PointCloud<PointT>::Ptr & cloud,
00535 const std::string & axis,
00536 float min,
00537 float max,
00538 bool negative)
00539 {
00540 UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
00541 UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
00542
00543 typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
00544 pcl::PassThrough<PointT> filter;
00545 filter.setNegative(negative);
00546 filter.setFilterFieldName(axis);
00547 filter.setFilterLimits(min, max);
00548 filter.setInputCloud(cloud);
00549 filter.filter(*output);
00550 return output;
00551 }
00552 pcl::PointCloud<pcl::PointXYZ>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
00553 {
00554 return passThroughImpl<pcl::PointXYZ>(cloud, axis, min ,max, negative);
00555 }
00556 pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
00557 {
00558 return passThroughImpl<pcl::PointXYZRGB>(cloud, axis, min ,max, negative);
00559 }
00560 pcl::PointCloud<pcl::PointXYZI>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
00561 {
00562 return passThroughImpl<pcl::PointXYZI>(cloud, axis, min ,max, negative);
00563 }
00564 pcl::PointCloud<pcl::PointNormal>::Ptr passThrough(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
00565 {
00566 return passThroughImpl<pcl::PointNormal>(cloud, axis, min ,max, negative);
00567 }
00568 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
00569 {
00570 return passThroughImpl<pcl::PointXYZRGBNormal>(cloud, axis, min ,max, negative);
00571 }
00572 pcl::PointCloud<pcl::PointXYZINormal>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
00573 {
00574 return passThroughImpl<pcl::PointXYZINormal>(cloud, axis, min ,max, negative);
00575 }
00576
00577 template<typename PointT>
00578 pcl::IndicesPtr cropBoxImpl(
00579 const typename pcl::PointCloud<PointT>::Ptr & cloud,
00580 const pcl::IndicesPtr & indices,
00581 const Eigen::Vector4f & min,
00582 const Eigen::Vector4f & max,
00583 const Transform & transform,
00584 bool negative)
00585 {
00586 UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
00587
00588 pcl::IndicesPtr output(new std::vector<int>);
00589 pcl::CropBox<PointT> filter;
00590 filter.setNegative(negative);
00591 filter.setMin(min);
00592 filter.setMax(max);
00593 if(!transform.isNull() && !transform.isIdentity())
00594 {
00595 filter.setTransform(transform.toEigen3f());
00596 }
00597 filter.setInputCloud(cloud);
00598 filter.setIndices(indices);
00599 filter.filter(*output);
00600 return output;
00601 }
00602
00603 pcl::IndicesPtr cropBox(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
00604 {
00605 return cropBoxImpl<pcl::PointXYZ>(cloud, indices, min, max, transform, negative);
00606 }
00607 pcl::IndicesPtr cropBox(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
00608 {
00609 return cropBoxImpl<pcl::PointNormal>(cloud, indices, min, max, transform, negative);
00610 }
00611 pcl::IndicesPtr cropBox(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
00612 {
00613 return cropBoxImpl<pcl::PointXYZRGB>(cloud, indices, min, max, transform, negative);
00614 }
00615 pcl::IndicesPtr cropBox(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
00616 {
00617 return cropBoxImpl<pcl::PointXYZRGBNormal>(cloud, indices, min, max, transform, negative);
00618 }
00619
00620 template<typename PointT>
00621 typename pcl::PointCloud<PointT>::Ptr cropBoxImpl(
00622 const typename pcl::PointCloud<PointT>::Ptr & cloud,
00623 const Eigen::Vector4f & min,
00624 const Eigen::Vector4f & max,
00625 const Transform & transform,
00626 bool negative)
00627 {
00628 UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
00629
00630 typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
00631 pcl::CropBox<PointT> filter;
00632 filter.setNegative(negative);
00633 filter.setMin(min);
00634 filter.setMax(max);
00635 if(!transform.isNull() && !transform.isIdentity())
00636 {
00637 filter.setTransform(transform.toEigen3f());
00638 }
00639 filter.setInputCloud(cloud);
00640 filter.filter(*output);
00641 return output;
00642 }
00643 pcl::PointCloud<pcl::PointXYZ>::Ptr cropBox(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
00644 {
00645 return cropBoxImpl<pcl::PointXYZ>(cloud, min, max, transform, negative);
00646 }
00647 pcl::PointCloud<pcl::PointNormal>::Ptr cropBox(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
00648 {
00649 return cropBoxImpl<pcl::PointNormal>(cloud, min, max, transform, negative);
00650 }
00651 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cropBox(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
00652 {
00653 return cropBoxImpl<pcl::PointXYZRGB>(cloud, min, max, transform, negative);
00654 }
00655 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cropBox(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
00656 {
00657 return cropBoxImpl<pcl::PointXYZRGBNormal>(cloud, min, max, transform, negative);
00658 }
00659
00660 template<typename PointT>
00661 pcl::IndicesPtr frustumFilteringImpl(
00662 const typename pcl::PointCloud<PointT>::Ptr & cloud,
00663 const pcl::IndicesPtr & indices,
00664 const Transform & cameraPose,
00665 float horizontalFOV,
00666 float verticalFOV,
00667 float nearClipPlaneDistance,
00668 float farClipPlaneDistance,
00669 bool negative)
00670 {
00671 UASSERT(horizontalFOV > 0.0f && verticalFOV > 0.0f);
00672 UASSERT(farClipPlaneDistance > nearClipPlaneDistance);
00673 UASSERT(!cameraPose.isNull());
00674
00675 pcl::IndicesPtr output(new std::vector<int>);
00676 pcl::FrustumCulling<PointT> fc;
00677 fc.setNegative(negative);
00678 fc.setInputCloud (cloud);
00679 if(indices.get() && indices->size())
00680 {
00681 fc.setIndices(indices);
00682 }
00683 fc.setVerticalFOV (verticalFOV);
00684 fc.setHorizontalFOV (horizontalFOV);
00685 fc.setNearPlaneDistance (nearClipPlaneDistance);
00686 fc.setFarPlaneDistance (farClipPlaneDistance);
00687
00688 fc.setCameraPose (cameraPose.toEigen4f());
00689 fc.filter (*output);
00690
00691 return output;
00692 }
00693 pcl::IndicesPtr frustumFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, const Transform & cameraPose, float horizontalFOV, float verticalFOV, float nearClipPlaneDistance, float farClipPlaneDistance, bool negative)
00694 {
00695 return frustumFilteringImpl<pcl::PointXYZ>(cloud, indices, cameraPose, horizontalFOV, verticalFOV, nearClipPlaneDistance, farClipPlaneDistance, negative);
00696 }
00697
00698 template<typename PointT>
00699 typename pcl::PointCloud<PointT>::Ptr frustumFilteringImpl(
00700 const typename pcl::PointCloud<PointT>::Ptr & cloud,
00701 const Transform & cameraPose,
00702 float horizontalFOV,
00703 float verticalFOV,
00704 float nearClipPlaneDistance,
00705 float farClipPlaneDistance,
00706 bool negative)
00707 {
00708 UASSERT(horizontalFOV > 0.0f && verticalFOV > 0.0f);
00709 UASSERT(farClipPlaneDistance > nearClipPlaneDistance);
00710 UASSERT(!cameraPose.isNull());
00711
00712 typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
00713 pcl::FrustumCulling<PointT> fc;
00714 fc.setNegative(negative);
00715 fc.setInputCloud (cloud);
00716 fc.setVerticalFOV (verticalFOV);
00717 fc.setHorizontalFOV (horizontalFOV);
00718 fc.setNearPlaneDistance (nearClipPlaneDistance);
00719 fc.setFarPlaneDistance (farClipPlaneDistance);
00720
00721 fc.setCameraPose (cameraPose.toEigen4f());
00722 fc.filter (*output);
00723
00724 return output;
00725 }
00726 pcl::PointCloud<pcl::PointXYZ>::Ptr frustumFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const Transform & cameraPose, float horizontalFOV, float verticalFOV, float nearClipPlaneDistance, float farClipPlaneDistance, bool negative)
00727 {
00728 return frustumFilteringImpl<pcl::PointXYZ>(cloud, cameraPose, horizontalFOV, verticalFOV, nearClipPlaneDistance, farClipPlaneDistance, negative);
00729 }
00730 pcl::PointCloud<pcl::PointXYZRGB>::Ptr frustumFiltering(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const Transform & cameraPose, float horizontalFOV, float verticalFOV, float nearClipPlaneDistance, float farClipPlaneDistance, bool negative)
00731 {
00732 return frustumFilteringImpl<pcl::PointXYZRGB>(cloud, cameraPose, horizontalFOV, verticalFOV, nearClipPlaneDistance, farClipPlaneDistance, negative);
00733 }
00734
00735
00736 template<typename PointT>
00737 typename pcl::PointCloud<PointT>::Ptr removeNaNFromPointCloudImpl(
00738 const typename pcl::PointCloud<PointT>::Ptr & cloud)
00739 {
00740 typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
00741 std::vector<int> indices;
00742 pcl::removeNaNFromPointCloud(*cloud, *output, indices);
00743 return output;
00744 }
00745 pcl::PointCloud<pcl::PointXYZ>::Ptr removeNaNFromPointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud)
00746 {
00747 return removeNaNFromPointCloudImpl<pcl::PointXYZ>(cloud);
00748 }
00749 pcl::PointCloud<pcl::PointXYZRGB>::Ptr removeNaNFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud)
00750 {
00751 return removeNaNFromPointCloudImpl<pcl::PointXYZRGB>(cloud);
00752 }
00753
00754 template<typename PointT>
00755 typename pcl::PointCloud<PointT>::Ptr removeNaNNormalsFromPointCloudImpl(
00756 const typename pcl::PointCloud<PointT>::Ptr & cloud)
00757 {
00758 typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
00759 std::vector<int> indices;
00760 pcl::removeNaNNormalsFromPointCloud(*cloud, *output, indices);
00761 return output;
00762 }
00763 pcl::PointCloud<pcl::PointNormal>::Ptr removeNaNNormalsFromPointCloud(
00764 const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud)
00765 {
00766 return removeNaNNormalsFromPointCloudImpl<pcl::PointNormal>(cloud);
00767 }
00768 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr removeNaNNormalsFromPointCloud(
00769 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud)
00770 {
00771 return removeNaNNormalsFromPointCloudImpl<pcl::PointXYZRGBNormal>(cloud);
00772 }
00773
00774
00775 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
00776 {
00777 pcl::IndicesPtr indices(new std::vector<int>);
00778 return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
00779 }
00780 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
00781 {
00782 pcl::IndicesPtr indices(new std::vector<int>);
00783 return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
00784 }
00785 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
00786 {
00787 pcl::IndicesPtr indices(new std::vector<int>);
00788 return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
00789 }
00790 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
00791 {
00792 pcl::IndicesPtr indices(new std::vector<int>);
00793 return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
00794 }
00795
00796 template<typename PointT>
00797 pcl::IndicesPtr radiusFilteringImpl(
00798 const typename pcl::PointCloud<PointT>::Ptr & cloud,
00799 const pcl::IndicesPtr & indices,
00800 float radiusSearch,
00801 int minNeighborsInRadius)
00802 {
00803 typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(false));
00804
00805 if(indices->size())
00806 {
00807 pcl::IndicesPtr output(new std::vector<int>(indices->size()));
00808 int oi = 0;
00809 tree->setInputCloud(cloud, indices);
00810 for(unsigned int i=0; i<indices->size(); ++i)
00811 {
00812 std::vector<int> kIndices;
00813 std::vector<float> kDistances;
00814 int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
00815 if(k > minNeighborsInRadius)
00816 {
00817 output->at(oi++) = indices->at(i);
00818 }
00819 }
00820 output->resize(oi);
00821 return output;
00822 }
00823 else
00824 {
00825 pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
00826 int oi = 0;
00827 tree->setInputCloud(cloud);
00828 for(unsigned int i=0; i<cloud->size(); ++i)
00829 {
00830 std::vector<int> kIndices;
00831 std::vector<float> kDistances;
00832 int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
00833 if(k > minNeighborsInRadius)
00834 {
00835 output->at(oi++) = i;
00836 }
00837 }
00838 output->resize(oi);
00839 return output;
00840 }
00841 }
00842
00843 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
00844 {
00845 return radiusFilteringImpl<pcl::PointXYZ>(cloud, indices, radiusSearch, minNeighborsInRadius);
00846 }
00847 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
00848 {
00849 return radiusFilteringImpl<pcl::PointNormal>(cloud, indices, radiusSearch, minNeighborsInRadius);
00850 }
00851 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
00852 {
00853 return radiusFilteringImpl<pcl::PointXYZRGB>(cloud, indices, radiusSearch, minNeighborsInRadius);
00854 }
00855 pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius)
00856 {
00857 return radiusFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, radiusSearch, minNeighborsInRadius);
00858 }
00859
00860 pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractFiltering(
00861 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
00862 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
00863 float radiusSearch,
00864 int minNeighborsInRadius)
00865 {
00866 pcl::IndicesPtr indices(new std::vector<int>);
00867 pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, minNeighborsInRadius);
00868 pcl::PointCloud<pcl::PointXYZRGB>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGB>);
00869 pcl::copyPointCloud(*cloud, *indicesOut, *out);
00870 return out;
00871 }
00872
00873 template<typename PointT>
00874 pcl::IndicesPtr subtractFilteringImpl(
00875 const typename pcl::PointCloud<PointT>::Ptr & cloud,
00876 const pcl::IndicesPtr & indices,
00877 const typename pcl::PointCloud<PointT>::Ptr & substractCloud,
00878 const pcl::IndicesPtr & substractIndices,
00879 float radiusSearch,
00880 int minNeighborsInRadius)
00881 {
00882 UASSERT(minNeighborsInRadius > 0);
00883 typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(false));
00884
00885 if(indices->size())
00886 {
00887 pcl::IndicesPtr output(new std::vector<int>(indices->size()));
00888 int oi = 0;
00889 if(substractIndices->size())
00890 {
00891 tree->setInputCloud(substractCloud, substractIndices);
00892 }
00893 else
00894 {
00895 tree->setInputCloud(substractCloud);
00896 }
00897 for(unsigned int i=0; i<indices->size(); ++i)
00898 {
00899 std::vector<int> kIndices;
00900 std::vector<float> kDistances;
00901 int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
00902 if(k < minNeighborsInRadius)
00903 {
00904 output->at(oi++) = indices->at(i);
00905 }
00906 }
00907 output->resize(oi);
00908 return output;
00909 }
00910 else
00911 {
00912 pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
00913 int oi = 0;
00914 if(substractIndices->size())
00915 {
00916 tree->setInputCloud(substractCloud, substractIndices);
00917 }
00918 else
00919 {
00920 tree->setInputCloud(substractCloud);
00921 }
00922 for(unsigned int i=0; i<cloud->size(); ++i)
00923 {
00924 std::vector<int> kIndices;
00925 std::vector<float> kDistances;
00926 int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
00927 if(k < minNeighborsInRadius)
00928 {
00929 output->at(oi++) = i;
00930 }
00931 }
00932 output->resize(oi);
00933 return output;
00934 }
00935 }
00936 pcl::IndicesPtr subtractFiltering(
00937 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
00938 const pcl::IndicesPtr & indices,
00939 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
00940 const pcl::IndicesPtr & substractIndices,
00941 float radiusSearch,
00942 int minNeighborsInRadius)
00943 {
00944 return subtractFilteringImpl<pcl::PointXYZRGB>(cloud, indices, substractCloud, substractIndices, radiusSearch, minNeighborsInRadius);
00945 }
00946
00947 pcl::PointCloud<pcl::PointNormal>::Ptr subtractFiltering(
00948 const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
00949 const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
00950 float radiusSearch,
00951 float maxAngle,
00952 int minNeighborsInRadius)
00953 {
00954 pcl::IndicesPtr indices(new std::vector<int>);
00955 pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
00956 pcl::PointCloud<pcl::PointNormal>::Ptr out(new pcl::PointCloud<pcl::PointNormal>);
00957 pcl::copyPointCloud(*cloud, *indicesOut, *out);
00958 return out;
00959 }
00960 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr subtractFiltering(
00961 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
00962 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
00963 float radiusSearch,
00964 float maxAngle,
00965 int minNeighborsInRadius)
00966 {
00967 pcl::IndicesPtr indices(new std::vector<int>);
00968 pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
00969 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00970 pcl::copyPointCloud(*cloud, *indicesOut, *out);
00971 return out;
00972 }
00973
00974 template<typename PointT>
00975 pcl::IndicesPtr subtractFilteringImpl(
00976 const typename pcl::PointCloud<PointT>::Ptr & cloud,
00977 const pcl::IndicesPtr & indices,
00978 const typename pcl::PointCloud<PointT>::Ptr & substractCloud,
00979 const pcl::IndicesPtr & substractIndices,
00980 float radiusSearch,
00981 float maxAngle,
00982 int minNeighborsInRadius)
00983 {
00984 UASSERT(minNeighborsInRadius > 0);
00985 typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(false));
00986
00987 if(indices->size())
00988 {
00989 pcl::IndicesPtr output(new std::vector<int>(indices->size()));
00990 int oi = 0;
00991 if(substractIndices->size())
00992 {
00993 tree->setInputCloud(substractCloud, substractIndices);
00994 }
00995 else
00996 {
00997 tree->setInputCloud(substractCloud);
00998 }
00999 for(unsigned int i=0; i<indices->size(); ++i)
01000 {
01001 std::vector<int> kIndices;
01002 std::vector<float> kDistances;
01003 int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
01004 if(k>=minNeighborsInRadius && maxAngle > 0.0f)
01005 {
01006 Eigen::Vector4f normal(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
01007 if (uIsFinite(normal[0]) &&
01008 uIsFinite(normal[1]) &&
01009 uIsFinite(normal[2]))
01010 {
01011 int count = k;
01012 for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
01013 {
01014 Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
01015 if(uIsFinite(v[0]) &&
01016 uIsFinite(v[1]) &&
01017 uIsFinite(v[2]))
01018 {
01019 float angle = pcl::getAngle3D(normal, v);
01020 if(angle > maxAngle)
01021 {
01022 k-=1;
01023 }
01024 }
01025 else
01026 {
01027 k-=1;
01028 }
01029 }
01030 }
01031 else
01032 {
01033 k=0;
01034 }
01035 }
01036 if(k < minNeighborsInRadius)
01037 {
01038 output->at(oi++) = indices->at(i);
01039 }
01040 }
01041 output->resize(oi);
01042 return output;
01043 }
01044 else
01045 {
01046 pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
01047 int oi = 0;
01048 if(substractIndices->size())
01049 {
01050 tree->setInputCloud(substractCloud, substractIndices);
01051 }
01052 else
01053 {
01054 tree->setInputCloud(substractCloud);
01055 }
01056 for(unsigned int i=0; i<cloud->size(); ++i)
01057 {
01058 std::vector<int> kIndices;
01059 std::vector<float> kDistances;
01060 int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
01061 if(k>=minNeighborsInRadius && maxAngle > 0.0f)
01062 {
01063 Eigen::Vector4f normal(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
01064 if (uIsFinite(normal[0]) &&
01065 uIsFinite(normal[1]) &&
01066 uIsFinite(normal[2]))
01067 {
01068 int count = k;
01069 for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
01070 {
01071 Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
01072 if(uIsFinite(v[0]) &&
01073 uIsFinite(v[1]) &&
01074 uIsFinite(v[2]))
01075 {
01076 float angle = pcl::getAngle3D(normal, v);
01077 if(angle > maxAngle)
01078 {
01079 k-=1;
01080 }
01081 }
01082 else
01083 {
01084 k-=1;
01085 }
01086 }
01087 }
01088 else
01089 {
01090 k=0;
01091 }
01092 }
01093 if(k < minNeighborsInRadius)
01094 {
01095 output->at(oi++) = i;
01096 }
01097 }
01098 output->resize(oi);
01099 return output;
01100 }
01101 }
01102
01103 pcl::IndicesPtr subtractFiltering(
01104 const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
01105 const pcl::IndicesPtr & indices,
01106 const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
01107 const pcl::IndicesPtr & substractIndices,
01108 float radiusSearch,
01109 float maxAngle,
01110 int minNeighborsInRadius)
01111 {
01112 return subtractFilteringImpl<pcl::PointNormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
01113 }
01114 pcl::IndicesPtr subtractFiltering(
01115 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
01116 const pcl::IndicesPtr & indices,
01117 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
01118 const pcl::IndicesPtr & substractIndices,
01119 float radiusSearch,
01120 float maxAngle,
01121 int minNeighborsInRadius)
01122 {
01123 return subtractFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
01124 }
01125
01126 pcl::IndicesPtr subtractAdaptiveFiltering(
01127 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
01128 const pcl::IndicesPtr & indices,
01129 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
01130 const pcl::IndicesPtr & substractIndices,
01131 float radiusSearchRatio,
01132 int minNeighborsInRadius,
01133 const Eigen::Vector3f & viewpoint)
01134 {
01135 UWARN("Add angle to avoid subtraction of points with opposite normals");
01136 UASSERT(minNeighborsInRadius > 0);
01137 UASSERT(radiusSearchRatio > 0.0f);
01138 pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>(false));
01139
01140 if(indices->size())
01141 {
01142 pcl::IndicesPtr output(new std::vector<int>(indices->size()));
01143 int oi = 0;
01144 if(substractIndices->size())
01145 {
01146 tree->setInputCloud(substractCloud, substractIndices);
01147 }
01148 else
01149 {
01150 tree->setInputCloud(substractCloud);
01151 }
01152 for(unsigned int i=0; i<indices->size(); ++i)
01153 {
01154 if(pcl::isFinite(cloud->at(indices->at(i))))
01155 {
01156 std::vector<int> kIndices;
01157 std::vector<float> kSqrdDistances;
01158 float radius = radiusSearchRatio*uNorm(
01159 cloud->at(indices->at(i)).x-viewpoint[0],
01160 cloud->at(indices->at(i)).y-viewpoint[1],
01161 cloud->at(indices->at(i)).z-viewpoint[2]);
01162 int k = tree->radiusSearch(cloud->at(indices->at(i)), radius, kIndices, kSqrdDistances);
01163 if(k < minNeighborsInRadius)
01164 {
01165 output->at(oi++) = indices->at(i);
01166 }
01167 }
01168 }
01169 output->resize(oi);
01170 return output;
01171 }
01172 else
01173 {
01174 pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
01175 int oi = 0;
01176 if(substractIndices->size())
01177 {
01178 tree->setInputCloud(substractCloud, substractIndices);
01179 }
01180 else
01181 {
01182 tree->setInputCloud(substractCloud);
01183 }
01184 for(unsigned int i=0; i<cloud->size(); ++i)
01185 {
01186 if(pcl::isFinite(cloud->at(i)))
01187 {
01188 std::vector<int> kIndices;
01189 std::vector<float> kSqrdDistances;
01190 float radius = radiusSearchRatio*uNorm(
01191 cloud->at(i).x-viewpoint[0],
01192 cloud->at(i).y-viewpoint[1],
01193 cloud->at(i).z-viewpoint[2]);
01194 int k = tree->radiusSearch(cloud->at(i), radius, kIndices, kSqrdDistances);
01195 if(k < minNeighborsInRadius)
01196 {
01197 output->at(oi++) = i;
01198 }
01199 }
01200 }
01201 output->resize(oi);
01202 return output;
01203 }
01204 }
01205 pcl::IndicesPtr subtractAdaptiveFiltering(
01206 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
01207 const pcl::IndicesPtr & indices,
01208 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
01209 const pcl::IndicesPtr & substractIndices,
01210 float radiusSearchRatio,
01211 float maxAngle,
01212 int minNeighborsInRadius,
01213 const Eigen::Vector3f & viewpoint)
01214 {
01215 UASSERT(minNeighborsInRadius > 0);
01216 UASSERT(radiusSearchRatio > 0.0f);
01217 pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>(false));
01218
01219 if(indices->size())
01220 {
01221 pcl::IndicesPtr output(new std::vector<int>(indices->size()));
01222 int oi = 0;
01223 if(substractIndices->size())
01224 {
01225 tree->setInputCloud(substractCloud, substractIndices);
01226 }
01227 else
01228 {
01229 tree->setInputCloud(substractCloud);
01230 }
01231 for(unsigned int i=0; i<indices->size(); ++i)
01232 {
01233 if(pcl::isFinite(cloud->at(indices->at(i))))
01234 {
01235 std::vector<int> kIndices;
01236 std::vector<float> kSqrdDistances;
01237 float radius = radiusSearchRatio*uNorm(
01238 cloud->at(indices->at(i)).x-viewpoint[0],
01239 cloud->at(indices->at(i)).y-viewpoint[1],
01240 cloud->at(indices->at(i)).z-viewpoint[2]);
01241 int k = tree->radiusSearch(cloud->at(indices->at(i)), radius, kIndices, kSqrdDistances);
01242 if(k>=minNeighborsInRadius && maxAngle > 0.0f)
01243 {
01244 Eigen::Vector4f normal(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
01245 if (uIsFinite(normal[0]) &&
01246 uIsFinite(normal[1]) &&
01247 uIsFinite(normal[2]))
01248 {
01249 int count = k;
01250 for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
01251 {
01252 Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
01253 if(uIsFinite(v[0]) &&
01254 uIsFinite(v[1]) &&
01255 uIsFinite(v[2]))
01256 {
01257 float angle = pcl::getAngle3D(normal, v);
01258 if(angle > maxAngle)
01259 {
01260 k-=1;
01261 }
01262 }
01263 else
01264 {
01265 k-=1;
01266 }
01267 }
01268 }
01269 else
01270 {
01271 k=0;
01272 }
01273 }
01274
01275 if(k < minNeighborsInRadius)
01276 {
01277 output->at(oi++) = indices->at(i);
01278 }
01279 }
01280 }
01281 output->resize(oi);
01282 return output;
01283 }
01284 else
01285 {
01286 pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
01287 int oi = 0;
01288 if(substractIndices->size())
01289 {
01290 tree->setInputCloud(substractCloud, substractIndices);
01291 }
01292 else
01293 {
01294 tree->setInputCloud(substractCloud);
01295 }
01296 for(unsigned int i=0; i<cloud->size(); ++i)
01297 {
01298 if(pcl::isFinite(cloud->at(i)))
01299 {
01300 std::vector<int> kIndices;
01301 std::vector<float> kSqrdDistances;
01302 float radius = radiusSearchRatio*uNorm(
01303 cloud->at(i).x-viewpoint[0],
01304 cloud->at(i).y-viewpoint[1],
01305 cloud->at(i).z-viewpoint[2]);
01306 int k = tree->radiusSearch(cloud->at(i), radius, kIndices, kSqrdDistances);
01307 if(k>=minNeighborsInRadius && maxAngle > 0.0f)
01308 {
01309 Eigen::Vector4f normal(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
01310 if (uIsFinite(normal[0]) &&
01311 uIsFinite(normal[1]) &&
01312 uIsFinite(normal[2]))
01313 {
01314 int count = k;
01315 for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
01316 {
01317 Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
01318 if(uIsFinite(v[0]) &&
01319 uIsFinite(v[1]) &&
01320 uIsFinite(v[2]))
01321 {
01322 float angle = pcl::getAngle3D(normal, v);
01323 if(angle > maxAngle)
01324 {
01325 k-=1;
01326 }
01327 }
01328 else
01329 {
01330 k-=1;
01331 }
01332 }
01333 }
01334 else
01335 {
01336 k=0;
01337 }
01338 }
01339 if(k < minNeighborsInRadius)
01340 {
01341 output->at(oi++) = i;
01342 }
01343 }
01344 }
01345 output->resize(oi);
01346 return output;
01347 }
01348 }
01349
01350
01351 pcl::IndicesPtr normalFiltering(
01352 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
01353 float angleMax,
01354 const Eigen::Vector4f & normal,
01355 int normalKSearch,
01356 const Eigen::Vector4f & viewpoint)
01357 {
01358 pcl::IndicesPtr indices(new std::vector<int>);
01359 return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
01360 }
01361 pcl::IndicesPtr normalFiltering(
01362 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
01363 float angleMax,
01364 const Eigen::Vector4f & normal,
01365 int normalKSearch,
01366 const Eigen::Vector4f & viewpoint)
01367 {
01368 pcl::IndicesPtr indices(new std::vector<int>);
01369 return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
01370 }
01371
01372
01373 template<typename PointT>
01374 pcl::IndicesPtr normalFilteringImpl(
01375 const typename pcl::PointCloud<PointT>::Ptr & cloud,
01376 const pcl::IndicesPtr & indices,
01377 float angleMax,
01378 const Eigen::Vector4f & normal,
01379 int normalKSearch,
01380 const Eigen::Vector4f & viewpoint)
01381 {
01382 pcl::IndicesPtr output(new std::vector<int>());
01383
01384 if(cloud->size())
01385 {
01386 typename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>(false));
01387
01388 pcl::NormalEstimationOMP<PointT, pcl::Normal> ne;
01389 ne.setInputCloud (cloud);
01390 if(indices->size())
01391 {
01392 ne.setIndices(indices);
01393 }
01394
01395 if(indices->size())
01396 {
01397 tree->setInputCloud(cloud, indices);
01398 }
01399 else
01400 {
01401 tree->setInputCloud(cloud);
01402 }
01403 ne.setSearchMethod (tree);
01404
01405 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
01406
01407 ne.setKSearch(normalKSearch);
01408 if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
01409 {
01410 ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
01411 }
01412
01413 ne.compute (*cloud_normals);
01414
01415 output->resize(cloud_normals->size());
01416 int oi = 0;
01417 for(unsigned int i=0; i<cloud_normals->size(); ++i)
01418 {
01419 Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
01420 float angle = pcl::getAngle3D(normal, v);
01421 if(angle < angleMax)
01422 {
01423 output->at(oi++) = indices->size()!=0?indices->at(i):i;
01424 }
01425 }
01426 output->resize(oi);
01427 }
01428
01429 return output;
01430 }
01431 pcl::IndicesPtr normalFiltering(
01432 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
01433 const pcl::IndicesPtr & indices,
01434 float angleMax,
01435 const Eigen::Vector4f & normal,
01436 int normalKSearch,
01437 const Eigen::Vector4f & viewpoint)
01438
01439 {
01440 return normalFilteringImpl<pcl::PointXYZ>(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
01441 }
01442 pcl::IndicesPtr normalFiltering(
01443 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
01444 const pcl::IndicesPtr & indices,
01445 float angleMax,
01446 const Eigen::Vector4f & normal,
01447 int normalKSearch,
01448 const Eigen::Vector4f & viewpoint)
01449 {
01450 return normalFilteringImpl<pcl::PointXYZRGB>(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
01451 }
01452
01453 template<typename PointT>
01454 pcl::IndicesPtr normalFilteringImpl(
01455 const typename pcl::PointCloud<PointT>::Ptr & cloud,
01456 const pcl::IndicesPtr & indices,
01457 float angleMax,
01458 const Eigen::Vector4f & normal)
01459 {
01460 pcl::IndicesPtr output(new std::vector<int>());
01461
01462 if(cloud->size())
01463 {
01464 int oi = 0;
01465 if(indices->size())
01466 {
01467 output->resize(indices->size());
01468 for(unsigned int i=0; i<indices->size(); ++i)
01469 {
01470 Eigen::Vector4f v(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
01471 float angle = pcl::getAngle3D(normal, v);
01472 if(angle < angleMax)
01473 {
01474 output->at(oi++) = indices->size()!=0?indices->at(i):i;
01475 }
01476 }
01477 }
01478 else
01479 {
01480 output->resize(cloud->size());
01481 for(unsigned int i=0; i<cloud->size(); ++i)
01482 {
01483 Eigen::Vector4f v(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
01484 float angle = pcl::getAngle3D(normal, v);
01485 if(angle < angleMax)
01486 {
01487 output->at(oi++) = indices->size()!=0?indices->at(i):i;
01488 }
01489 }
01490 }
01491
01492 output->resize(oi);
01493 }
01494
01495 return output;
01496 }
01497 pcl::IndicesPtr normalFiltering(
01498 const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
01499 const pcl::IndicesPtr & indices,
01500 float angleMax,
01501 const Eigen::Vector4f & normal,
01502 int normalKSearch,
01503 const Eigen::Vector4f & viewpoint)
01504 {
01505 return normalFilteringImpl<pcl::PointNormal>(cloud, indices, angleMax, normal);
01506 }
01507 pcl::IndicesPtr normalFiltering(
01508 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
01509 const pcl::IndicesPtr & indices,
01510 float angleMax,
01511 const Eigen::Vector4f & normal,
01512 int normalKSearch,
01513 const Eigen::Vector4f & viewpoint)
01514 {
01515 return normalFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, angleMax, normal);
01516 }
01517
01518 std::vector<pcl::IndicesPtr> extractClusters(
01519 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
01520 float clusterTolerance,
01521 int minClusterSize,
01522 int maxClusterSize,
01523 int * biggestClusterIndex)
01524 {
01525 pcl::IndicesPtr indices(new std::vector<int>);
01526 return extractClusters(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
01527 }
01528 std::vector<pcl::IndicesPtr> extractClusters(
01529 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
01530 float clusterTolerance,
01531 int minClusterSize,
01532 int maxClusterSize,
01533 int * biggestClusterIndex)
01534 {
01535 pcl::IndicesPtr indices(new std::vector<int>);
01536 return extractClusters(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
01537 }
01538
01539 template<typename PointT>
01540 std::vector<pcl::IndicesPtr> extractClustersImpl(
01541 const typename pcl::PointCloud<PointT>::Ptr & cloud,
01542 const pcl::IndicesPtr & indices,
01543 float clusterTolerance,
01544 int minClusterSize,
01545 int maxClusterSize,
01546 int * biggestClusterIndex)
01547 {
01548 typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(true));
01549 pcl::EuclideanClusterExtraction<PointT> ec;
01550 ec.setClusterTolerance (clusterTolerance);
01551 ec.setMinClusterSize (minClusterSize);
01552 ec.setMaxClusterSize (maxClusterSize);
01553 ec.setInputCloud (cloud);
01554
01555 if(indices->size())
01556 {
01557 ec.setIndices(indices);
01558 tree->setInputCloud(cloud, indices);
01559 }
01560 else
01561 {
01562 tree->setInputCloud(cloud);
01563 }
01564 ec.setSearchMethod (tree);
01565
01566 std::vector<pcl::PointIndices> cluster_indices;
01567 ec.extract (cluster_indices);
01568
01569 int maxIndex=-1;
01570 unsigned int maxSize = 0;
01571 std::vector<pcl::IndicesPtr> output(cluster_indices.size());
01572 for(unsigned int i=0; i<cluster_indices.size(); ++i)
01573 {
01574 output[i] = pcl::IndicesPtr(new std::vector<int>(cluster_indices[i].indices));
01575
01576 if(maxSize < cluster_indices[i].indices.size())
01577 {
01578 maxSize = (unsigned int)cluster_indices[i].indices.size();
01579 maxIndex = i;
01580 }
01581 }
01582 if(biggestClusterIndex)
01583 {
01584 *biggestClusterIndex = maxIndex;
01585 }
01586
01587 return output;
01588 }
01589
01590 std::vector<pcl::IndicesPtr> extractClusters(
01591 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
01592 const pcl::IndicesPtr & indices,
01593 float clusterTolerance,
01594 int minClusterSize,
01595 int maxClusterSize,
01596 int * biggestClusterIndex)
01597 {
01598 return extractClustersImpl<pcl::PointXYZ>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
01599 }
01600 std::vector<pcl::IndicesPtr> extractClusters(
01601 const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
01602 const pcl::IndicesPtr & indices,
01603 float clusterTolerance,
01604 int minClusterSize,
01605 int maxClusterSize,
01606 int * biggestClusterIndex)
01607 {
01608 return extractClustersImpl<pcl::PointNormal>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
01609 }
01610 std::vector<pcl::IndicesPtr> extractClusters(
01611 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
01612 const pcl::IndicesPtr & indices,
01613 float clusterTolerance,
01614 int minClusterSize,
01615 int maxClusterSize,
01616 int * biggestClusterIndex)
01617 {
01618 return extractClustersImpl<pcl::PointXYZRGB>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
01619 }
01620 std::vector<pcl::IndicesPtr> extractClusters(
01621 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
01622 const pcl::IndicesPtr & indices,
01623 float clusterTolerance,
01624 int minClusterSize,
01625 int maxClusterSize,
01626 int * biggestClusterIndex)
01627 {
01628 return extractClustersImpl<pcl::PointXYZRGBNormal>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
01629 }
01630
01631 template<typename PointT>
01632 pcl::IndicesPtr extractIndicesImpl(
01633 const typename pcl::PointCloud<PointT>::Ptr & cloud,
01634 const pcl::IndicesPtr & indices,
01635 bool negative)
01636 {
01637 pcl::IndicesPtr output(new std::vector<int>);
01638 pcl::ExtractIndices<PointT> extract;
01639 extract.setInputCloud (cloud);
01640 extract.setIndices(indices);
01641 extract.setNegative(negative);
01642 extract.filter(*output);
01643 return output;
01644 }
01645
01646 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
01647 {
01648 return extractIndicesImpl<pcl::PointXYZ>(cloud, indices, negative);
01649 }
01650 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
01651 {
01652 return extractIndicesImpl<pcl::PointNormal>(cloud, indices, negative);
01653 }
01654 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
01655 {
01656 return extractIndicesImpl<pcl::PointXYZRGB>(cloud, indices, negative);
01657 }
01658 pcl::IndicesPtr extractIndices(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative)
01659 {
01660 return extractIndicesImpl<pcl::PointXYZRGBNormal>(cloud, indices, negative);
01661 }
01662
01663 template<typename PointT>
01664 typename pcl::PointCloud<PointT>::Ptr extractIndicesImpl(
01665 const typename pcl::PointCloud<PointT>::Ptr & cloud,
01666 const pcl::IndicesPtr & indices,
01667 bool negative,
01668 bool keepOrganized)
01669 {
01670 typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
01671 pcl::ExtractIndices<PointT> extract;
01672 extract.setInputCloud (cloud);
01673 extract.setIndices(indices);
01674 extract.setNegative(negative);
01675 extract.setKeepOrganized(keepOrganized);
01676 extract.filter(*output);
01677 return output;
01678 }
01679 pcl::PointCloud<pcl::PointXYZ>::Ptr extractIndices(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized)
01680 {
01681 return extractIndicesImpl<pcl::PointXYZ>(cloud, indices, negative, keepOrganized);
01682 }
01683 pcl::PointCloud<pcl::PointXYZRGB>::Ptr extractIndices(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized)
01684 {
01685 return extractIndicesImpl<pcl::PointXYZRGB>(cloud, indices, negative, keepOrganized);
01686 }
01687
01688
01689
01690
01691
01692 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr extractIndices(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, bool negative, bool keepOrganized)
01693 {
01694 return extractIndicesImpl<pcl::PointXYZRGBNormal>(cloud, indices, negative, keepOrganized);
01695 }
01696
01697 pcl::IndicesPtr extractPlane(
01698 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
01699 float distanceThreshold,
01700 int maxIterations,
01701 pcl::ModelCoefficients * coefficientsOut)
01702 {
01703 pcl::IndicesPtr indices(new std::vector<int>);
01704 return extractPlane(cloud, indices, distanceThreshold, maxIterations, coefficientsOut);
01705 }
01706
01707 pcl::IndicesPtr extractPlane(
01708 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
01709 const pcl::IndicesPtr & indices,
01710 float distanceThreshold,
01711 int maxIterations,
01712 pcl::ModelCoefficients * coefficientsOut)
01713 {
01714
01715 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
01716 pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
01717
01718 pcl::SACSegmentation<pcl::PointXYZ> seg;
01719
01720 seg.setOptimizeCoefficients (true);
01721 seg.setMaxIterations (maxIterations);
01722
01723 seg.setModelType (pcl::SACMODEL_PLANE);
01724 seg.setMethodType (pcl::SAC_RANSAC);
01725 seg.setDistanceThreshold (distanceThreshold);
01726
01727 seg.setInputCloud (cloud);
01728 if(indices->size())
01729 {
01730 seg.setIndices(indices);
01731 }
01732 seg.segment (*inliers, *coefficients);
01733
01734 if(coefficientsOut)
01735 {
01736 *coefficientsOut = *coefficients;
01737 }
01738
01739 return pcl::IndicesPtr(new std::vector<int>(inliers->indices));
01740 }
01741
01742 }
01743
01744 }