C++ PointCloud::clear方法代码示例

 inline void
 cvToCloud(const cv::Mat_<cv::Point3f>& points3d, pcl::PointCloud<PointT>& cloud, const cv::Mat& mask = cv::Mat())
 {
   cloud.clear();
   cloud.width = points3d.size().width;
   cloud.height = points3d.size().height;
   cv::Mat_<cv::Point3f>::const_iterator point_it = points3d.begin(), point_end = points3d.end();
   const bool has_mask = !mask.empty();
   cv::Mat_<uchar>::const_iterator mask_it;
   if (has_mask)
     mask_it = mask.begin<uchar>();
   for (; point_it != point_end; ++point_it, (has_mask ? ++mask_it : mask_it))
   {
     if (has_mask && !*mask_it)
       continue;
     cv::Point3f p = *point_it;
     if (p.x != p.x && p.y != p.y && p.z != p.z) //throw out NANs
       continue;
     PointT cp;
     cp.x = p.x;
     cp.y = p.y;
     cp.z = p.z;
     cloud.push_back(cp);
   }
 }

void
load_pointcloud_from_file(char *filename, pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud)
{
	int r, g, b;
	long int num_points;
	pcl::PointXYZRGB p3D;

	pointcloud->clear();

	FILE *f = fopen(filename, "r");

	if (f == NULL)
	{
		carmen_warn("Error: The pointcloud '%s' not exists!\n", filename);
		return;
	}

	fscanf(f, "%ld\n", &num_points);

	for (long i = 0; i < num_points; i++)
	{
		fscanf(f, "%f %f %f %d %d %d\n",
			&p3D.x, &p3D.y, &p3D.z,
			&r, &g, &b
		);

		p3D.r = (unsigned char) r;
		p3D.g = (unsigned char) g;
		p3D.b = (unsigned char) b;

		pointcloud->push_back(p3D);
	}

	fclose(f);
}

void depthPointsHandler(const sensor_msgs::PointCloud2ConstPtr& depthPoints2)
{
  frameCount = (frameCount + 1) % 4;
  if (frameCount != 0) {
    return;
  }

  pcl::PointCloud<DepthPoint>::Ptr depthPointsCur = depthPoints[0];
  depthPointsCur->clear();
  pcl::fromROSMsg(*depthPoints2, *depthPointsCur);

  for (int i = 0; i < keyframeNum - 1; i++) {
    depthPoints[i] = depthPoints[i + 1];
    depthPointsTime[i] = depthPointsTime[i + 1];
  }
  depthPoints[keyframeNum - 1] = depthPointsCur;
  depthPointsTime[keyframeNum - 1] = depthPoints2->header.stamp.toSec();

  keyframeCount++;
  if (keyframeCount >= keyframeNum && depthPointsTime[0] >= lastPubTime) {
    depthPointsStacked->clear();
    for (int i = 0; i < keyframeNum; i++) {
      *depthPointsStacked += *depthPoints[i];
    }

    sensor_msgs::PointCloud2 depthPoints3;
    pcl::toROSMsg(*depthPointsStacked, depthPoints3);
    depthPoints3.header.frame_id = "camera";
    depthPoints3.header.stamp = depthPoints2->header.stamp;
    depthPointsPubPointer->publish(depthPoints3);

    lastPubTime = depthPointsTime[keyframeNum - 1];
  }
}

  /**
   * \breif convert an opencv collection of points to a pcl::PoinCloud, your opencv mat should have NAN's for invalid points.
   * @param points3d opencv matrix of nx1 3 channel points
   * @param cloud output cloud
   * @param rgb the rgb, required, will color points
   * @param mask the mask, required, must be same size as rgb
   */
  inline void
  cvToCloudXYZRGB(const cv::Mat_<cv::Point3f>& points3d, pcl::PointCloud<pcl::PointXYZRGB>& cloud, const cv::Mat& rgb,
                  const cv::Mat& mask, bool brg = true)
  {
    cloud.clear();
    cv::Mat_<cv::Point3f>::const_iterator point_it = points3d.begin(), point_end = points3d.end();
    cv::Mat_<cv::Vec3b>::const_iterator rgb_it = rgb.begin<cv::Vec3b>();
    cv::Mat_<uchar>::const_iterator mask_it;
    if(!mask.empty())
      mask_it = mask.begin<uchar>();
    for (; point_it != point_end; ++point_it, ++rgb_it)
    {
      if(!mask.empty())
      {
        ++mask_it;
        if (!*mask_it)
          continue;
      }

      cv::Point3f p = *point_it;
      if (p.x != p.x && p.y != p.y && p.z != p.z) //throw out NANs
        continue;
      pcl::PointXYZRGB cp;
      cp.x = p.x;
      cp.y = p.y;
      cp.z = p.z;
      cp.r = (*rgb_it)[2]; //expecting in BGR format.
      cp.g = (*rgb_it)[1];
      cp.b = (*rgb_it)[0];
      cloud.push_back(cp);
    }
  }

void
convertFreenectFrameToPointCloud(
  libfreenect2::Registration* registration,
  libfreenect2::Frame* undistorted,
  libfreenect2::Frame* registered,
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud)
{
  cloud->clear();
  float x, y, z, rgb;

  for (int c = 0; c < 512; c++)
  {
    for (int r = 0; r < 424; r++)
    {
      registration->getPointXYZRGB(undistorted, registered, r, c, x, y, z, rgb);
      if (std::isnan(x)) continue;
      
      pcl::PointXYZRGB point;
      point.x = x;
      point.y = y;
      point.z = z;
      point.rgb = rgb;
      cloud->points.push_back (point);
    }
  }
}

 /**
  * \breif convert an opencv collection of points to a pcl::PoinCloud, your opencv mat should have NAN's for invalid points.
  * @param points3d opencv matrix of nx1 3 channel points
  * @param cloud output cloud
  * @param rgb the rgb, required, will color points
  * @param mask the mask, required, must be same size as rgb
  */
 inline void
 cvToCloudXYZRGBNormal(const cv::Mat& cvcloud, pcl::PointCloud<pcl::PointXYZRGBNormal>& cloud)
 {
   std::size_t rows = cvcloud.rows;
   std::size_t cols = cvcloud.cols;
   cloud.clear();
 
   for (std::size_t i = 0; i<rows; ++i) {
     
     pcl::PointXYZRGBNormal cp;
     cp.x = cvcloud.at<float>(i,0);
     cp.y = cvcloud.at<float>(i,1);
     cp.z = cvcloud.at<float>(i,2);
     if (cols > 3)
     {
       cp.normal_x = cvcloud.at<float>(i,3);
       cp.normal_y = cvcloud.at<float>(i,4);
       cp.normal_z = cvcloud.at<float>(i,5);
     }
     if (cols > 6)
     {
       cp.r = cvcloud.at<float>(i,6);
       cp.g = cvcloud.at<float>(i,7);
       cp.b = cvcloud.at<float>(i,8);
     }
     cloud.push_back(cp);
   }
 }

void
Triangulation::convertSurface2Vertices (const ON_NurbsSurface &nurbs, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                                        std::vector<pcl::Vertices> &vertices, unsigned resolution)
{
  // copy knots
  if (nurbs.KnotCount (0) <= 1 || nurbs.KnotCount (1) <= 1)
  {
    printf ("[Triangulation::convertSurface2Vertices] Warning: ON knot vector empty.\n");
    return;
  }

  cloud->clear ();
  vertices.clear ();

  double x0 = nurbs.Knot (0, 0);
  double x1 = nurbs.Knot (0, nurbs.KnotCount (0) - 1);
  double w = x1 - x0;
  double y0 = nurbs.Knot (1, 0);
  double y1 = nurbs.Knot (1, nurbs.KnotCount (1) - 1);
  double h = y1 - y0;

  createVertices (cloud, float (x0), float (y0), 0.0f, float (w), float (h), resolution, resolution);
  createIndices (vertices, 0, resolution, resolution);

  for (auto &v : *cloud)
  {
    double point[9];
    nurbs.Evaluate (v.x, v.y, 1, 3, point);

    v.x = static_cast<float> (point[0]);
    v.y = static_cast<float> (point[1]);
    v.z = static_cast<float> (point[2]);
  }
}

template <typename VoxelT, typename WeightT> void
pcl::TSDFVolume<VoxelT, WeightT>::convertToTsdfCloud (pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) const
{
  int sx = header_.resolution(0);
  int sy = header_.resolution(1);
  int sz = header_.resolution(2);

  const int step = 2;
  const int cloud_size = header_.getVolumeSize() / (step*step*step);

  cloud->clear();
  cloud->reserve (std::min (cloud_size/10, 500000));

  int volume_idx = 0, cloud_idx = 0;
//#pragma omp parallel for // if used, increment over idx not possible! use index calculation
  for (int z = 0; z < sz; z+=step)
    for (int y = 0; y < sy; y+=step)
      for (int x = 0; x < sx; x+=step, ++cloud_idx)
      {
        volume_idx = sx*sy*z + sx*y + x;
        // pcl::PointXYZI &point = cloud->points[cloud_idx];

        if (weights_->at(volume_idx) == 0 || volume_->at(volume_idx) > 0.98 )
          continue;

        pcl::PointXYZI point;
        point.x = x; point.y = y; point.z = z;//*64;
        point.intensity = volume_->at(volume_idx);
        cloud->push_back (point);
      }

  // cloud->width = cloud_size;
  // cloud->height = 1;
}

void SNA::cloud_cb(const sensor_msgs::PointCloud2::ConstPtr& cloud)
{
	pcl::PCLPointCloud2 pcl_pc2;
	pcl_conversions::toPCL(*cloud,pcl_pc2);
	pcl::PointCloud<pcl::PointXYZ> buffer_cloud;
	pcl::fromPCLPointCloud2(pcl_pc2,buffer_cloud);
	pcl::PointXYZ temp;
	
	for(size_t i=0;i<buffer_cloud.size();i++)
	{	
		temp.x = buffer_cloud.points[i].x;
		temp.y = buffer_cloud.points[i].y;
		temp.z = buffer_cloud.points[i].z;		
		assembly_.push_back(temp);	
	}
	pcl::toROSMsg(assembly_,output_);	
	if(!is_moving_)
	{			
		assembly_.clear();
	}
	else
	{
		output_.header.frame_id = "/ChestLidar";	
		assembled_cloud_pub_.publish(output_);
		if(dxl_err_ < 0.01){
		remap_ = output_;
		assembled_cloud_pub_.publish(remap_);
		}
	}
}

void laserCloudLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudLast2)
{
  timeLaserCloudLast = laserCloudLast2->header.stamp.toSec();

  laserCloudLast->clear();
  pcl::fromROSMsg(*laserCloudLast2, *laserCloudLast);

  newLaserCloudLast = true;
}

void SimpleCloudGrabber::copyCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &source,
                              pcl::PointCloud<pcl::PointXYZ>::Ptr &dest)
{
    dest->clear();
    for (pcl::PointXYZ pt : source->points)
    {
        dest->points.push_back(pt);
    }
    dest->width = dest->points.size();
    dest->height = 1;
}

bool RegionGrowing::seedRegionGrowing(
    pcl::PointCloud<PointNormalT>::Ptr src_points,
    const PointT seed_point, const PointCloud::Ptr cloud,
    PointNormal::Ptr normals) {
    if (cloud->empty() || normals->size() != cloud->size()) {
       ROS_ERROR("- Region growing failed. Incorrect inputs sizes ");
       return false;
    }
    if (isnan(seed_point.x) || isnan(seed_point.y) || isnan(seed_point.z)) {
       ROS_ERROR("- Seed Point is Nan. Skipping");
       return false;
    }

    this->kdtree_->setInputCloud(cloud);
    
    std::vector<int> neigbor_indices;
    this->getPointNeigbour<int>(neigbor_indices, seed_point, 1);
    int seed_index = neigbor_indices[0];
    
    const int in_dim = static_cast<int>(cloud->size());
    int *labels = reinterpret_cast<int*>(malloc(sizeof(int) * in_dim));
    
#ifdef _OPENMP
#pragma omp parallel for num_threads(this->num_threads_)
#endif
    for (int i = 0; i < in_dim; i++) {
       if (i == seed_index) {
          labels[i] = 1;
       }
       labels[i] = -1;
    }
    this->seedCorrespondingRegion(labels, cloud, normals,
                                  seed_index, seed_index);
    src_points->clear();
    for (int i = 0; i < in_dim; i++) {
       if (labels[i] != -1) {
          PointNormalT pt;
          pt.x = cloud->points[i].x;
          pt.y = cloud->points[i].y;
          pt.z = cloud->points[i].z;
          pt.r = cloud->points[i].r;
          pt.g = cloud->points[i].g;
          pt.b = cloud->points[i].b;
          pt.normal_x = normals->points[i].normal_x;
          pt.normal_y = normals->points[i].normal_y;
          pt.normal_z = normals->points[i].normal_z;
          src_points->push_back(pt);
       }
    }
    free(labels);
    return true;
}

void ldr_to_cloud(const Eigen::MatrixXf& ldr_data, pcl::PointCloud<pcl::PointXYZ>& cloud)
{
    cloud.clear();
    cloud.is_dense = false;
    cloud.points.resize(ldr_data.rows());
    for (int r=0; r < ldr_data.rows(); r++)
    {
        pcl::PointXYZ& p = cloud.at(r);
        p.x = ldr_data(r, 0);
        p.y = ldr_data(r, 1);
        p.z = ldr_data(r, 2);
    }
}

	void computeGradient(const pcl::PointCloud<pcl::PointXY> &current_2d_scan, pcl::PointCloud<pcl::PointXY> &gradient_scan)
	{
		unsigned int n_points = current_2d_scan.size();
		gradient_scan.clear();
		gradient_scan.resize(n_points);

		for(unsigned int i = 0; i < n_points; i++)
		{
			pcl::PointXY grad;
			grad.x = abs( current_2d_scan[i].x -  prev_2d_scan[i].x ); // dx
			grad.y = abs( current_2d_scan[i].y -  prev_2d_scan[i].y ); // dy
			gradient_scan[i] = grad;
		}
	};

int
accumulate_clouds(carmen_velodyne_partial_scan_message *velodyne_message, char *velodyne_storage_dir)
{
	static char cloud_name[1024];
	static int first_iteraction = 1;
	static carmen_pose_3D_t zero_pose;
	static pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_pointcloud;
	static rotation_matrix *r_matrix_car_to_global;
	int current_point_cloud_index;

	carmen_vector_3D_t robot_velocity = {0.0, 0.0, 0.0};
	double pose_timestamp = 0.0;
	double phi = 0.0;

	if (first_iteraction)
	{
		memset(&zero_pose, 0, sizeof(carmen_pose_3D_t));
		source_pointcloud = boost::shared_ptr< pcl::PointCloud<pcl::PointXYZRGB> >(new pcl::PointCloud<pcl::PointXYZRGB>);
		r_matrix_car_to_global = compute_rotation_matrix(r_matrix_car_to_global, zero_pose.orientation);
		first_iteraction = 0;
	}

	accumulate_partial_scan(velodyne_message, &velodyne_params, &velodyne_data);

	if (two_complete_clouds_have_been_acquired())
	{
		current_point_cloud_index = velodyne_data.point_cloud_index;

		velodyne_data.robot_pose[velodyne_data.point_cloud_index] = zero_pose;
		velodyne_data.robot_phi[velodyne_data.point_cloud_index] = phi;
		velodyne_data.robot_velocity[velodyne_data.point_cloud_index] = robot_velocity;
		velodyne_data.robot_timestamp[velodyne_data.point_cloud_index] = pose_timestamp;

		add_velodyne_spherical_points_to_pointcloud(source_pointcloud, &(velodyne_data.points[current_point_cloud_index]), velodyne_data.intensity[current_point_cloud_index], r_matrix_car_to_global,
				&zero_pose, &velodyne_params, &velodyne_data);

		sprintf(cloud_name, "%s/%lf.ply", velodyne_storage_dir, velodyne_message->timestamp);
		save_pointcloud_to_file(cloud_name, source_pointcloud);

		// DEBUG:
//		char cloud_name[1024];
//		sprintf(cloud_name, "%s/CLOUDS-%lf.ply", velodyne_storage_dir, velodyne_message->timestamp);
//		pcl::io::savePLYFile(cloud_name, *source_pointcloud);

		source_pointcloud->clear();
		return 1;
	}

	return 0;
}