i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added util3d::segmentObstaclesFromGround() method, templated some PCL methods (new rtabmap/core/impl/util3d.hpp)

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1921 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-24 16:58:32 +00:00
parent 2bd32e5a95
commit 60b0fd2e98
12 changed files with 658 additions and 626 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/CMakeLists.txt
View File

@@ -126,6 +126,6 @@ INSTALL(TARGETS rtabmap_core
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/
DESTINATION "${INSTALL_INCLUDE_DIR}"
COMPONENT devel
FILES_MATCHING PATTERN "*.h"
FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
PATTERN ".svn" EXCLUDE)

16
corelib/src/Memory.cpp
View File

@@ -1903,8 +1903,8 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
std::vector<int> indices;
newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
oldCloud = util3d::removeNaNNormalsFromPointCloud(oldCloud);
newCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(newCloud);
oldCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(oldCloud);
// 3D
double fitness = 0;
@@ -1981,8 +1981,8 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
//voxelize
if(_icp2VoxelSize > 0.0f)
{
oldCloud = util3d::voxelize(oldCloud, _icp2VoxelSize);
newCloud = util3d::voxelize(newCloud, _icp2VoxelSize);
oldCloud = util3d::voxelize<pcl::PointXYZ>(oldCloud, _icp2VoxelSize);
newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, _icp2VoxelSize);
}
double fitness = 0.0f;
@@ -2000,7 +2000,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
//pcl::io::savePCDFile("lccold.pcd", *oldCloud);
//pcl::io::savePCDFile("lccnewguess.pcd", *newCloud);
newCloud = util3d::transformPointCloud(newCloud, icpT);
newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
//pcl::io::savePCDFile("lccnewicp.pcd", *newCloud);
// verify if there are enough correspondences
@@ -2094,7 +2094,7 @@ Transform Memory::computeScanMatchingTransform(
//voxelize
if(assembledOldClouds->size() && _icp2VoxelSize > 0.0f)
{
assembledOldClouds = util3d::voxelize(assembledOldClouds, _icp2VoxelSize);
assembledOldClouds = util3d::voxelize<pcl::PointXYZ>(assembledOldClouds, _icp2VoxelSize);
}
// get the new cloud
@@ -2106,7 +2106,7 @@ Transform Memory::computeScanMatchingTransform(
//voxelize
if(newCloud->size() && _icp2VoxelSize > 0.0f)
{
newCloud = util3d::voxelize(newCloud, _icp2VoxelSize);
newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, _icp2VoxelSize);
}
//pcl::io::savePCDFile("old.pcd", *assembledOldClouds);
@@ -2126,7 +2126,7 @@ Transform Memory::computeScanMatchingTransform(
UDEBUG("icpT=%s", icpT.prettyPrint().c_str());
newCloud = util3d::transformPointCloud(newCloud, icpT);
newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
//pcl::io::savePCDFile("newCorrected.pcd", *newCloud);
// verify if there enough correspondences

2
corelib/src/Odometry.cpp
View File

@@ -1104,7 +1104,7 @@ Transform OdometryICP::computeTransform(const SensorData & data, int * quality,
pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
std::vector<int> indices;
newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
newCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(newCloud);
if(newCloudXYZ->size() != newCloud->size())
{
UWARN("removed nan normals...");

453
corelib/src/util3d.cpp
View File

@@ -33,9 +33,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/registration/transformation_estimation_2D.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/distances.h>
#include <pcl/surface/gp3.h>
@@ -43,9 +40,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/surface/mls.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/crop_box.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/extract_indices.h>
#include <opencv2/nonfree/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
@@ -640,158 +634,6 @@ pcl::PointXYZ projectDepthTo3D(
return pt;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, float voxelSize)
{
UASSERT(voxelSize > 0.0f);
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
pcl::VoxelGrid<pcl::PointXYZ> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, float voxelSize)
{
UASSERT(voxelSize > 0.0f);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::VoxelGrid<pcl::PointXYZRGB> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr sampling(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int samples)
{
UASSERT(samples > 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
pcl::RandomSample<pcl::PointXYZ> filter;
filter.setSample(samples);
filter.setInputCloud(cloud);
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr sampling(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int samples)
{
UASSERT(samples > 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::RandomSample<pcl::PointXYZRGB> filter;
filter.setSample(samples);
filter.setInputCloud(cloud);
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr passThrough(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const std::string & axis,
float min,
float max)
{
UASSERT(max > min);
UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PassThrough<pcl::PointXYZ> filter;
filter.setFilterFieldName(axis);
filter.setFilterLimits(min, max);
filter.setInputCloud(cloud);
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const std::string & axis,
float min,
float max)
{
UASSERT(max > min);
UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PassThrough<pcl::PointXYZRGB> filter;
filter.setFilterFieldName(axis);
filter.setFilterLimits(min, max);
filter.setInputCloud(cloud);
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<int> indices;
pcl::removeNaNFromPointCloud (*cloud, *output, indices);
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<int> indices;
pcl::removeNaNFromPointCloud (*cloud, *output, indices);
return output;
}
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud)
{
pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
std::vector<int> indices;
pcl::removeNaNNormalsFromPointCloud (*cloud, *output, indices);
return output;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
std::vector<int> indices;
pcl::removeNaNNormalsFromPointCloud(*cloud, *output, indices);
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & transform)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud, *output, transformToEigen4f(transform));
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & transform)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::transformPointCloud(*cloud, *output, transformToEigen4f(transform));
return output;
}
pcl::PointXYZ RTABMAP_EXP transformPoint(
const pcl::PointXYZ & pt,
const Transform & transform)
{
return pcl::transformPoint(pt, transformToEigen3f(transform));
}
pcl::PointXYZRGB RTABMAP_EXP transformPoint(
const pcl::PointXYZRGB & pt,
const Transform & transform)
{
return pcl::transformPoint(pt, transformToEigen3f(transform));
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
const cv::Mat & imageDepth,
float cx, float cy,
@@ -2029,25 +1871,25 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
{
if(maxDepth>0.0)
{
cloud = passThrough(cloud, "z", 0, maxDepth);
cloud = passThrough<pcl::PointXYZ>(cloud, "z", 0, maxDepth);
}
if(cloud->size())
{
if(voxel>0)
{
cloud = voxelize(cloud, voxel);
cloud = voxelize<pcl::PointXYZ>(cloud, voxel);
}
else if(samples>0 && (int)cloud->size() > samples)
{
cloud = sampling(cloud, samples);
cloud = sampling<pcl::PointXYZ>(cloud, samples);
}
if(cloud->size())
{
if(!transform.isNull() && !transform.isIdentity())
{
cloud = transformPointCloud(cloud, transform);
cloud = transformPointCloud<pcl::PointXYZ>(cloud, transform);
}
}
}
@@ -2112,7 +1954,7 @@ pcl::PolygonMesh::Ptr createMesh(
float gp3MaximumAngle,
bool gp3NormalConsistency)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormalsNoNaN = removeNaNNormalsFromPointCloud(cloudWithNormals);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormalsNoNaN = removeNaNNormalsFromPointCloud<pcl::PointXYZRGBNormal>(cloudWithNormals);
// Create search tree*
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
@@ -2538,80 +2380,35 @@ bool occupancy2DFromCloud3D(
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
//voxelize
pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelizedCloud = util3d::voxelize(cloud, cellSize);
//convert to XYZ
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*voxelizedCloud, *cloudXYZ);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelizedCloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, cellSize);
pcl::IndicesPtr groundIndices, obstaclesIndices;
// Find the ground
pcl::IndicesPtr flatSurfaces = util3d::normalFiltering(
cloudXYZ,
segmentObstaclesFromGround<pcl::PointXYZRGB>(cloud,
groundIndices,
obstaclesIndices,
cellSize,
groundNormalAngle,
Eigen::Vector4f(0,0,1,0),
cellSize*2.0f,
Eigen::Vector4f(0,0,100,0));
minClusterSize);
int biggestFlatSurfaceIndex;
std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = util3d::extractClusters(
cloudXYZ,
flatSurfaces,
cellSize*2.0f,
minClusterSize,
std::numeric_limits<int>::max(),
&biggestFlatSurfaceIndex);
// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
groundIndices = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
Eigen::Vector4f min,max;
pcl::getMinMax3D(*cloudXYZ, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
if(groundIndices->size())
{
if((int)i!=biggestFlatSurfaceIndex)
{
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloudXYZ, *clusteredFlatSurfaces.at(i), centroid);
if(centroid[2] >= min[2] && centroid[2] <= max[2])
{
groundIndices = util3d::concatenate(groundIndices, clusteredFlatSurfaces.at(i));
}
}
pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
//project on XY plane
util3d::projectCloudOnXYPlane<pcl::PointXYZ>(groundCloud);
//voxelize to grid cell size
groundCloud = util3d::voxelize<pcl::PointXYZ>(groundCloud, cellSize);
}
pcl::copyPointCloud(*cloudXYZ, *groundIndices, *groundCloud);
if(groundIndices->size() != cloudXYZ->size())
if(obstaclesIndices->size())
{
// Remove ground
pcl::IndicesPtr otherStuffIndices = util3d::extractNegativeIndices(cloudXYZ, groundIndices);
//Cluster remaining stuff (obstacles)
std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
cloudXYZ,
otherStuffIndices,
cellSize*2.0f,
minClusterSize);
// merge indices
obstaclesIndices = util3d::concatenate(clusteredObstaclesSurfaces);
if(obstaclesIndices->size())
{
pcl::copyPointCloud(*cloudXYZ, *obstaclesIndices, *obstaclesCloud);
}
pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
//project on XY plane
util3d::projectCloudOnXYPlane<pcl::PointXYZ>(obstaclesCloud);
//voxelize to grid cell size
obstaclesCloud = util3d::voxelize<pcl::PointXYZ>(obstaclesCloud, cellSize);
}
//project on XY plane
util3d::projectCloudOnXYPlane(groundCloud);
util3d::projectCloudOnXYPlane(obstaclesCloud);
//voxelize to grid cell size
groundCloud = util3d::voxelize(groundCloud, cellSize);
obstaclesCloud = util3d::voxelize(obstaclesCloud, cellSize);
ground = cv::Mat();
if(groundCloud->size())
{
@@ -2846,7 +2643,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
{
if(uContains(scans, iter->first))
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::transformPointCloud(scans.at(iter->first), iter->second);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = transformPointCloud<pcl::PointXYZ>(scans.at(iter->first), iter->second);
pcl::PointXYZ min, max;
pcl::getMinMax3D(*cloud, min, max);
minMax.push_back(min);
@@ -3052,195 +2849,6 @@ cv::Mat convertMap2Image8U(const cv::Mat & map8S)
return map8U;
}
void projectCloudOnXYPlane(
pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud)
{
for(unsigned int i=0; i<cloud->size(); ++i)
{
cloud->at(i).z = 0;
}
}
pcl::IndicesPtr radiusFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
}
pcl::IndicesPtr radiusFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
if(indices->size())
{
pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator
tree->setInputCloud(cloud, indices);
for(unsigned int i=0; i<indices->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
if(k > minNeighborsInRadius)
{
output->at(oi++) = indices->at(i);
}
}
output->resize(oi);
return output;
}
else
{
pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator
tree->setInputCloud(cloud);
for(unsigned int i=0; i<cloud->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
if(k > minNeighborsInRadius)
{
output->at(oi++) = i;
}
}
output->resize(oi);
return output;
}
}
pcl::IndicesPtr normalFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float angleMax,
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint)
{
pcl::IndicesPtr indices(new std::vector<int>);
return normalFiltering(cloud, indices, angleMax, normal, radiusSearch, viewpoint);
}
pcl::IndicesPtr normalFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint)
{
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud);
if(indices->size())
{
ne.setIndices(indices);
}
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
if(indices->size())
{
tree->setInputCloud(cloud, indices);
}
else
{
tree->setInputCloud(cloud);
}
ne.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (radiusSearch);
if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
{
ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
}
ne.compute (*cloud_normals);
pcl::IndicesPtr output(new std::vector<int>(cloud_normals->size()));
int oi = 0; // output iterator
Eigen::Vector3f n(normal[0], normal[1], normal[2]);
for(unsigned int i=0; i<cloud_normals->size(); ++i)
{
Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
float angle = pcl::getAngle3D(normal, v);
if(angle < angleMax)
{
output->at(oi++) = indices->size()!=0?indices->at(i):i;
}
}
output->resize(oi);
return output;
}
std::vector<pcl::IndicesPtr> extractClusters(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float clusterTolerance,
int minClusterSize,
int maxClusterSize,
int * biggestClusterIndex)
{
pcl::IndicesPtr indices(new std::vector<int>);
return extractClusters(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
}
std::vector<pcl::IndicesPtr> extractClusters(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float clusterTolerance,
int minClusterSize,
int maxClusterSize,
int * biggestClusterIndex)
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (clusterTolerance);
ec.setMinClusterSize (minClusterSize);
ec.setMaxClusterSize (maxClusterSize);
ec.setInputCloud (cloud);
if(indices->size())
{
ec.setIndices(indices);
kdTree->setInputCloud(cloud, indices);
}
else
{
kdTree->setInputCloud(cloud);
}
ec.setSearchMethod (kdTree);
std::vector<pcl::PointIndices> cluster_indices;
ec.extract (cluster_indices);
int maxIndex=-1;
unsigned int maxSize = 0;
std::vector<pcl::IndicesPtr> output(cluster_indices.size());
for(unsigned int i=0; i<cluster_indices.size(); ++i)
{
output[i] = pcl::IndicesPtr(new std::vector<int>(cluster_indices[i].indices));
if(maxSize < cluster_indices[i].indices.size())
{
maxSize = cluster_indices[i].indices.size();
maxIndex = i;
}
}
if(biggestClusterIndex)
{
*biggestClusterIndex = maxIndex;
}
return output;
}
pcl::IndicesPtr concatenate(const std::vector<pcl::IndicesPtr> & indices)
{
//compute total size
@@ -3273,19 +2881,6 @@ pcl::IndicesPtr concatenate(const pcl::IndicesPtr & indicesA, const pcl::Indices
return ind;
}
pcl::IndicesPtr extractNegativeIndices(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices)
{
pcl::IndicesPtr output(new std::vector<int>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud);
extract.setIndices(indices);
extract.setNegative(true);
extract.filter(*output);
return output;
}
}
}