Exemple #1
0
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;
}
Exemple #2
0
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;
  }
Exemple #4
0
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;
      }
    }
  }
}
Exemple #6
0
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);
	}
}