pcl::IndicesPtr normalFiltering(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint)
{
typedef typename pcl::search::KdTree<PointT> KdTree;
typedef typename KdTree::Ptr KdTreePtr;
pcl::NormalEstimation<PointT, pcl::Normal> ne;
ne.setInputCloud (cloud);
if(indices->size())
{
ne.setIndices(indices);
}
KdTreePtr tree (new KdTree(false));
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;
}
pcl::IndicesPtr radiusFiltering(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius)
{
typedef typename pcl::search::KdTree<PointT> KdTree;
typedef typename KdTree::Ptr KdTreePtr;
KdTreePtr tree (new KdTree(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;
}
}
bool checkChange()
{
cv::Mat mask = cv::Mat::zeros(color.rows, color.cols, CV_8U);
cv::Mat img = cv::Mat::zeros(color.rows, color.cols, CV_32FC4);
size_t changedPixels = 0;
size_t size = indices->size();
changes.clear();
// Check pixel changes
for(size_t i = 0; i < indices->size(); ++i)
{
const int idx = indices->at(i);
cv::Vec4f v = invariantColor(color.at<cv::Vec3b>(idx), depth.at<uint16_t>(idx));
img.at<cv::Vec4f>(idx) = v;
mask.at<uint8_t>(idx) = 1;
if(checkChange(v, idx))
{
changes.push_back(idx);
++changedPixels;
}
}
// Check pixels that are masked out in current but not in last frame
for(size_t i = 0; i < lastIndices.size(); ++i)
{
const int idx = lastIndices.at(i);
if(!mask.at<uint8_t>(idx))
{
changes.push_back(idx);
++changedPixels;
++size;
}
}
lastImg = img;
lastMask = mask;
indices->swap(lastIndices);
const float diff = changedPixels / (float)size;
outInfo(changedPixels << " from " << size << " pixels changed (" << diff * 100 << "%)");
return diff > threshold;
}
std::vector<pcl::IndicesPtr> extractClusters(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float clusterTolerance,
int minClusterSize,
int maxClusterSize,
int * biggestClusterIndex)
{
typedef typename pcl::search::KdTree<PointT> KdTree;
typedef typename KdTree::Ptr KdTreePtr;
KdTreePtr tree(new KdTree);
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (clusterTolerance);
ec.setMinClusterSize (minClusterSize);
ec.setMaxClusterSize (maxClusterSize);
ec.setInputCloud (cloud);
if(indices->size())
{
ec.setIndices(indices);
tree->setInputCloud(cloud, indices);
}
else
{
tree->setInputCloud(cloud);
}
ec.setSearchMethod (tree);
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;
}
void updateIndices(pcl::IndicesPtr indices, pcl::PointIndices::Ptr inliers)
{
int toRemove = inliers->indices.size();
for(int i=0;i<toRemove;i++)
{
int origSize = indices->size();
for(int j=0;j<origSize;j++)
{
if((*indices)[j]==inliers->indices[i])
{
indices->erase(indices->begin()+j);
break;
}
}
}
}
void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
pcl::IndicesPtr & ground,
pcl::IndicesPtr & obstacles,
float normalRadiusSearch,
float groundNormalAngle,
int minClusterSize)
{
ground.reset(new std::vector<int>);
obstacles.reset(new std::vector<int>);
// Find the ground
pcl::IndicesPtr flatSurfaces = util3d::normalFiltering<PointT>(
cloud,
groundNormalAngle,
Eigen::Vector4f(0,0,1,0),
normalRadiusSearch*2.0f,
Eigen::Vector4f(0,0,100,0));
int biggestFlatSurfaceIndex;
std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = util3d::extractClusters<PointT>(
cloud,
flatSurfaces,
normalRadiusSearch*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
ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
Eigen::Vector4f min,max;
pcl::getMinMax3D<PointT>(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
{
if((int)i!=biggestFlatSurfaceIndex)
{
Eigen::Vector4f centroid;
pcl::compute3DCentroid<PointT>(*cloud, *clusteredFlatSurfaces.at(i), centroid);
if(centroid[2] >= min[2] && centroid[2] <= max[2])
{
ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
}
}
}
if(ground->size() != cloud->size())
{
// Remove ground
pcl::IndicesPtr otherStuffIndices = util3d::extractNegativeIndices<PointT>(cloud, ground);
//Cluster remaining stuff (obstacles)
std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters<PointT>(
cloud,
otherStuffIndices,
normalRadiusSearch*2.0f,
minClusterSize);
// merge indices
obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
}
}