Example #1
0
template<typename PointT> void
pcl::approximatePolygon (const PlanarPolygon<PointT>& polygon, PlanarPolygon<PointT>& approx_polygon, float threshold, bool refine, bool closed)
{
    const Eigen::Vector4f& coefficients = polygon.getCoefficients ();
    const typename pcl::PointCloud<PointT>::VectorType &contour = polygon.getContour ();

    Eigen::Vector3f rotation_axis (coefficients[1], -coefficients[0], 0.0f);
    rotation_axis.normalize ();

    float rotation_angle = acosf (coefficients [2]);
    Eigen::Affine3f transformation = Eigen::Translation3f (0, 0, coefficients [3]) * Eigen::AngleAxisf (rotation_angle, rotation_axis);

    typename pcl::PointCloud<PointT>::VectorType polygon2D (contour.size ());
    for (unsigned pIdx = 0; pIdx < polygon2D.size (); ++pIdx)
        polygon2D [pIdx].getVector3fMap () = transformation * contour [pIdx].getVector3fMap ();

    typename pcl::PointCloud<PointT>::VectorType approx_polygon2D;
    approximatePolygon2D<PointT> (polygon2D, approx_polygon2D, threshold, refine, closed);

    typename pcl::PointCloud<PointT>::VectorType &approx_contour = approx_polygon.getContour ();
    approx_contour.resize (approx_polygon2D.size ());

    Eigen::Affine3f inv_transformation = transformation.inverse ();
    for (unsigned pIdx = 0; pIdx < approx_polygon2D.size (); ++pIdx)
        approx_contour [pIdx].getVector3fMap () = inv_transformation * approx_polygon2D [pIdx].getVector3fMap ();
}
void
MultiviewRecognizerWithChangeDetection<PointT>::reconstructionFiltering(typename pcl::PointCloud<PointT>::Ptr observation,
        pcl::PointCloud<pcl::Normal>::Ptr observation_normals, const Eigen::Matrix4f &absolute_pose, size_t view_id) {
    CloudPtr observation_transformed(new Cloud);
    pcl::transformPointCloud(*observation, *observation_transformed, absolute_pose);
    CloudPtr cloud_tmp(new Cloud);
    std::vector<int> indices;
    v4r::ChangeDetector<PointT>::difference(observation_transformed, removed_points_cumulated_history_[view_id],
                                            cloud_tmp, indices, param_.tolerance_for_cloud_diff_);

    /* Visualization of changes removal for reconstruction:
    Cloud rec_changes;
    rec_changes += *cloud_transformed;
    v4r::VisualResultsStorage::copyCloudColored(*removed_points_cumulated_history_[view_id], rec_changes, 255, 0, 0);
    v4r::VisualResultsStorage::copyCloudColored(*cloud_tmp, rec_changes, 200, 0, 200);
    stringstream ss;
    ss << view_id;
    visResStore.savePcd("reconstruction_changes_" + ss.str(), rec_changes);*/

    std::vector<bool> preserved_mask(observation->size(), false);
    for (std::vector<int>::iterator i = indices.begin(); i < indices.end(); i++) {
        preserved_mask[*i] = true;
    }
    for (size_t j = 0; j < preserved_mask.size(); j++) {
        if (!preserved_mask[j]) {
            setNan(observation->at(j));
            setNan(observation_normals->at(j));
        }
    }
    PCL_INFO("Points by change detection removed: %d\n", observation->size() - indices.size());
}
Example #3
0
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
    pcl::octree::OctreePointCloudSearch<PointInT> octree (octree_leaf_size_);
    octree.setInputCloud (input_);
    octree.addPointsFromInputCloud ();

    typename pcl::PointCloud<PointInT>::VectorType occupied_cells;
    octree.getOccupiedVoxelCenters (occupied_cells);

    // Determine the voxels crosses along the line segments
    // formed by every pair of occupied cells.
    std::vector< std::vector<int> > line_histograms;
    for (size_t i = 0; i < occupied_cells.size (); ++i)
    {
        Eigen::Vector3f origin = occupied_cells[i].getVector3fMap ();

        for (size_t j = i+1; j < occupied_cells.size (); ++j)
        {
            typename pcl::PointCloud<PointInT>::VectorType intersected_cells;
            Eigen::Vector3f end = occupied_cells[j].getVector3fMap ();
            octree.getApproxIntersectedVoxelCentersBySegment (origin, end, intersected_cells, 0.5f);

            // Intersected cells are ordered from closest to furthest w.r.t. the origin.
            std::vector<int> histogram;
            for (size_t k = 0; k < intersected_cells.size (); ++k)
            {
                std::vector<int> indices;
                octree.voxelSearch (intersected_cells[k], indices);
                int label = emptyLabel ();
                if (indices.size () != 0)
                {
                    label = getDominantLabel (indices);
                }
                histogram.push_back (label);
            }

            line_histograms.push_back(histogram);
        }
    }

    std::vector< std::vector<int> > transition_histograms;
    computeTransitionHistograms (line_histograms, transition_histograms);

    std::vector<float> distances;
    computeDistancesToMean (transition_histograms, distances);

    std::vector<float> gfpfh_histogram;
    computeDistanceHistogram (distances, gfpfh_histogram);

    output.clear ();
    output.width = 1;
    output.height = 1;
    output.points.resize (1);
    std::copy (gfpfh_histogram.begin (), gfpfh_histogram.end (), output.points[0].histogram);
}
Example #4
0
template <typename PointT> inline float
pcl::getMeanPointDensity (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, float max_dist, int nr_threads)
{
  const float max_dist_sqr = max_dist * max_dist;
  const std::size_t s = cloud.size ();

  pcl::search::KdTree <PointT> tree;
  tree.setInputCloud (cloud);

  float mean_dist = 0.f;
  int num = 0;
  std::vector <int> ids (2);
  std::vector <float> dists_sqr (2);

#ifdef _OPENMP
#pragma omp parallel for \
  reduction (+:mean_dist, num) \
  private (ids, dists_sqr) shared (tree, cloud) \
  default (none)num_threads (nr_threads)
#endif

  for (int i = 0; i < 1000; i++)
  {
    tree.nearestKSearch (cloud->points[rand () % s], 2, ids, dists_sqr);
    if (dists_sqr[1] < max_dist_sqr)
    {
      mean_dist += std::sqrt (dists_sqr[1]);
      num++;
    }
  }

  return (mean_dist / num);
};
Example #5
0
  void WorldDownloadManager::cropCloudWithSphere(const Eigen::Vector3f & center,const float radius,
  typename pcl::PointCloud<PointT>::ConstPtr cloud,typename pcl::PointCloud<PointT>::Ptr out_cloud)
{
  const uint cloud_size = cloud->size();

  std::vector<bool> valid_points(cloud_size,true);

  // check the points
  for (uint i = 0; i < cloud_size; i++)
  {
    const PointT & pt = (*cloud)[i];
    const Eigen::Vector3f ept(pt.x,pt.y,pt.z);

    if ((ept - center).squaredNorm() > radius * radius)
      valid_points[i] = false;
  }

  // discard invalid points
  out_cloud->clear();
  out_cloud->reserve(cloud_size);
  uint count = 0;

  for (uint i = 0; i < cloud_size; i++)
    if (valid_points[i])
    {
      out_cloud->push_back((*cloud)[i]);
      count++;
    }
  out_cloud->resize(count);
}
Example #6
0
  void WorldDownloadManager::cropCloud(const Eigen::Vector3f & min,const Eigen::Vector3f & max,
  typename pcl::PointCloud<PointT>::ConstPtr cloud,typename pcl::PointCloud<PointT>::Ptr out_cloud)
{
  const uint cloud_size = cloud->size();

  std::vector<bool> valid_points(cloud_size,true);

  // check the points
  for (uint i = 0; i < cloud_size; i++)
  {
    const PointT & pt = (*cloud)[i];

    if (pt.x > max.x() || pt.y > max.y() || pt.z > max.z() ||
      pt.x < min.x() || pt.y < min.y() || pt.z < min.z())
      valid_points[i] = false;
  }

  // discard invalid points
  out_cloud->clear();
  out_cloud->reserve(cloud_size);
  uint count = 0;

  for (uint i = 0; i < cloud_size; i++)
    if (valid_points[i])
    {
      out_cloud->push_back((*cloud)[i]);
      count++;
    }
  out_cloud->resize(count);
}
Example #7
0
  static bool colorize(const typename pcl::PointCloud<PointTypeIn>& iCloud,
                       const Eigen::Affine3d& iCloudToCamera,
                       const bot_core::image_t& iImage,
                       const BotCamTrans* iCamTrans,
                       typename pcl::PointCloud<PointTypeOut>& oCloud) {
    pcl::copyPointCloud(iCloud, oCloud);
    pcl::PointCloud<PointTypeOut> tempCloud;
    pcl::transformPointCloud(iCloud, tempCloud, iCloudToCamera.cast<float>());

    int numPoints = iCloud.size();
    for (int i = 0; i < numPoints; ++i) {
      double p[3] = {tempCloud[i].x, tempCloud[i].y, tempCloud[i].z};
      double pix[3];
      bot_camtrans_project_point(iCamTrans, p, pix);
      oCloud[i].r = oCloud[i].g = oCloud[i].b = 0;
      if (pix[2] < 0) {
        continue;
      }

      uint8_t r, g, b;
      if (interpolate(pix[0], pix[1], iImage, r, g, b)) {
        oCloud[i].r = r;
        oCloud[i].g = g;
        oCloud[i].b = b;
      }
    }

    return true;
  }
Example #8
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;
	}
}
Example #9
0
void projectCloudOnXYPlane(
		typename pcl::PointCloud<PointT>::Ptr & cloud)
{
	for(unsigned int i=0; i<cloud->size(); ++i)
	{
		cloud->at(i).z = 0;
	}
}
Example #10
0
VectorXb boxMask(typename pcl::PointCloud<T>::ConstPtr in, float xmin, float ymin, float zmin, float xmax, float ymax, float zmax) {
  int i=0;
  VectorXb out(in->size());
  BOOST_FOREACH(const T& pt, in->points) {
    out[i] = (pt.x >= xmin && pt.x <= xmax && pt.y >= ymin && pt.y <= ymax && pt.z >= zmin && pt.z <= zmax);
    ++i;
  }
  return out;
}
Example #11
0
template <typename PointT> bool
PCLVisualizer::addCorrespondences (
   const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
   const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
   const std::vector<int> &correspondences,
   const std::string &id,
   int viewport)
{
  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
  if (am_it != shape_actor_map_->end ())
  {
    PCL_WARN ("[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
    return (false);
  }

  vtkSmartPointer<vtkAppendPolyData> polydata = vtkSmartPointer<vtkAppendPolyData>::New ();

  vtkSmartPointer<vtkUnsignedCharArray> line_colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
  line_colors->SetNumberOfComponents (3);
  line_colors->SetName ("Colors");
  // Use Red by default (can be changed later)
  unsigned char rgb[3];
  rgb[0] = 1 * 255.0;
  rgb[1] = 0 * 255.0;
  rgb[2] = 0 * 255.0;

  // Draw lines between the best corresponding points
  for (size_t i = 0; i < source_points->size (); ++i)
  {
    const PointT &p_src = source_points->points[i];
    const PointT &p_tgt = target_points->points[correspondences[i]];
    
    // Add the line
        vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
    line->SetPoint1 (p_src.x, p_src.y, p_src.z);
    line->SetPoint2 (p_tgt.x, p_tgt.y, p_tgt.z);
        line->Update ();
        polydata->AddInput (line->GetOutput ());
    line_colors->InsertNextTupleValue (rgb);
      }
      polydata->Update ();
  vtkSmartPointer<vtkPolyData> line_data = polydata->GetOutput ();
  line_data->GetCellData ()->SetScalars (line_colors);

      // Create an Actor
      vtkSmartPointer<vtkLODActor> actor;
  createActorFromVTKDataSet (line_data, actor);
  actor->GetProperty ()->SetRepresentationToWireframe ();
  actor->GetProperty ()->SetOpacity (0.5);
      addActorToRenderer (actor, viewport);

      // Save the pointer/ID pair to the global actor map
  (*shape_actor_map_)[id] = actor;
      //style_->setCloudActorMap (cloud_actor_map_);
      return (true);
    }
Example #12
0
pcl::PointCloud<pcl::PointXYZ>::Ptr toXYZ(typename pcl::PointCloud<T>::ConstPtr in) {
  pcl::PointCloud<pcl::PointXYZ>::Ptr out(new pcl::PointCloud<PointXYZ>());
  out->reserve(in->size());
  out->width = in->width;
  out->height = in->height;
  BOOST_FOREACH(const T& pt, in->points) {
    out->points.push_back(PointXYZ(pt.x, pt.y, pt.z));
  }
  return out;
}
Example #13
0
void ICCVTutorial<FeatureType>::findCorrespondences (typename pcl::PointCloud<FeatureType>::Ptr source, typename pcl::PointCloud<FeatureType>::Ptr target, std::vector<int>& correspondences) const
{
  cout << "correspondence assignment..." << std::flush;
  correspondences.resize (source->size());

  // Use a KdTree to search for the nearest matches in feature space
  pcl::KdTreeFLANN<FeatureType> descriptor_kdtree;
  descriptor_kdtree.setInputCloud (target);

  // Find the index of the best match for each keypoint, and store it in "correspondences_out"
  const int k = 1;
  std::vector<int> k_indices (k);
  std::vector<float> k_squared_distances (k);
  for (size_t i = 0; i < source->size (); ++i)
  {
    descriptor_kdtree.nearestKSearch (*source, i, k, k_indices, k_squared_distances);
    correspondences[i] = k_indices[0];
  }
  cout << "OK" << endl;
}
Example #14
0
template <typename PointT> void
pcl::SupervoxelClustering<PointT>::setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr& cloud)
{
  if ( cloud->size () == 0 )
  {
    PCL_ERROR ("[pcl::SupervoxelClustering::setInputCloud] Empty cloud set, doing nothing \n");
    return;
  }
  
  input_ = cloud;
  adjacency_octree_->setInputCloud (cloud);
}
void SurfelMapPublisher::publishPointCloud(const boost::shared_ptr<MapType>& map)
{
  if (m_pointCloudPublisher.getNumSubscribers() == 0)
    return;

  typename pcl::PointCloud<PointType>::Ptr cellPointsCloud(new pcl::PointCloud<PointType>());
  map->lock();
  map->getCellPoints(cellPointsCloud);
  map->unlock();
  cellPointsCloud->header.frame_id = map->getFrameId();
  cellPointsCloud->header.stamp = pcl_conversions::toPCL(map->getLastUpdateTimestamp());
  m_pointCloudPublisher.publish(cellPointsCloud);
  ROS_DEBUG_STREAM("publishing cell points: " << cellPointsCloud->size());
}
Example #16
0
size_t bbox_filter(typename pcl::PointCloud<P>::Ptr in,
		   typename pcl::PointCloud<P>::Ptr inliers, // inside interval
		   typename pcl::PointCloud<P>::Ptr outliers, // outside interval
		   std::string axis, double min, double max)
{
	typename pcl::PassThrough<P> pass;
	cout << "filtering " << axis << "  " << min << " - " << max << endl;
	pass.setInputCloud (in);
	pass.setFilterFieldName (axis);
	pass.setFilterLimits (min, max);

	pass.setFilterLimitsNegative(false);
	pass.filter (*inliers);

	pass.setFilterLimitsNegative(true);
	pass.filter (*outliers);

	return inliers->size();
}
Example #17
0
File: io.hpp Project: kanster/GPMap
int loadPointCloud(typename pcl::PointCloud<PointT>::Ptr		&pPointCloud, 
						 const std::string								&strFilePath, 
						 const bool											fAccumulation = false)
{
	// point cloud
	typename pcl::PointCloud<PointT>::Ptr pTempCloud(new pcl::PointCloud<PointT>());

	// load the file based on the file extension
	int result = -2;
	const std::string strFileExtension(extractFileExtension(strFilePath));
	if(strFileExtension.compare(".pcd") == 0)		result = pcl::io::loadPCDFile<PointT>(strFilePath.c_str(), *pTempCloud);
	if(strFileExtension.compare(".ply") == 0)		result = pcl::io::loadPLYFile<PointT>(strFilePath.c_str(), *pTempCloud);
	

	// error 
	switch(result)
	{
		case -2:
		{
			PCL_ERROR("Unknown file extension!");
			system("pause");
			break;
		}
		
		case -1:
		{
			PCL_ERROR("Couldn't read the file!");
			system("pause");
			break;
		}
	}

    //remove NaN Points
    pcl::removeNaNFromPointCloud(*pTempCloud, *pTempCloud, std::vector<int>());

	// accumulate
	if(fAccumulation)		*pPointCloud += *pTempCloud;
	else						pPointCloud = pTempCloud;

	return pPointCloud->size();
}
  void 
  gestureCallback (const openni_tracker_with_pointing::PointingGestureJoints& msg)
  {
    ROS_INFO("gesture rcvd!");
    publishMarkerArrayForGesture(msg);


    Eigen::Vector3f hand_pt (msg.hand.x, msg.hand.y, msg.hand.z);
    Eigen::Vector3f elbow_pt (msg.elbow.x, msg.elbow.y, msg.elbow.z);
    Eigen::Vector3f head_pt (msg.head.x, msg.head.y, msg.head.z);
    

    //double hor,ver;
    //getAngleErrors(hand_pt,elbow_pt,hand_pt,hor,ver);


    for (size_t i = 0; i < clusters_.size (); i++)
      {
	
      }
    

  }
int computeNormalsAndEigenvalues(const typename pcl::PointCloud<PointInT>::Ptr
                                 &in_cloud,
                                 const float &radius, const int &n_threads,
                                 pcl::PointCloud<PointOutT> &out_cloud)
{
    size_t n_points = in_cloud->size();
    // resize the out cloud
    out_cloud.resize(n_points);

    // build the kdtree
    pcl::KdTreeFLANN<PointInT> flann;
    flann.setInputCloud(in_cloud);

    // place for neighs ids and distances
    std::vector<int> indices;
    std::vector<float> distances;
#ifdef USE_OPENMP
#pragma omp parallel for shared(out_cloud) private(indices, distances)         \
    num_threads(n_threads)
#endif
    for (int i = 0; i < n_points; ++i) {
        // get neighbors for this point
        flann.radiusSearch((*in_cloud)[i], radius, indices, distances);

        // estimate normal and eigenvalues
        computePointNormal(*in_cloud, indices, out_cloud[i].normal_x,
                           out_cloud[i].normal_y, out_cloud[i].normal_z,
                           out_cloud[i].lam0, out_cloud[i].lam1,
                           out_cloud[i].lam2);

        flipNormal
            <PointInT>((*(in_cloud))[i], 0.0, 0.0, 0.0, out_cloud[i].normal_x,
                       out_cloud[i].normal_y, out_cloud[i].normal_z);
    }

    return 1;
}
Example #20
0
 inline std::vector<int> all_indices(const typename pcl::PointCloud<T> &cloud) {
     std::vector<int> indices;
     indices.resize(cloud.size());
     std::iota(indices.begin(), indices.end(), 0);
     return indices;
 }
Example #21
0
File: gicp.hpp Project: 87west/pcl
template<typename PointT> void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud, 
                                                                                    const typename pcl::search::KdTree<PointT>::Ptr kdtree,
                                                                                    std::vector<Eigen::Matrix3d>& cloud_covariances)
{
  if (k_correspondences_ > int (cloud->size ()))
  {
    PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", cloud->size (), k_correspondences_);
    return;
  }

  Eigen::Vector3d mean;
  std::vector<int> nn_indecies; nn_indecies.reserve (k_correspondences_);
  std::vector<float> nn_dist_sq; nn_dist_sq.reserve (k_correspondences_);

  // We should never get there but who knows
  if(cloud_covariances.size () < cloud->size ())
    cloud_covariances.resize (cloud->size ());

  typename pcl::PointCloud<PointT>::const_iterator points_iterator = cloud->begin ();
  std::vector<Eigen::Matrix3d>::iterator matrices_iterator = cloud_covariances.begin ();
  for(;
      points_iterator != cloud->end ();
      ++points_iterator, ++matrices_iterator)
  {
    const PointT &query_point = *points_iterator;
    Eigen::Matrix3d &cov = *matrices_iterator;
    // Zero out the cov and mean
    cov.setZero ();
    mean.setZero ();

    // Search for the K nearest neighbours
    kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
    
    // Find the covariance matrix
    for(int j = 0; j < k_correspondences_; j++) {
      const PointT &pt = (*cloud)[nn_indecies[j]];
      
      mean[0] += pt.x;
      mean[1] += pt.y;
      mean[2] += pt.z;
      
      cov(0,0) += pt.x*pt.x;
      
      cov(1,0) += pt.y*pt.x;
      cov(1,1) += pt.y*pt.y;
      
      cov(2,0) += pt.z*pt.x;
      cov(2,1) += pt.z*pt.y;
      cov(2,2) += pt.z*pt.z;    
    }
  
    mean /= static_cast<double> (k_correspondences_);
    // Get the actual covariance
    for (int k = 0; k < 3; k++)
      for (int l = 0; l <= k; l++) 
      {
        cov(k,l) /= static_cast<double> (k_correspondences_);
        cov(k,l) -= mean[k]*mean[l];
        cov(l,k) = cov(k,l);
      }
    
    // Compute the SVD (covariance matrix is symmetric so U = V')
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
    cov.setZero ();
    Eigen::Matrix3d U = svd.matrixU ();
    // Reconstitute the covariance matrix with modified singular values using the column     // vectors in V.
    for(int k = 0; k < 3; k++) {
      Eigen::Vector3d col = U.col(k);
      double v = 1.; // biggest 2 singular values replaced by 1
      if(k == 2)   // smallest singular value replaced by gicp_epsilon
        v = gicp_epsilon_;
      cov+= v * col * col.transpose(); 
    }
  }
}
Example #22
0
template <typename PointT> void
pcl::approximatePolygon2D (const typename pcl::PointCloud<PointT>::VectorType &polygon,
                           typename pcl::PointCloud<PointT>::VectorType &approx_polygon,
                           float threshold, bool refine, bool closed)
{
    approx_polygon.clear ();
    if (polygon.size () < 3)
        return;

    std::vector<std::pair<unsigned, unsigned> > intervals;
    std::pair<unsigned,unsigned> interval (0, 0);

    if (closed)
    {
        float max_distance = .0f;
        for (unsigned idx = 1; idx < polygon.size (); ++idx)
        {
            float distance = (polygon [0].x - polygon [idx].x) * (polygon [0].x - polygon [idx].x) +
                             (polygon [0].y - polygon [idx].y) * (polygon [0].y - polygon [idx].y);

            if (distance > max_distance)
            {
                max_distance = distance;
                interval.second = idx;
            }
        }

        for (unsigned idx = 1; idx < polygon.size (); ++idx)
        {
            float distance = (polygon [interval.second].x - polygon [idx].x) * (polygon [interval.second].x - polygon [idx].x) +
                             (polygon [interval.second].y - polygon [idx].y) * (polygon [interval.second].y - polygon [idx].y);

            if (distance > max_distance)
            {
                max_distance = distance;
                interval.first = idx;
            }
        }

        if (max_distance < threshold * threshold)
            return;

        intervals.push_back (interval);
        std::swap (interval.first, interval.second);
        intervals.push_back (interval);
    }
    else
    {
        interval.first = 0;
        interval.second = static_cast<unsigned int> (polygon.size ()) - 1;
        intervals.push_back (interval);
    }

    std::vector<unsigned> result;
    // recursively refine
    while (!intervals.empty ())
    {
        std::pair<unsigned, unsigned>& currentInterval = intervals.back ();
        float line_x = polygon [currentInterval.first].y - polygon [currentInterval.second].y;
        float line_y = polygon [currentInterval.second].x - polygon [currentInterval.first].x;
        float line_d = polygon [currentInterval.first].x * polygon [currentInterval.second].y - polygon [currentInterval.first].y * polygon [currentInterval.second].x;

        float linelen = 1.0f / sqrt (line_x * line_x + line_y * line_y);

        line_x *= linelen;
        line_y *= linelen;
        line_d *= linelen;

        float max_distance = 0.0;
        unsigned first_index = currentInterval.first + 1;
        unsigned max_index = 0;

        // => 0-crossing
        if (currentInterval.first > currentInterval.second)
        {
            for (unsigned idx = first_index; idx < polygon.size(); idx++)
            {
                float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
                if (distance > max_distance)
                {
                    max_distance = distance;
                    max_index  = idx;
                }
            }
            first_index = 0;
        }

        for (unsigned int idx = first_index; idx < currentInterval.second; idx++)
        {
            float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
            if (distance > max_distance)
            {
                max_distance = distance;
                max_index  = idx;
            }
        }

        if (max_distance > threshold)
        {
            std::pair<unsigned, unsigned> interval (max_index, currentInterval.second);
            currentInterval.second = max_index;
            intervals.push_back (interval);
        }
        else
        {
            result.push_back (currentInterval.second);
            intervals.pop_back ();
        }
    }

    approx_polygon.reserve (result.size ());
    if (refine)
    {
        std::vector<Eigen::Vector3f> lines (result.size ());
        std::reverse (result.begin (), result.end ());
        for (unsigned rIdx = 0; rIdx < result.size (); ++rIdx)
        {
            unsigned nIdx = rIdx + 1;
            if (nIdx == result.size ())
                nIdx = 0;

            Eigen::Vector2f centroid = Eigen::Vector2f::Zero ();
            Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero ();
            unsigned pIdx = result[rIdx];
            unsigned num_points = 0;
            if (pIdx > result[nIdx])
            {
                num_points = static_cast<unsigned> (polygon.size ()) - pIdx;
                for (; pIdx < polygon.size (); ++pIdx)
                {
                    covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
                    covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
                    covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
                    centroid [0] += polygon [pIdx].x;
                    centroid [1] += polygon [pIdx].y;
                }
                pIdx = 0;
            }

            num_points += result[nIdx] - pIdx;
            for (; pIdx < result[nIdx]; ++pIdx)
            {
                covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
                covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
                covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
                centroid [0] += polygon [pIdx].x;
                centroid [1] += polygon [pIdx].y;
            }

            covariance.coeffRef (2) = covariance.coeff (1);

            float norm = 1.0f / float (num_points);
            centroid *= norm;
            covariance *= norm;
            covariance.coeffRef (0) -= centroid [0] * centroid [0];
            covariance.coeffRef (1) -= centroid [0] * centroid [1];
            covariance.coeffRef (3) -= centroid [1] * centroid [1];

            float eval;
            Eigen::Vector2f normal;
            eigen22 (covariance, eval, normal);

            // select the one which is more "parallel" to the original line
            Eigen::Vector2f direction;
            direction [0] = polygon[result[nIdx]].x - polygon[result[rIdx]].x;
            direction [1] = polygon[result[nIdx]].y - polygon[result[rIdx]].y;
            direction.normalize ();

            if (fabs (direction.dot (normal)) > float(M_SQRT1_2))
            {
                std::swap (normal [0], normal [1]);
                normal [0] = -normal [0];
            }

            // needs to be on the left side of the edge
            if (direction [0] * normal [1] < direction [1] * normal [0])
                normal *= -1.0;

            lines [rIdx].head<2> () = normal;
            lines [rIdx] [2] = -normal.dot (centroid);
        }

        float threshold2 = threshold * threshold;
        for (unsigned rIdx = 0; rIdx < lines.size (); ++rIdx)
        {
            unsigned nIdx = rIdx + 1;
            if (nIdx == result.size ())
                nIdx = 0;

            Eigen::Vector3f vertex = lines [rIdx].cross (lines [nIdx]);
            vertex /= vertex [2];
            vertex [2] = 0.0;

            PointT point;
            // test whether we need another edge since the intersection point is too far away from the original vertex
            Eigen::Vector3f pq = polygon [result[nIdx]].getVector3fMap () - vertex;
            pq [2] = 0.0;

            float distance = pq.squaredNorm ();
            if (distance > threshold2)
            {
                // test whether the old point is inside the new polygon or outside
                if ((pq [0] * lines [rIdx] [0] + pq [1] * lines [rIdx] [1] < 0.0) &&
                        (pq [0] * lines [nIdx] [0] + pq [1] * lines [nIdx] [1] < 0.0) )
                {
                    float distance1 = lines [rIdx] [0] * polygon[result[nIdx]].x + lines [rIdx] [1] * polygon[result[nIdx]].y + lines [rIdx] [2];
                    float distance2 = lines [nIdx] [0] * polygon[result[nIdx]].x + lines [nIdx] [1] * polygon[result[nIdx]].y + lines [nIdx] [2];

                    point.x = polygon[result[nIdx]].x - distance1 * lines [rIdx] [0];
                    point.y = polygon[result[nIdx]].y - distance1 * lines [rIdx] [1];

                    approx_polygon.push_back (point);

                    vertex [0] = polygon[result[nIdx]].x - distance2 * lines [nIdx] [0];
                    vertex [1] = polygon[result[nIdx]].y - distance2 * lines [nIdx] [1];
                }
            }
            point.getVector3fMap () = vertex;
            approx_polygon.push_back (point);
        }
    }
    else
    {
        // we have a new polygon in results, but inverted (clockwise <-> counter-clockwise)
        for (std::vector<unsigned>::reverse_iterator it = result.rbegin (); it != result.rend (); ++it)
            approx_polygon.push_back (polygon [*it]);
    }
}
Example #23
0
void occupancy2DFromCloud3D(
		const typename pcl::PointCloud<PointT>::Ptr & cloud,
		const pcl::IndicesPtr & indices,
		cv::Mat & ground,
		cv::Mat & obstacles,
		float cellSize,
		float groundNormalAngle,
		int minClusterSize)
{
	if(cloud->size() == 0)
	{
		return;
	}
	pcl::IndicesPtr groundIndices, obstaclesIndices;

	segmentObstaclesFromGround<PointT>(
			cloud,
			indices,
			groundIndices,
			obstaclesIndices,
			cellSize,
			groundNormalAngle,
			minClusterSize);

	pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);

	if(groundIndices->size())
	{
		pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
		//project on XY plane
		util3d::projectCloudOnXYPlane(groundCloud);
		//voxelize to grid cell size
		groundCloud = util3d::voxelize(groundCloud, cellSize);
	}

	if(obstaclesIndices->size())
	{
		pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
		//project on XY plane
		util3d::projectCloudOnXYPlane(obstaclesCloud);
		//voxelize to grid cell size
		obstaclesCloud = util3d::voxelize(obstaclesCloud, cellSize);
	}

	ground = cv::Mat();
	if(groundCloud->size())
	{
		ground = cv::Mat((int)groundCloud->size(), 1, CV_32FC2);
		for(unsigned int i=0;i<groundCloud->size(); ++i)
		{
			ground.at<cv::Vec2f>(i)[0] = groundCloud->at(i).x;
			ground.at<cv::Vec2f>(i)[1] = groundCloud->at(i).y;
		}
	}

	obstacles = cv::Mat();
	if(obstaclesCloud->size())
	{
		obstacles = cv::Mat((int)obstaclesCloud->size(), 1, CV_32FC2);
		for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
		{
			obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
			obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y;
		}
	}
}
Example #24
0
void WorldDownloadManager::cropMesh(const kinfu_msgs::KinfuCloudPoint & min,
  const kinfu_msgs::KinfuCloudPoint & max,typename pcl::PointCloud<PointT>::ConstPtr cloud,
  TrianglesConstPtr triangles,typename pcl::PointCloud<PointT>::Ptr out_cloud, TrianglesPtr out_triangles)
{
  const uint triangles_size = triangles->size();
  const uint cloud_size = cloud->size();

  std::vector<bool> valid_points(cloud_size,true);

  std::vector<uint> valid_points_remap(cloud_size,0);

  uint offset;

  // check the points
  for (uint i = 0; i < cloud_size; i++)
  {
    const PointT & pt = (*cloud)[i];

    if (pt.x > max.x || pt.y > max.y || pt.z > max.z ||
      pt.x < min.x || pt.y < min.y || pt.z < min.z)
      valid_points[i] = false;
  }

  // discard invalid points
  out_cloud->clear();
  out_cloud->reserve(cloud_size);
  offset = 0;

  for (uint i = 0; i < cloud_size; i++)
    if (valid_points[i])
    {
      out_cloud->push_back((*cloud)[i]);

      // save new position for triangles remap
      valid_points_remap[i] = offset;

      offset++;
    }
  out_cloud->resize(offset);

  // discard invalid triangles
  out_triangles->clear();
  out_triangles->reserve(triangles_size);
  offset = 0;

  for (uint i = 0; i < triangles_size; i++)
  {
    const kinfu_msgs::KinfuMeshTriangle & tri = (*triangles)[i];
    bool is_valid = true;

    // validate all the vertices
    for (uint h = 0; h < 3; h++)
      if (!valid_points[tri.vertex_id[h]])
      {
        is_valid = false;
        break;
      }

    if (is_valid)
    {
      kinfu_msgs::KinfuMeshTriangle out_tri;

      // remap the triangle
      for (uint h = 0; h < 3; h++)
        out_tri.vertex_id[h] = valid_points_remap[(*triangles)[i].vertex_id[h]];

      out_triangles->push_back(out_tri);
      offset++;
    }

  }
  out_triangles->resize(offset);
}
Example #25
0
  void WorldDownloadManager::removeDuplicatePoints(typename pcl::PointCloud<PointT>::ConstPtr cloud,
  TrianglesConstPtr triangles,typename pcl::PointCloud<PointT>::Ptr out_cloud,TrianglesPtr out_triangles)
{
  const uint64 input_size = cloud->size();
  std::vector<uint64> ordered(input_size);
  for (uint64 i = 0; i < input_size; i++)
    ordered[i] = i;

  std::sort(ordered.begin(),ordered.end(),WorldDownloadManager_mergeMeshTrianglesComparator<PointT>(*cloud));

  typedef WorldDownloadManager_mergeMeshTrianglesAverageClass<PointT> AverageClass;
  std::vector<uint64> re_association_vector(input_size);
  std::vector<AverageClass, Eigen::aligned_allocator<AverageClass> > average_vector;

  uint64 output_i = 0;
  re_association_vector[ordered[0]] = 0;

  for (uint64 src_i = 1; src_i < input_size; src_i++)
  {
    const PointT & prev = (*cloud)[ordered[src_i - 1]];
    const PointT & current = (*cloud)[ordered[src_i]];
    const int not_equal = std::memcmp(prev.data,current.data,sizeof(float) * 3);
    if (not_equal)
      output_i++;
    re_association_vector[ordered[src_i]] = output_i;
  }
  const uint64 output_size = output_i + 1;

  out_cloud->resize(output_size);
  average_vector.resize(output_size);
  for (uint64 i = 0; i < input_size; i++)
    average_vector[re_association_vector[i]].insert((*cloud)[i]);
  for (uint64 i = 0; i < output_size; i++)
    (*out_cloud)[i] = average_vector[i].get();

  if (out_triangles && triangles)
  {
    const uint64 triangles_size = triangles->size();
    uint64 out_triangles_size = 0;
    out_triangles->resize(triangles_size);
    for (uint64 i = 0; i < triangles_size; i++)
    {
      kinfu_msgs::KinfuMeshTriangle new_tri;
      for (uint h = 0; h < 3; h++)
        new_tri.vertex_id[h] = re_association_vector[(*triangles)[i].vertex_id[h]];

      bool degenerate = false;
      for (uint h = 0; h < 3; h++)
        if (new_tri.vertex_id[h] == new_tri.vertex_id[(h + 1) % 3])
          degenerate = true;

      if (!degenerate)
      {
        (*out_triangles)[out_triangles_size] = new_tri;
        out_triangles_size++;
      }
    }

    out_triangles->resize(out_triangles_size);
  }
}
Example #26
0
void MapPublisher::publishMap(const boost::shared_ptr<MapType>& map)
{
  if (pub_map_msg_.getNumSubscribers() == 0)
    return;
  pcl::StopWatch timer;
  timer.reset();

  mrs_laser_mapping::MultiResolutionMapPtr map_msg(new mrs_laser_mapping::MultiResolutionMap);
  map_msg->header.stamp = map->getLastUpdateTimestamp();
  map_msg->header.frame_id = map->getFrameId();
  map_msg->levels = map->getLevels();
  map_msg->size_in_meters = map->getSizeInMeters();
  map_msg->resolution = map->getResolution();
  map_msg->cell_capacity = map->getCellCapacity();

  unsigned int nr_points = 0;
  for (unsigned int i = 0; i < map->getLevels(); i++)
  {
    typename pcl::PointCloud<PointType>::Ptr points(new pcl::PointCloud<PointType>());
    map->getCellPoints(points, i);

    sensor_msgs::PointCloud2 ros_cloud;

    pcl::toROSMsg(*points, ros_cloud);

    map_msg->cloud.push_back(ros_cloud);
    nr_points += points->size();
  }

  /*
   * get all free cells
   */
  std::vector<float> cell_sizes;
  std::vector<std::vector<Eigen::Vector3f>> occupied_cell_offsets;
  typename MapType::CellPointerVectorVector occupied_grid_cells;
  int levels = map->getLevels();

  for (int l = 0; l < levels; l++)
  {
    cell_sizes.push_back(map->getCellSize(l));

    std::vector<Eigen::Vector3f> occupied_cells_level;
    typename MapType::CellPointerVector occupied_grid_cells_level;

    map->getCellsWithOffset(occupied_grid_cells_level, occupied_cells_level, l, -99999.f);  // TODO: this sucks...

    occupied_cell_offsets.push_back(occupied_cells_level);
    occupied_grid_cells.push_back(occupied_grid_cells_level);
  }

  for (int l = cell_sizes.size() - 1; l >= 0; l--)
  {
    for (size_t i = 0; i < occupied_cell_offsets[l].size(); i++)
    {
      if ((*occupied_grid_cells[l][i]).getOccupancy() <= 0.f)
      {  // thos
         // 				if ( occupiedGridCells[l][i]->m_debugState == mrs_laser_maps::GridCellWithStatistics::FREE ) {
        geometry_msgs::Point p;
        p.x = occupied_cell_offsets[l][i](0);
        p.y = occupied_cell_offsets[l][i](1);
        p.z = occupied_cell_offsets[l][i](2);

        map_msg->free_cell_centers.push_back(p);
      }
    }
  }

  pub_map_msg_.publish(map_msg);
  ROS_DEBUG_STREAM_NAMED("map_publisher","published map message took: " << timer.getTime() << " ms with " << nr_points
                                                 << " points. from timestamp " << map_msg->header.stamp << " at time "
                                                 << ros::Time::now());
}
    void
    process ()
    {
      std::cout << "threshold: " << threshold_ << std::endl;
      std::cout << "depth dependent: " << (depth_dependent_ ? "true\n" : "false\n");
      unsigned char red [6] = {255,   0,   0, 255, 255,   0};
      unsigned char grn [6] = {  0, 255,   0, 255,   0, 255};
      unsigned char blu [6] = {  0,   0, 255,   0, 255, 255};

      pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
      ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
      ne.setMaxDepthChangeFactor (0.02f);
      ne.setNormalSmoothingSize (20.0f);
      
      typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr refinement_compare (new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ());
      refinement_compare->setDistanceThreshold (threshold_, depth_dependent_);
      
      pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
      mps.setMinInliers (5000);
      mps.setAngularThreshold (0.017453 * 3.0); //3 degrees
      mps.setDistanceThreshold (0.03); //2cm
      mps.setRefinementComparator (refinement_compare);
      
      std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
      typename pcl::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>);
      typename pcl::PointCloud<PointT>::Ptr approx_contour (new pcl::PointCloud<PointT>);
      char name[1024];

      typename pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
      double normal_start = pcl::getTime ();
      ne.setInputCloud (cloud);
      ne.compute (*normal_cloud);
      double normal_end = pcl::getTime ();
      std::cout << "Normal Estimation took " << double (normal_end - normal_start) << std::endl;

      double plane_extract_start = pcl::getTime ();
      mps.setInputNormals (normal_cloud);
      mps.setInputCloud (cloud);
      if (refine_)
        mps.segmentAndRefine (regions);
      else
        mps.segment (regions);
      double plane_extract_end = pcl::getTime ();
      std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << " with planar regions found: " << regions.size () << std::endl;
      std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl;

      typename pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>);

      viewer.removeAllPointClouds (0);
      viewer.removeAllShapes (0);
      pcl::visualization::PointCloudColorHandlerCustom<PointT> single_color (cloud, 0, 255, 0);
      viewer.addPointCloud<PointT> (cloud, single_color, "cloud");
      
      pcl::PlanarPolygon<PointT> approx_polygon;
      //Draw Visualization
      for (size_t i = 0; i < regions.size (); i++)
      {
        Eigen::Vector3f centroid = regions[i].getCentroid ();
        Eigen::Vector4f model = regions[i].getCoefficients ();
        pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]);
        pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * model[0]),
                                           centroid[1] + (0.5f * model[1]),
                                           centroid[2] + (0.5f * model[2]));
        sprintf (name, "normal_%d", unsigned (i));
        viewer.addArrow (pt2, pt1, 1.0, 0, 0, std::string (name));

        contour->points = regions[i].getContour ();        
        sprintf (name, "plane_%02d", int (i));
        pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i], grn[i], blu[i]);
        viewer.addPointCloud (contour, color, name);

        pcl::approximatePolygon (regions[i], approx_polygon, threshold_, polygon_refinement_);
        approx_contour->points = approx_polygon.getContour ();
        std::cout << "polygon: " << contour->size () << " -> " << approx_contour->size () << std::endl;
        typename pcl::PointCloud<PointT>::ConstPtr approx_contour_const = approx_contour;
        
//        sprintf (name, "approx_plane_%02d", int (i));
//        viewer.addPolygon<PointT> (approx_contour_const, 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name);
        for (unsigned idx = 0; idx < approx_contour->points.size (); ++idx)
        {
          sprintf (name, "approx_plane_%02d_%03d", int (i), int(idx));
          viewer.addLine (approx_contour->points [idx], approx_contour->points [(idx+1)%approx_contour->points.size ()], 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name);
        }
      }
    }
Example #28
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);
	}
}
    bool
    segmentCloud (pointing_object_evaluation::ObjectSegmentationRequest::Request &req, pointing_object_evaluation::ObjectSegmentationRequest::Response &res)
    {
      CloudPtr cloud (new Cloud ());
      {
        //boost::lock_guard<boost::mutex> lock (cloud_mutex_);
        *cloud = *cloud_;
      }
      
      unsigned char red [6] = {255,   0,   0, 255, 255,   0};
      unsigned char grn [6] = {  0, 255,   0, 255,   0, 255};
      unsigned char blu [6] = {  0,   0, 255,   0, 255, 255};
      pcl::PointCloud<pcl::PointXYZRGB> aggregate_cloud;

      // Estimate Normals
      double ne_start = pcl::getTime ();
      pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
      ne_.setInputCloud (cloud);
      ne_.compute (*normal_cloud);
      double ne_end = pcl::getTime ();
      //std::cout << "NE took: " << double(ne_end - ne_start) << std::endl;
      
      // Segment Planes
      double mps_start = pcl::getTime ();
      std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
      std::vector<pcl::ModelCoefficients> model_coefficients;
      std::vector<pcl::PointIndices> inlier_indices;  
      pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
      std::vector<pcl::PointIndices> label_indices;
      std::vector<pcl::PointIndices> boundary_indices;
      mps_.setInputNormals (normal_cloud);
      mps_.setInputCloud (cloud);
      //mps.segment (regions);
      mps_.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);      
      
      //Segment Objects
      typename pcl::PointCloud<PointT>::CloudVectorType clusters;

      if (regions.size () > 0)
      {
        //printf ("got some planes!\n");
        std::vector<bool> plane_labels;
        plane_labels.resize (label_indices.size (), false);
        for (size_t i = 0; i < label_indices.size (); i++)
        {
          if (label_indices[i].indices.size () > 10000)
          {
            plane_labels[i] = true;
          }
        }  
        //printf ("made label vec\n");
        
        typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr euclidean_cluster_comparator_ (new typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>());
        euclidean_cluster_comparator_->setInputCloud (cloud);
        euclidean_cluster_comparator_->setLabels (labels);
        euclidean_cluster_comparator_->setExcludeLabels (plane_labels);
        euclidean_cluster_comparator_->setDistanceThreshold (0.01f, false);
        

        pcl::PointCloud<pcl::Label> euclidean_labels;
        std::vector<pcl::PointIndices> euclidean_label_indices;
        pcl::OrganizedConnectedComponentSegmentation<PointT,pcl::Label> euclidean_segmentation (euclidean_cluster_comparator_);
        euclidean_segmentation.setInputCloud (cloud);
        euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
        
        //printf ("done segmenting the clusters\n");
        
        std::vector<Eigen::Vector4f> centroids;
        
        for (size_t i = 0; i < euclidean_label_indices.size (); i++)
        {
          if (euclidean_label_indices[i].indices.size () > 1000)
          {
            pcl::PointCloud<PointT> cluster;
            pcl::copyPointCloud (*cloud,euclidean_label_indices[i].indices,cluster);	    
            clusters.push_back (cluster);
            
	    /*Eigen::Vector4f centroid;
            pcl::compute3DCentroid (cluster, centroid);
	    centroids.push_back (centroid);*/
            
	    //pcl::PointXYZ centroid_pt (centroid[0], centroid[1], centroid[2]);
            
            
            //double dist = sqrt (centroid[0]*centroid[0] + centroid[1]*centroid[1] + centroid[2]*centroid[2]);
            //object_points_.push_back (centroid_pt);

	    
            // Send to RViz
            pcl::PointCloud<pcl::PointXYZRGB> color_cluster;
            pcl::copyPointCloud (cluster, color_cluster);
            for (size_t j = 0; j < color_cluster.size (); j++)
            {
              color_cluster.points[j].r = (color_cluster.points[j].r + red[i%6]) / 2;
              color_cluster.points[j].g = (color_cluster.points[j].g + grn[i%6]) / 2;
              color_cluster.points[j].b = (color_cluster.points[j].b + blu[i%6]) / 2;
            }
            aggregate_cloud += color_cluster;
	    
          }    
        }
        
        PCL_INFO ("Got %d euclidean clusters!\n", clusters.size ());
	//PCL_INFO ("Got %d centroids!\n", centroids.size ());

        aggregate_cloud.header = cloud->header;
        sensor_msgs::PointCloud2 cloud_msg;
        pcl::toROSMsg (aggregate_cloud, cloud_msg);
        object_cloud_pub_.publish (cloud_msg);

        // TODO: mutex
        
	/*clusters_ = clusters;
	  centroids_= centroids;*/
      }
      
    }
  void OctreeVoxelGrid::generateVoxelCloudImpl(const sensor_msgs::PointCloud2ConstPtr& input_msg)
  {

    typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
    typename pcl::PointCloud<PointT>::Ptr cloud_voxeled (new pcl::PointCloud<PointT> ());
    pcl::fromROSMsg(*input_msg, *cloud);

    // generate octree
    typename pcl::octree::OctreePointCloud<PointT> octree(resolution_);
    // add point cloud to octree
    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud();
    // get points where grid is occupied
    typename pcl::octree::OctreePointCloud<PointT>::AlignedPointTVector point_vec;
    octree.getOccupiedVoxelCenters(point_vec);
    // put points into point cloud
    cloud_voxeled->width = point_vec.size();
    cloud_voxeled->height = 1;
    for (int i = 0; i < point_vec.size(); i++) {
      cloud_voxeled->push_back(point_vec[i]);
    }

    // publish point cloud
    sensor_msgs::PointCloud2 output_msg;
    toROSMsg(*cloud_voxeled, output_msg);
    output_msg.header = input_msg->header;
    pub_cloud_.publish(output_msg);

    // publish marker
    if (publish_marker_flag_) {
      visualization_msgs::Marker marker_msg;
      marker_msg.type = visualization_msgs::Marker::CUBE_LIST;
      marker_msg.scale.x = resolution_;
      marker_msg.scale.y = resolution_;
      marker_msg.scale.z = resolution_;
      marker_msg.header = input_msg->header;
      marker_msg.pose.orientation.w = 1.0;
      if (marker_color_ == "flat") {
        marker_msg.color = jsk_topic_tools::colorCategory20(0);
        marker_msg.color.a = marker_color_alpha_;
      }
      
      // compute min and max
      Eigen::Vector4f minpt, maxpt;
      pcl::getMinMax3D<PointT>(*cloud_voxeled, minpt, maxpt);
      PointT p;
      for (size_t i = 0; i < cloud_voxeled->size(); i++) {
        p = cloud_voxeled->at(i);
        geometry_msgs::Point point_ros;
        point_ros.x = p.x;
        point_ros.y = p.y;
        point_ros.z = p.z;
        marker_msg.points.push_back(point_ros);
        if (marker_color_ == "flat") {
          marker_msg.colors.push_back(jsk_topic_tools::colorCategory20(0));
        }
        else if (marker_color_ == "z") {
          marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.z - minpt[2]) / (maxpt[2] - minpt[2])));
        }
        else if (marker_color_ == "x") {
          marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.x - minpt[0]) / (maxpt[0] - minpt[0])));
        }
        else if (marker_color_ == "y") {
          marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.y - minpt[1]) / (maxpt[1] - minpt[1])));
        }
        marker_msg.colors[marker_msg.colors.size() - 1].a = marker_color_alpha_;
      }
      pub_marker_.publish(marker_msg);
    }
  }