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

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
appendToHistory(const LastData & data)
{
  if (history_cloud->size() < max_history_size)
  {
    pcl::PointXYZRGB point;
    history_cloud->push_back(point);
  }

  toPointXYZRGB(history_cloud->points[history_cloud_index], data);
  history_cloud_index = (history_cloud_index + 1) % max_history_size;

}

void complex_reproject_cloud(const cv::Mat& Q, cv::Mat& img_rgb, cv::Mat& img_disparity, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& point_cloud_ptr)
{
    //Get the interesting parameters from Q
    double Q03, Q13, Q23, Q32, Q33;
    Q03 = Q.at<double>(0,3);
    Q13 = Q.at<double>(1,3);
    Q23 = Q.at<double>(2,3);
    Q32 = Q.at<double>(3,2);
    Q33 = Q.at<double>(3,3);

    std::cout << "Q(0,3) = "<< Q03 <<"; Q(1,3) = "<< Q13 <<"; Q(2,3) = "<< Q23 <<"; Q(3,2) = "<< Q32 <<"; Q(3,3) = "<< Q33 <<";" << std::endl;

    double px, py, pz;
    uchar pr, pg, pb;

    for (int i = 0; i < img_rgb.rows; i++)
    {
        uchar* rgb_ptr = img_rgb.ptr<uchar>(i);
        uchar* disp_ptr = img_disparity.ptr<uchar>(i);
        for (int j = 0; j < img_rgb.cols; j++)
        {
            //Get 3D coordinates
            uchar d = disp_ptr[j];
            if ( d == 0 ) continue; //Discard bad pixels
            double pw = -1.0 * static_cast<double>(d) * Q32 + Q33; 
            px = static_cast<double>(j) + Q03;
            py = static_cast<double>(i) + Q13;
            pz = Q23;

            px = px/pw;
            py = py/pw;
            pz = pz/pw;
            //Get RGB info
            pb = rgb_ptr[3*j];
            pg = rgb_ptr[3*j+1];
            pr = rgb_ptr[3*j+2];
            //Insert info into point cloud structure
            pcl::PointXYZRGB point;
            point.x = px;
            point.y = py;
            point.z = pz;
            uint32_t rgb = (static_cast<uint32_t>(pr) << 16 |
                  static_cast<uint32_t>(pg) << 8 | static_cast<uint32_t>(pb));
            point.rgb = *reinterpret_cast<float*>(&rgb);
            point_cloud_ptr->push_back (point);
        }
    }
    point_cloud_ptr->width = (int) point_cloud_ptr->points.size();
    point_cloud_ptr->height = 1;

}

void DownSampler::downSamplePointsDeterministic(
    const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& points,
    const int target_num_points,
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr& down_sampled_points,
    const bool use_ceil)
{
  const size_t num_points = points->size();

  // Check if the points are already sufficiently down-sampled.
  if (target_num_points >= num_points * 0.8){
    *down_sampled_points = *points;
    return;
  }

  // Select every N points to reach the target number of points.
  int everyN = 0;
  if (use_ceil) {
    everyN = ceil(static_cast<double>(num_points) /
                  static_cast<double>(target_num_points));
  } else {
    everyN = static_cast<double>(num_points) /
        static_cast<double>(target_num_points);
  }

  // Allocate space for the new points.
  down_sampled_points->reserve(target_num_points);

  //Just to ensure that we don't end up with 0 points, add 1 point to this
  down_sampled_points->push_back((*points)[0]);

  // Select every N points to reach the target number of points.
  for (size_t i = 1; i < num_points; ++i) {
    if (i % everyN == 0){
      const pcl::PointXYZRGB& pt = (*points)[i];
      down_sampled_points->push_back(pt);
    }
  }
}

void add_velodyne_point_to_pointcloud(
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud,
		unsigned char intensity, carmen_vector_3D_t point) {
	pcl::PointXYZRGB p3D;

	p3D.x = point.x;
	p3D.y = point.y;
	p3D.z = point.z;
	p3D.r = intensity;
	p3D.g = intensity;
	p3D.b = intensity;

	pointcloud->push_back(p3D);
}

void GeneralTransform::Mat2PCD_Trans(cv::Mat& cloud_mat, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
    int size = cloud_mat.rows;
    std::vector<pcl::PointXYZ> points_vec(size);
    cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
    for(int i = 0; i < size; i++)
    {
        pcl::PointXYZ point;
        point.x = cloud_mat.at<double>(i, 0);
        point.y = cloud_mat.at<double>(i, 1);
        point.z = cloud_mat.at<double>(i, 2);
        cloud->push_back(point);
    }   
}

void filterCloudByHeight(
  const pcl::PointCloud<pcl::PointXYZRGBNormal>& cloud_in,
  pcl::PointCloud<pcl::PointXYZRGBNormal>& cloud_out,
  double min_z,
  double max_z)
{
  for (unsigned int i = 0; i < cloud_in.points.size(); ++i)
  {
    const pcl::PointXYZRGBNormal& p = cloud_in.points[i]; 
    
    if (p.z >= min_z && p.z < max_z)
      cloud_out.push_back(p); 
  }
}

bool getFramePointCloud(int frameID, pcl::PointCloud<pcl::PointXYZRGB> &pointCloud)
{
	FrameData frameData;
	if (!frameData.loadImageRGBD(frameID)) {
		pointCloud.points.clear();
		return false;
	}

	// allocate the point cloud buffer
	pointCloud.width = NBPIXELS_WIDTH;
	pointCloud.height = NBPIXELS_HEIGHT;
	pointCloud.points.clear();
	pointCloud.points.reserve(NBPIXELS_WIDTH*NBPIXELS_HEIGHT);	// memory preallocation
	//pointCloud.points.resize(NBPIXELS_WIDTH*NBPIXELS_HEIGHT);

	//printf("Generating point cloud frame %d\n", frameID);

	float focalInv = 0.001 / Config::_FocalLength;
	unsigned int rgb;
	int depth_index = 0;
	IplImage *img = frameData.getImage();
	for (int ind_y =0; ind_y < NBPIXELS_HEIGHT; ind_y++)
	{
		for (int ind_x=0; ind_x < NBPIXELS_WIDTH; ind_x++, depth_index++)
		{
			//pcl::PointXYZRGB& pt = pointCloud(ind_x,ind_y);
			TDepthPixel depth = frameData.getDepthData()[depth_index];
			if (depth != 0)
			{
				pcl::PointXYZRGB pt;

				// locate point in meters
				pt.z = (ind_x - NBPIXELS_X_HALF) * depth * focalInv;
				pt.y = (NBPIXELS_Y_HALF - ind_y) * depth * focalInv;
				pt.x = depth * 0.001 ; // depth values are given in mm

				// reinterpret color bytes
				unsigned char b = ((uchar *)(img->imageData + ind_y*img->widthStep))[ind_x*img->nChannels + 0];
				unsigned char g = ((uchar *)(img->imageData + ind_y*img->widthStep))[ind_x*img->nChannels + 1];
				unsigned char r = ((uchar *)(img->imageData + ind_y*img->widthStep))[ind_x*img->nChannels + 2];
				rgb = (((unsigned int)r)<<16) | (((unsigned int)g)<<8) | ((unsigned int)b);
				pt.rgb = *reinterpret_cast<float*>(&rgb);
				pointCloud.push_back(pt);
			}
		}
	}

	return true;
}

void PopulatePCLPointCloud(const vector<Point3d>& pointcloud, 
						   const std::vector<cv::Vec3b>& pointcloud_RGB,
						   const Mat& img_1_orig, 
						   const Mat& img_2_orig,
						   const vector<KeyPoint>& correspImg1Pt)
	//Populate point cloud
{
	cout << "Creating point cloud...";
	double t = getTickCount();
	Mat_<Vec3b> img1_v3b,img2_v3b;
	if (!img_1_orig.empty() && !img_2_orig.empty()) {
		img1_v3b = Mat_<Vec3b>(img_1_orig);
		img2_v3b = Mat_<Vec3b>(img_2_orig);
	}
	for (unsigned int i=0; i<pointcloud.size(); i++) {
		Vec3b rgbv(255,255,255);
		if(!img_1_orig.empty()) {
			Point p = correspImg1Pt[i].pt;
			//		Point p1 = pt_set2[i];
			rgbv = img1_v3b(p.y,p.x); //(img1_v3b(p.y,p.x) + img2_v3b(p1.y,p1.x)) * 0.5;
		} else if (pointcloud_RGB.size()>0) {
			rgbv = pointcloud_RGB[i];
		}

		
		if (pointcloud[i].x != pointcloud[i].x || isnan(pointcloud[i].x) ||
			pointcloud[i].y != pointcloud[i].y || isnan(pointcloud[i].y) || 
			pointcloud[i].z != pointcloud[i].z || isnan(pointcloud[i].z) ||
			fabsf(pointcloud[i].x) > 10.0 || 
			fabsf(pointcloud[i].y) > 10.0 || 
			fabsf(pointcloud[i].z) > 10.0) {
			continue;
		}
		
		pcl::PointXYZRGB pclp;
		pclp.x = pointcloud[i].x;
		pclp.y = pointcloud[i].y;
		pclp.z = pointcloud[i].z;
		uint32_t rgb = ((uint32_t)rgbv[2] << 16 | (uint32_t)rgbv[1] << 8 | (uint32_t)rgbv[0]);
		pclp.rgb = *reinterpret_cast<float*>(&rgb);
		cloud->push_back(pclp);
	}
	cloud->width = (uint32_t) cloud->points.size();
	cloud->height = 1;
	t = ((double)getTickCount() - t)/getTickFrequency();
	cout << "Done. (" << t <<"s)"<< endl;
	pcl::PLYWriter pw;
	pw.write("pointcloud.ply",*cloud);
}

	// Methods
	// Return a first list of points of objects which has a proper size
	void findCandidates(const kut_ugv_sensor_fusion::lidar_object_listConstPtr& object_list, pcl::PointCloud<pcl::PointXY> &current_2d_scan)
	{
		current_2d_scan.clear();
		for(unsigned int i = 0; i < object_list->object.size(); i++)
		{
			// Check the size of the object
			if( object_list->object[i].width > OBJECT_MIN_WIDTH && object_list->object[i].height > OBJECT_MIN_HEIGHT
					&& object_list->object[i].width < OBJECT_MAX_WIDTH && object_list->object[i].height < OBJECT_MAX_HEIGHT)
			{
				pcl::PointXY p;
				p.x = object_list->object[i].centroid.x;
				p.y = object_list->object[i].centroid.y;
				current_2d_scan.push_back(p);
			}
		}
	}

bool insertPoints(const osg::Geometry* geometry, pcl::PointCloud<PointT>& pointcloud) {
	const osg::Vec3Array* vertex_points = (osg::Vec3Array*)geometry->getVertexArray();
	for (osg::Vec3Array::size_type i = 0; i < vertex_points->size(); ++i) {
		PointT point;
		point.x = (*vertex_points)[i][0];
		point.y = (*vertex_points)[i][1];
		point.z = (*vertex_points)[i][2];
		pointcloud.push_back(point);
	}

	pointcloud.is_dense = false;
	pointcloud.width = pointcloud.size();
	pointcloud.height = 1;

	return !pointcloud.empty();
}

 void addPhysicalPoint()
 {
   geometry_msgs::PointStamped pt_out;
   
   try
   {
     tf_listener_.transformPoint(fixed_frame, gripper_tip, pt_out);
   }
   catch (tf::TransformException& ex)
   {
     ROS_WARN("[calibrate] TF exception:\n%s", ex.what());
     return;
   }
   
   physical_points_.push_back(pcl::PointXYZ(pt_out.point.x, pt_out.point.y, pt_out.point.z));
 }

void addToCloud(int lin_idx, pcl::PointCloud<PointXYZGD>::Ptr outcloud, int groundAdj = 0) 
{
    int num_bin_pts = ptBins[lin_idx].size();
    if(num_bin_pts < NONDRIVE_POINTS_THRESH) {
        return;
    }
    for(int k=0;k<num_bin_pts;k++) {
         PointXYZGD pt;
       	 pt.x = ptBins[lin_idx][k].x;
         pt.y = ptBins[lin_idx][k].y;             
       	 pt.z = ptBins[lin_idx][k].z;
       	 pt.ground_adj = groundAdj;
         pt.drivable = ptBins[lin_idx][k].drivable;
         outcloud->push_back(pt); //add the point to outcloud
    }
}

void TestClass::makePointsForTest(pcl::PointCloud<pcl::PointXYZI>::Ptr in_pcl_pc_ptr)
{
  pcl::PointXYZI point;
  point.x = 12.9892;
  point.y = -9.98058;
  point.z = 0;
  point.intensity = 4;
  in_pcl_pc_ptr->push_back(point);
  point.x = 11.8697;
  point.y = -11.123;
  point.z = -0.189377;
  point.intensity = 35;
  in_pcl_pc_ptr->push_back(point);
  point.x = 12.489;
  point.y = -9.59703;
  point.z = -2.15565;
  point.intensity = 11;
  in_pcl_pc_ptr->push_back(point);
  point.x = 12.9084;
  point.y = -10.9626;
  point.z = -2.15565;
  point.intensity = 11;
  in_pcl_pc_ptr->push_back(point);
  point.x = 13.8676;
  point.y = -9.61668;
  point.z = 0.0980819;
  point.intensity = 14;
  in_pcl_pc_ptr->push_back(point);
  point.x = 13.5673;
  point.y = -12.9834;
  point.z = 0.21862;
  point.intensity = 1;
  in_pcl_pc_ptr->push_back(point);
  point.x = 13.8213;
  point.y = -10.8529;
  point.z = -1.22883;
  point.intensity = 19;
  in_pcl_pc_ptr->push_back(point);
  point.x = 11.8957;
  point.y = -10.3189;
  point.z = -1.28556;
  point.intensity = 13;
  in_pcl_pc_ptr->push_back(point);
}

// remove table
void remove_table(pcl::PointCloud<pcl::PointXYZRGB>& cloud, pcl::PointCloud<pcl::PointXYZRGB>& rmc)
{
    for(int i=0;i<cloud.points.size();i++){
        if(cloud.points[i].z <= (m_size.max_z + 0.01)) continue;
        int X = (100/VOXEL_SIZE)*(cloud.points[i].x-m_size.min_x);
        int Y = (100/VOXEL_SIZE)*(cloud.points[i].y-m_size.min_y);
        int Z = (100/VOXEL_SIZE)*(cloud.points[i].z-m_size.min_z);

        if(((X < 0) || (SIZE_X <= X)) || ((Y < 0) || (SIZE_Y <= Y)) || ((Z < 2.0) || (SIZE_Z <= Z)))
            continue;

        if(!(T_voxel[X][Y][Z]))
            rmc.push_back(cloud.points[i]);
    }

    return;
}