void VoxelGrid::inputCloud(typename pcl::PointCloud<PointT>::ConstPtr cloud)
{
    double res = readParameter<double>("resolution");
    Eigen::Vector4f leaf(res, res, res, 0);

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

    typename pcl::PointCloud<PointT>::ConstPtr in;

    if (remove_nan_) {
        typename pcl::PointCloud<PointT>::Ptr tmp(new pcl::PointCloud<PointT>);
        tmp->points.reserve(cloud->points.size());

        for (auto it = cloud->begin(); it != cloud->end(); ++it) {
            if (!std::isnan(it->x)) {
                tmp->points.push_back(*it);
            }
        }
        in = tmp;

    } else {
        in = cloud;
    }

    pcl::VoxelGrid<PointT> voxel_f;
    voxel_f.setInputCloud(in);
    voxel_f.setLeafSize(leaf);
    voxel_f.filter(*out);

    PointCloudMessage::Ptr msg(new PointCloudMessage(cloud->header.frame_id, cloud->header.stamp));
    out->header = cloud->header;
    msg->value = out;

    msg::publish(output_, msg);
}
Ejemplo n.º 2
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);
}
Ejemplo n.º 3
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);
}
Ejemplo n.º 4
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());
}
Ejemplo n.º 6
0
void projectCloudOnXYPlane(
		typename pcl::PointCloud<PointT>::Ptr & cloud)
{
	for(unsigned int i=0; i<cloud->size(); ++i)
	{
		cloud->at(i).z = 0;
	}
}
Ejemplo n.º 7
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);
}
Ejemplo n.º 8
0
inline void trim_Z(boost::shared_ptr<pcl::PointCloud<PointT>> &cloud, double zrange_min, double zrange_max) {
    typename pcl::PointCloud<PointT>::Ptr cloud_filtered_pass (new pcl::PointCloud<PointT>(512, 424));
    pcl::PassThrough<pcl::PointXYZRGB> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(-zrange_max, -zrange_min);
    pass.filter (*cloud_filtered_pass);
    cloud_filtered_pass.swap (cloud);

}
Ejemplo n.º 9
0
inline void rotate_Z(boost::shared_ptr<pcl::PointCloud<PointT>> &cloud, double angle) {

    Eigen::Affine3f transform_Z = Eigen::Affine3f::Identity();
    // The same rotation matrix as before; tetha radians arround Z axis
    transform_Z.rotate (Eigen::AngleAxisf (angle, Eigen::Vector3f::UnitZ()));

    // Executing the transformation
    typename pcl::PointCloud<PointT>::Ptr transformed_cloud (new pcl::PointCloud<PointT>(512, 424));
    pcl::transformPointCloud (*cloud, *transformed_cloud, transform_Z);
    transformed_cloud.swap (cloud);
}
Ejemplo n.º 10
0
template <typename PointT> void
pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getNormals (typename pcl::PointCloud<Normal>::Ptr &normals) const
{
  normals.reset (new pcl::PointCloud<Normal>);
  normals->clear ();
  normals->resize (leaves_.size ());
  typename SupervoxelHelper::const_iterator leaf_itr;
  typename pcl::PointCloud<Normal>::iterator normal_itr = normals->begin ();
  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++normal_itr)
  {
    const VoxelData& leaf_data = (*leaf_itr)->getData ();
    leaf_data.getNormal (*normal_itr);
  }
}
Ejemplo n.º 11
0
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());
}
Ejemplo n.º 12
0
template <typename PointT> void
pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getNormals (typename pcl::PointCloud<Normal>::Ptr &normals) const
{
  normals = boost::make_shared<pcl::PointCloud<Normal> > ();
  normals->clear ();
  normals->resize (leaves_.size ());
  typename std::set<LeafContainerT*>::iterator leaf_itr;
  typename pcl::PointCloud<Normal>::iterator normal_itr = normals->begin ();
  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++normal_itr)
  {
    const VoxelData& leaf_data = (*leaf_itr)->getData ();
    leaf_data.getNormal (*normal_itr);
  }
}
Ejemplo n.º 13
0
template <typename PointT> void
pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getVoxels (typename pcl::PointCloud<PointT>::Ptr &voxels) const
{
  voxels = boost::make_shared<pcl::PointCloud<PointT> > ();
  voxels->clear ();
  voxels->resize (leaves_.size ());
  typename pcl::PointCloud<PointT>::iterator voxel_itr = voxels->begin ();
  //typename std::set<LeafContainerT*>::iterator leaf_itr;
  for (typename std::set<LeafContainerT*>::const_iterator leaf_itr = leaves_.begin (); 
	  leaf_itr != leaves_.end (); 
	  ++leaf_itr, ++voxel_itr)
  {
    const VoxelData& leaf_data = (*leaf_itr)->getData ();
    leaf_data.getPoint (*voxel_itr);
  }
}
Ejemplo n.º 14
0
template <typename T> bool
pcl::visualization::ImageViewer::addMask (
    const typename pcl::PointCloud<T>::ConstPtr &image,
    const pcl::PointCloud<T> &mask, 
    double r, double g, double b,
    const std::string &layer_id, double opacity)
{
  // We assume that the data passed into image is organized, otherwise this doesn't make sense
  if (!image->isOrganized ())
    return (false);

  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
  if (am_it == layer_map_.end ())
  {
    PCL_DEBUG ("[pcl::visualization::ImageViewer::addMask] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
    am_it = createLayer (layer_id, image_viewer_->GetRenderWindow ()->GetSize ()[0] - 1, image_viewer_->GetRenderWindow ()->GetSize ()[1] - 1, opacity, true);
  }

  am_it->canvas->SetDrawColor (r * 255.0, g * 255.0, b * 255.0, opacity * 255.0);

  // Construct a search object to get the camera parameters
  pcl::search::OrganizedNeighbor<T> search;
  search.setInputCloud (image);
  for (size_t i = 0; i < mask.points.size (); ++i)
  {
    pcl::PointXY p_projected;
    search.projectPoint (mask.points[i], p_projected);

    am_it->canvas->DrawPoint (int (p_projected.x), 
                              int (float (image->height) - p_projected.y));
  }

  return (true);
}
Ejemplo n.º 15
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;
  }
Ejemplo n.º 16
0
inline void clipNSEW(boost::shared_ptr<pcl::PointCloud<PointT>> &cloud, double clip_N, double clip_S, double  clip_E, double clip_W) {

    typename pcl::PointCloud<PointT>::Ptr cloud_filtered_pass (new pcl::PointCloud<PointT>(512, 424));
    pcl::PassThrough<PointT> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("x");
    pass.setFilterLimits(-clip_W, clip_E);
    pass.filter (*cloud_filtered_pass);
    cloud_filtered_pass.swap (cloud);

    pass.setInputCloud(cloud);
    pass.setFilterFieldName("y");
    pass.setFilterLimits(-clip_S, clip_N);
    pass.filter (*cloud_filtered_pass);
    cloud_filtered_pass.swap (cloud);
}
Ejemplo n.º 17
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);
};
Ejemplo n.º 18
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;
	}
}
Ejemplo n.º 19
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;
}
Ejemplo n.º 20
0
inline void trimNSEW(boost::shared_ptr<pcl::PointCloud<PointT>> &cloud, double trim_N, double trim_S, double  trim_E, double trim_W) {

    struct bound_box bbox;
    typename pcl::PointCloud<PointT>::Ptr cloud_filtered_pass (new pcl::PointCloud<PointT>(512, 424));
    getMinMax(*cloud, bbox);
    pcl::PassThrough<PointT> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("x");
    pass.setFilterLimits(bbox.W + trim_W, bbox.E - trim_E);
    pass.filter (*cloud_filtered_pass);
    cloud_filtered_pass.swap (cloud);

    pass.setInputCloud(cloud);
    pass.setFilterFieldName("y");
    pass.setFilterLimits(bbox.S + trim_S, bbox.N - trim_N);
    pass.filter (*cloud_filtered_pass);
    cloud_filtered_pass.swap (cloud);
}
Ejemplo n.º 21
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);
    }
 PcdGtAnnotator()
 {
     f_ = 525.f;
     source_.reset(new v4r::ModelOnlySource<pcl::PointXYZRGBNormal, pcl::PointXYZRGB>);
     models_dir_ = "";
     gt_dir_ = "";
     threshold_ = 0.01f;
     first_view_only_ = false;
     reconstructed_scene_.reset(new pcl::PointCloud<PointT>);
 }
Ejemplo n.º 23
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;
}
Ejemplo n.º 24
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;
}
Ejemplo n.º 25
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);
}
Ejemplo n.º 26
0
template <typename T> bool
pcl::visualization::ImageViewer::addRectangle (
    const typename pcl::PointCloud<T>::ConstPtr &image,
    const pcl::PointCloud<T> &mask, 
    double r, double g, double b,
    const std::string &layer_id, double opacity)
{
  // We assume that the data passed into image is organized, otherwise this doesn't make sense
  if (!image->isOrganized ())
    return (false);

  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
  if (am_it == layer_map_.end ())
  {
    PCL_DEBUG ("[pcl::visualization::ImageViewer::addRectangle] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
    am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, true);
  }

  // Construct a search object to get the camera parameters
  pcl::search::OrganizedNeighbor<T> search;
  search.setInputCloud (image);
  std::vector<pcl::PointXY> pp_2d (mask.points.size ());
  for (size_t i = 0; i < mask.points.size (); ++i)
    search.projectPoint (mask.points[i], pp_2d[i]);

  pcl::PointXY min_pt_2d, max_pt_2d;
  min_pt_2d.x = min_pt_2d.y = std::numeric_limits<float>::max ();
  max_pt_2d.x = max_pt_2d.y = std::numeric_limits<float>::min ();
  // Search for the two extrema
  for (size_t i = 0; i < pp_2d.size (); ++i)
  {
    if (pp_2d[i].x < min_pt_2d.x) min_pt_2d.x = pp_2d[i].x;
    if (pp_2d[i].y < min_pt_2d.y) min_pt_2d.y = pp_2d[i].y;
    if (pp_2d[i].x > max_pt_2d.x) max_pt_2d.x = pp_2d[i].x;
    if (pp_2d[i].y > max_pt_2d.y) max_pt_2d.y = pp_2d[i].y;
  }
#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION < 10))
  min_pt_2d.y = float (image->height) - min_pt_2d.y;
  max_pt_2d.y = float (image->height) - max_pt_2d.y;
#endif

  vtkSmartPointer<context_items::Rectangle> rect = vtkSmartPointer<context_items::Rectangle>::New ();
  rect->setColors (static_cast<unsigned char> (255.0 * r), 
                   static_cast<unsigned char> (255.0 * g), 
                   static_cast<unsigned char> (255.0 * b));
  rect->setOpacity (opacity);
  rect->set (min_pt_2d.x, min_pt_2d.y, (max_pt_2d.x - min_pt_2d.x), (max_pt_2d.y - min_pt_2d.y));
  am_it->actor->GetScene ()->AddItem (rect);

  return (true);
}
Ejemplo n.º 27
0
template <typename T> bool
pcl::visualization::ImageViewer::addPlanarPolygon (
    const typename pcl::PointCloud<T>::ConstPtr &image,
    const pcl::PlanarPolygon<T> &polygon, 
    double r, double g, double b,
    const std::string &layer_id, double opacity)
{
  // We assume that the data passed into image is organized, otherwise this doesn't make sense
  if (!image->isOrganized ())
    return (false);

  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
  if (am_it == layer_map_.end ())
  {
    PCL_DEBUG ("[pcl::visualization::ImageViewer::addPlanarPolygon] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
    am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, true);
  }
  
  // Construct a search object to get the camera parameters and fill points
  pcl::search::OrganizedNeighbor<T> search;
  search.setInputCloud (image);
  const float image_height_f = static_cast<float> (image->height);
  std::vector<float> xy;
  xy.reserve ((polygon.getContour ().size () + 1) * 2);
  for (size_t i = 0; i < polygon.getContour ().size (); ++i)
  {
    pcl::PointXY p;
    search.projectPoint (polygon.getContour ()[i], p);
    xy.push_back (p.x);
    #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION < 10))
    xy.push_back (image_height_f - p.y);
    #else
    xy.push_back (p.y);
    #endif
  }

  // Close the polygon
  xy[xy.size () - 2] = xy[0];
  xy[xy.size () - 1] = xy[1];

  vtkSmartPointer<context_items::Polygon> poly = vtkSmartPointer<context_items::Polygon>::New ();
  poly->setColors (static_cast<unsigned char> (r * 255.0), 
                   static_cast<unsigned char> (g * 255.0), 
                   static_cast<unsigned char> (b * 255.0));
  poly->set (xy);
  am_it->actor->GetScene ()->AddItem (poly);

  return (true);
}
Ejemplo n.º 28
0
template <typename T> bool
pcl::visualization::ImageViewer::addRectangle (
    const typename pcl::PointCloud<T>::ConstPtr &image,
    const pcl::PointCloud<T> &mask, 
    const std::string &layer_id, double opacity)
{
  // We assume that the data passed into image is organized, otherwise this doesn't make sense
  if (!image->isOrganized ())
    return (false);

  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
  if (am_it == layer_map_.end ())
  {
    PCL_DEBUG ("[pcl::visualization::ImageViewer::addRectangle] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
    am_it = createLayer (layer_id, image_viewer_->GetRenderWindow ()->GetSize ()[0] - 1, image_viewer_->GetRenderWindow ()->GetSize ()[1] - 1, opacity, true);
  }

  am_it->canvas->SetDrawColor (0.0, 255.0, 0.0, opacity * 255.0);

  // Construct a search object to get the camera parameters
  pcl::search::OrganizedNeighbor<T> search;
  search.setInputCloud (image);
  std::vector<pcl::PointXY> pp_2d (mask.points.size ());
  for (size_t i = 0; i < mask.points.size (); ++i)
    search.projectPoint (mask.points[i], pp_2d[i]);

  pcl::PointXY min_pt_2d, max_pt_2d;
  min_pt_2d.x = min_pt_2d.y = std::numeric_limits<float>::max ();
  max_pt_2d.x = max_pt_2d.y = std::numeric_limits<float>::min ();
  // Search for the two extrema
  for (size_t i = 0; i < pp_2d.size (); ++i)
  {
    if (pp_2d[i].x < min_pt_2d.x) min_pt_2d.x = pp_2d[i].x;
    if (pp_2d[i].y < min_pt_2d.y) min_pt_2d.y = pp_2d[i].y;
    if (pp_2d[i].x > max_pt_2d.x) max_pt_2d.x = pp_2d[i].x;
    if (pp_2d[i].y > max_pt_2d.y) max_pt_2d.y = pp_2d[i].y;
  }
  min_pt_2d.y = float (image->height) - min_pt_2d.y;
  max_pt_2d.y = float (image->height) - max_pt_2d.y;

  am_it->canvas->DrawSegment (int (min_pt_2d.x), int (min_pt_2d.y), int (min_pt_2d.x), int (max_pt_2d.y));
  am_it->canvas->DrawSegment (int (min_pt_2d.x), int (max_pt_2d.y), int (max_pt_2d.x), int (max_pt_2d.y));
  am_it->canvas->DrawSegment (int (max_pt_2d.x), int (max_pt_2d.y), int (max_pt_2d.x), int (min_pt_2d.y));
  am_it->canvas->DrawSegment (int (max_pt_2d.x), int (min_pt_2d.y), int (min_pt_2d.x), int (min_pt_2d.y));


  return (true);
}
Ejemplo n.º 29
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();
}
Ejemplo n.º 30
0
template <typename T> bool
pcl::visualization::ImageViewer::addMask (
    const typename pcl::PointCloud<T>::ConstPtr &image,
    const pcl::PointCloud<T> &mask, 
    double r, double g, double b,
    const std::string &layer_id, double opacity)
{
  // We assume that the data passed into image is organized, otherwise this doesn't make sense
  if (!image->isOrganized ())
    return (false);

  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
  if (am_it == layer_map_.end ())
  {
    PCL_DEBUG ("[pcl::visualization::ImageViewer::addMask] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
    am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, false);
  }

  // Construct a search object to get the camera parameters
  pcl::search::OrganizedNeighbor<T> search;
  search.setInputCloud (image);
  std::vector<float> xy;
  xy.reserve (mask.size () * 2);
  for (size_t i = 0; i < mask.size (); ++i)
  {
    pcl::PointXY p_projected;
    search.projectPoint (mask[i], p_projected);

    xy.push_back (p_projected.x);
    xy.push_back (static_cast<float> (image->height) - p_projected.y);
  }

  vtkSmartPointer<context_items::Points> points = vtkSmartPointer<context_items::Points>::New ();
  points->setColors (static_cast<unsigned char> (r*255.0), 
                     static_cast<unsigned char> (g*255.0), 
                     static_cast<unsigned char> (b*255.0));
  points->setOpacity (opacity);
  points->set (xy);
  am_it->actor->GetScene ()->AddItem (points);
  return (true);
}