C++ (Cpp) pcl_source_ptr Exemples

Exemple #1

Fichier : segment_plane.cpp Projet : AriYu/ros_practice

void SegmentPlane::detectPlaneCallback(const sensor_msgs::PointCloud2::ConstPtr& source_pc)
{
  pcl::PointCloud<pcl::PointXYZ> pcl_source;
  pcl::fromROSMsg(*source_pc, pcl_source);
  pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_source_ptr(new pcl::PointCloud<pcl::PointXYZ>(pcl_source));
  pcl::PointCloud<pcl::PointXYZ>::Ptr resized_pc_ptr (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>);

  pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
  approximate_voxel_filter.setLeafSize (leaf_size_x_, leaf_size_y_, leaf_size_z_);
  approximate_voxel_filter.setInputCloud (pcl_source_ptr);
  approximate_voxel_filter.filter (*resized_pc_ptr);

  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients(true);
  // Mandatory
  seg.setModelType(pcl::SACMODEL_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setDistanceThreshold(0.1);

  // Create the filtering object
  pcl::ExtractIndices<pcl::PointXYZ> extract;
  
  seg.setInputCloud(resized_pc_ptr);
  seg.segment(*inliers, *coefficients);

  if(inliers->indices.size() == 0)
    {
      ROS_WARN("Could not estimate a planar model for the given dataset");
      return;
    }

  extract.setInputCloud(resized_pc_ptr);
  extract.setIndices(inliers);
  extract.setNegative(false);
  extract.filter(*cloud_p);
  
  sensor_msgs::PointCloud2 inliers_pc;
  pcl::toROSMsg(*cloud_p, inliers_pc);
  inliers_pc.header.stamp = ros::Time::now();
  inliers_pc.header.frame_id = frame_id_;
  inliers_pc_pub_.publish(inliers_pc);
}

Exemple #2

Fichier : kinect_downsampler.cpp Projet : Ry0/multi_kinect

void KinectV2Downsampler::resizeCallback(const sensor_msgs::PointCloud2::ConstPtr& source_pc)
{
  pcl::PointCloud<pcl::PointXYZ> pcl_source;
  pcl::fromROSMsg(*source_pc, pcl_source);
  pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_source_ptr(new pcl::PointCloud<pcl::PointXYZ>(pcl_source));
  pcl::PointCloud<pcl::PointXYZ>::Ptr resized_pc_ptr(new pcl::PointCloud<pcl::PointXYZ>);

  pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
  approximate_voxel_filter.setLeafSize(leaf_size_x_, leaf_size_y_, leaf_size_z_);
  approximate_voxel_filter.setInputCloud(pcl_source_ptr);
  approximate_voxel_filter.filter(*resized_pc_ptr);

  sensor_msgs::PointCloud2 resized_pc;
  pcl::toROSMsg(*resized_pc_ptr, resized_pc);
  resized_pc.header.stamp = ros::Time::now();
  resized_pc.header.frame_id = frame_id_;
  resized_pc_pub_.publish(resized_pc);
}