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

double
dist_nn_3d_model(pcl::PointCloud<pcl::PointXYZ>::Ptr model_cloud, pcl::PointCloud<pcl::PointXYZ> obj_cloud,
		carmen_point_t particle_pose, carmen_vector_3D_t car_global_position)
{
	double theta = -particle_pose.theta;
	pcl::PointXYZ point;
	double dist = 0.0;
	int k = 1; //k-nearest neighbor

	// kd-tree object
	pcl::search::KdTree<pcl::PointXYZ> kdtree;
	kdtree.setInputCloud(model_cloud);
	// This vector will store the output neighbors.
	std::vector<int> pointIndices(k);
	// This vector will store their squared distances to the search point.
	std::vector<float> squaredDistances(k);
	for (pcl::PointCloud<pcl::PointXYZ>::iterator it = obj_cloud.begin(); it != obj_cloud.end(); ++it)
	{
		// Necessary transformations (translation and rotation):
		float x = car_global_position.x + it->x - particle_pose.x;
		float y = car_global_position.y + it->y - particle_pose.y;
		point.z = it->z;
		point.x = x*cos(theta) - y*sin(theta);
		point.y = x*sin(theta) + y*cos(theta);

		if (kdtree.nearestKSearch(point, k, pointIndices, squaredDistances) > 0)
		{
//			dist += sqrt(double(squaredDistances[0]));
			dist += double(squaredDistances[0]);
		}
	}

	return dist;
}

void InnerModelManager::setPointCloudData(const std::string id, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud)
{
    QString m = QString("setPointCloudData");

    /// Aqui Marco va a mejorar el código :-) felicidad (comprobar que la nube existe)
    IMVPointCloud *pcNode = imv->pointCloudsHash[QString::fromStdString(id)];

    int points = cloud->size();
    pcNode->points->resize(points);
    pcNode->colors->resize(points);
    pcNode->setPointSize(3);
    int i = 0;
    for(pcl::PointCloud<pcl::PointXYZRGBA>::iterator it = cloud->begin(); it != cloud->end(); it++ )
    {
        if (!pcl_isnan(it->x)&&!pcl_isnan(it->y)&&!pcl_isnan(it->z))
        {
            std::cout<<"Adding: "<<it->x<<" "<<it->y<<" "<<it->z<<endl;
            pcNode->points->operator[](i) = QVecToOSGVec(QVec::vec3(it->x, it->y, it->z));
            pcNode->colors->operator[](i) = osg::Vec4f(float(it->r)/255, float(it->g)/255, float(it->b)/255, 1.f);
        }
        i++;
    }
    pcNode->update();
    imv->update();
}

template <typename GeneratorT> int
pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::fill (int width, int height, pcl::PointCloud<pcl::PointXY>& cloud)
{
  if (width < 1)
  {
    PCL_ERROR ("[pcl::common::CloudGenerator] Cloud width must be >= 1\n!");
    return (-1);
  }
  
  if (height < 1)
  {
    PCL_ERROR ("[pcl::common::CloudGenerator] Cloud height must be >= 1\n!");
    return (-1);
  }
  
  if (!cloud.empty ())
    PCL_WARN ("[pcl::common::CloudGenerator] Cloud data will be erased with new data\n!");

  cloud.width = width;
  cloud.height = height;
  cloud.resize (cloud.width * cloud.height);
  cloud.is_dense = true;

  for (pcl::PointCloud<pcl::PointXY>::iterator points_it = cloud.begin ();
       points_it != cloud.end ();
       ++points_it)
  {
    points_it->x = x_generator_.run ();
    points_it->y = y_generator_.run ();
  }
  return (0);
}

void occlude_pcd(pcl::PointCloud<pcl::PointXYZ>::Ptr & cld_ptr,
                 int dim, double threshA, double threshB)
{
    for(pcl::PointCloud<pcl::PointXYZ>::iterator it = cld_ptr->begin();
            it != cld_ptr->end();)
    {
        if((it->data[dim] < threshA) || (it->data[dim] > threshB))
            it = cld_ptr->erase(it);
        else
            ++it;
    }
}

void PointCloudDetection::setPointcloud(pcl::PointCloud<PointRGBT>::Ptr points){
    pointcloud_.clear();
    pcl::PointCloud<PointRGBT>::iterator iter;
    for (iter=points->begin();iter!=points->end();iter++){
        PointRGBT color_pt(iter->r,iter->g,iter->b);
        color_pt.x=iter->x;
        color_pt.y=iter->y;
        color_pt.z=iter->z;
        pointcloud_.push_back(color_pt);
    }
    this->computeNormals();
}

	bool DynModelExporter::getCenterAndScale(Plane<float> &plane, pcl::PointCloud<PointXYZRGB>::Ptr scene_cloud, PointXYZ &center, PointXYZ &scale)
		{
			center.x = 0.0;
			center.y = 0.0;
			center.z = 0.0;

			float size = 0.0;
			PointXYZ min(9999999.0, 9999999.0, 9999999.0);
			PointXYZ max(-9999999.0, -9999999.0, -9999999.0);

			for (pcl::PointCloud<PointXYZRGB>::iterator it = scene_cloud->begin(); it != scene_cloud->end(); ++it)
			{
				 // 1cm TODO - make as param
				 if (plane.distance(cv::Vec3f(it->x, it->y, it->z)) < 0.05)
				 {
					 center.x += it->x;
					 center.y += it->y;
					 center.z += it->z;
					 size += 1.0;

					 if (it->x < min.x) min.x = it->x;
					 if (it->y < min.y) min.y = it->y;
					 if (it->z < min.z) min.z = it->z;

					 if (it->x > max.x) max.x = it->x;
					 if (it->y > max.y) max.y = it->y;
					 if (it->z > max.z) max.z = it->z;
				 }
			}

			//std::cout << std::endl << std::endl << min << " " << max << std::endl << std::endl;
			if (size > 10)
			{
				center.x /= size;
				center.y /= size;
				center.z /= size;
				//center.z = -(center.x*plane.a + center.y*plane.b + plane.d)/plane.c;
			}
			else return false;

			scale.x = max.x - min.x;
			if (scale.x > 3)
				scale.x = 3;
			scale.y = max.y - min.y;
			if (scale.y > 3)
				scale.y = 3;
			scale.z = max.z - min.z;
			if (scale.z > 3)
				scale.z = 3;

			return true;
		}

  bool OverRelation::solve(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &source,
                           pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &target,
                           pcl::PointCloud<pcl::PointXYZRGBA>::VectorType &source_voxel_vector,
                           pcl::PointCloud<pcl::PointXYZRGBA>::VectorType &target_voxel_vector){

    Vector4f source_center;
    pcl::compute3DCentroid(*source, source_center);
    Vector4f target_center;
    pcl::compute3DCentroid(*target, target_center);
    Vector3f target_to_source((source_center[0]-target_center[0]), (source_center[1]-target_center[1]), (source_center[2]-target_center[2]));
    target_to_source = target_to_source.normalized();

    //check is it about above?
    // float dot_result = target_to_source.dot(Vector3f(1,0,0));
    // ROS_INFO("DOT_RESULT x: %f", dot_result);
    // dot_result = target_to_source.dot(Vector3f(0,1,0));
    // ROS_INFO("DOT_RESULT y: %f", dot_result);

    float dot_result = target_to_source.dot(Vector3f(0,-1,0));
    // dot_result = target_to_source.dot(Vector3f(0,0,1));
    if( dot_result <   1 - threshold_)
      return false;
    ROS_INFO("DOT_RESULT y: %f", dot_result);


    for(pcl::PointCloud<pcl::PointXYZRGBA>::VectorType::iterator target_it = target_voxel_vector.begin();
        target_it < target_voxel_vector.end();
        target_it++){
      for(pcl::PointCloud<pcl::PointXYZRGBA>::VectorType::iterator source_it = source_voxel_vector.begin();
          source_it < source_voxel_vector.end();
          source_it++){
        if((*target_it).y < (*source_it).y)
          return false;
      }
    }
    return true;
  }

template <typename PointT> void
pcl::LCCPSegmentation<PointT>::relabelCloud (pcl::PointCloud<pcl::PointXYZL> &labeled_cloud_arg)
{
  if (grouping_data_valid_)
  {
    // Relabel all Points in cloud with new labels
    typename pcl::PointCloud<pcl::PointXYZL>::iterator voxel_itr = labeled_cloud_arg.begin ();
    for (; voxel_itr != labeled_cloud_arg.end (); ++voxel_itr)
    {
      voxel_itr->label = sv_label_to_seg_label_map_[voxel_itr->label];
    }
  }
  else
  {
    PCL_WARN ("[pcl::LCCPSegmentation::relabelCloud] WARNING: Call function segment first. Nothing has been done. \n");
  }
}

pcl::PointCloud<PointT>::Ptr removePlane(const pcl::PointCloud<PointT>::Ptr scene)
{
    pcl::ModelCoefficients::Ptr plane_coef(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    // Create the segmentation object
    pcl::SACSegmentation<PointT> seg;
    // Optional
    seg.setOptimizeCoefficients(true);
    // Mandatory
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(0.02);

    seg.setInputCloud(scene);
    seg.segment(*inliers, *plane_coef);
    
    pcl::ProjectInliers<PointT> proj;
    proj.setModelType (pcl::SACMODEL_PLANE);
    proj.setInputCloud (scene);
    proj.setModelCoefficients (plane_coef);

    pcl::PointCloud<PointT>::Ptr scene_projected(new pcl::PointCloud<PointT>());
    proj.filter (*scene_projected);

    pcl::PointCloud<PointT>::iterator it_ori = scene->begin();
    pcl::PointCloud<PointT>::iterator it_proj = scene_projected->begin();
    
    pcl::PointCloud<PointT>::Ptr scene_f(new pcl::PointCloud<PointT>());
    for( int base = 0 ; it_ori < scene->end(), it_proj < scene_projected->end() ; it_ori++, it_proj++, base++ )
    {
        
        float diffx = it_ori->x-it_proj->x;
        float diffy = it_ori->y-it_proj->y;
        float diffz = it_ori->z-it_proj->z;

        if( diffx * it_ori->x + diffy * it_ori->y + diffz * it_ori->z >= 0 )
            continue;
        //distance from the point to the plane
        float dist = sqrt(diffx*diffx + diffy*diffy + diffz*diffz);
        
        if ( dist >= 0.03 )//fabs((*it_ori).x) <= 0.1 && fabs((*it_ori).y) <= 0.1 )
            scene_f->push_back(*it_ori);
    }
    
    return scene_f;
}

  void addSupervoxelConnectionsToViewer(pcl::PointXYZRGBA &supervoxel_center,
                                        pcl::PointCloud< pcl::PointXYZRGBA> &adjacent_supervoxel_centers,
                                        std::string supervoxel_name,
                                        pcl::visualization::PCLVisualizer &viewer)
  {
    vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
    vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New();
    vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New();

    // Setup colors
    unsigned char red[3] = {255, 0, 0};
    unsigned char green[3] = {0, 255, 0};
    unsigned char blue[3] = {0, 0, 255};

    vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
    colors->SetNumberOfComponents(3);
    colors->SetName("Colors");
    colors->InsertNextTupleValue(red);
    colors->InsertNextTupleValue(green);
    colors->InsertNextTupleValue(blue);


    //Iterate through all adjacent points, and add a center point to adjacent point pair
    pcl::PointCloud< pcl::PointXYZRGBA>::iterator adjacent_itr = adjacent_supervoxel_centers.begin();
    for(; adjacent_itr != adjacent_supervoxel_centers.end(); ++adjacent_itr)
    {
      points->InsertNextPoint(supervoxel_center.data);
      points->InsertNextPoint(adjacent_itr->data);
    }
    // Create a polydata to store everything in
    vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
    // Add the points to the dataset
    polyData->SetPoints(points);
    polyLine->GetPointIds()->SetNumberOfIds(points->GetNumberOfPoints());
    for(unsigned int i = 0; i < points->GetNumberOfPoints(); i++)
    {
      polyLine->GetPointIds()->SetId(i, i);
    }
    cells->InsertNextCell(polyLine);
    // Add the lines to the dataset
    polyData->SetLines(cells);
    //    polyData->GetPointData()->SetScalars(colors);
    viewer.addModelFromPolyData(polyData, supervoxel_name);
  }

void Node::crop_cloud(pcl::PointCloud<pcl::PointXYZ> &pcl_cloud, int laser, ros::Time time)
{
//CROP CLOUD
  pcl::PointCloud<pcl::PointXYZ>::iterator i;

  for (i = pcl_cloud.begin(); i != pcl_cloud.end();)
  {

    bool remove_point = 0;

    if (i->z < 0 || i->z > max_z)
    {
      remove_point = 1;
    }

    if (sqrt(pow(i->x,2) + pow(i->y,2)) > max_radius)
    {
      remove_point = 1;
    }

    tf::StampedTransform transform;
    listener_.lookupTransform ("/world", "/laser1", time, transform);

    if (sqrt(pow(transform.getOrigin().x() - i->x,2) + pow(transform.getOrigin().y() - i->y,2)) > 0.25 && laser == 1)
    {
      remove_point = 1;
    }

    if (remove_point == 1)
    {
      i = pcl_cloud.erase(i);
    }
    else
    {
      i++;
    }

  }
//END CROP CLOUD
}

    pcl::PointCloud<pcl::PointXYZ>::Ptr PlanarCutTransformator::transformPc(pcl::PointCloud<pcl::PointXYZ>::Ptr pc) {

        pcl::PointCloud<pcl::PointXYZ> retPc;

        pcl::PointCloud<pcl::PointXYZ>::iterator pointIt = pc->begin();
        pcl::PointCloud<pcl::PointXYZ>::iterator lastIt = pc->end();

        while(pointIt != lastIt) {
            pcl::PointXYZ currentPoint = *pointIt;
            vec r = stdToArmadilloVec(createJointsVector(3, currentPoint.x, currentPoint.y, currentPoint.z));
            vec comp = normalVec.t() * (r - plainOriginVec);
            double coordinate = comp(0);
            if(coordinate >= 0) {
                retPc.push_back(*pointIt);
            } else {
                // point gets kicked out
            }
            ++pointIt;
        }

        return retPc.makeShared();

    }

cos_lib::bounding_box::bounding_box(pcl::PointCloud<point_clstr>::Ptr cloud)
{
    pcl::PointCloud<point_clstr>::iterator cloud_it;
    float xmin=99999,xmax=-99999,ymin=99999,ymax=-99999,zmin=99999,zmax=-99999;
    cloud_it++;
    for(cloud_it=cloud->begin(); cloud_it!=cloud->end(); cloud_it++)
    {
        if((*cloud_it).x < xmin) xmin = (*cloud_it).x;
        if((*cloud_it).x >= xmax) xmax = (*cloud_it).x;
        if((*cloud_it).y < ymin) ymin = (*cloud_it).y;
        if((*cloud_it).y >= ymax) ymax = (*cloud_it).y;
        if((*cloud_it).z < zmin) zmin = (*cloud_it).z;
        if((*cloud_it).z >= zmax) zmax = (*cloud_it).z;
        this->addPointIntoBox(&(*cloud_it));
    }
    this->A = new point_clstr(xmin,ymax,zmin, 0, 255, 0);
    this->B = new point_clstr(xmin,ymax,zmax, 0, 255, 0);
    this->C = new point_clstr(xmax,ymax,zmax, 0, 255, 0);
    this->D = new point_clstr(xmax,ymax,zmin, 0, 255, 0);
    this->E = new point_clstr(xmin,ymin,zmax, 0, 255, 0);
    this->F = new point_clstr(xmax,ymin,zmax, 0, 255, 0);
    this->G = new point_clstr(xmax,ymin,zmin, 0, 255, 0);
    this->H = new point_clstr(xmin,ymin,zmin, 0, 255, 0);
}

void LocalPlanner::filterPointCloud(pcl::PointCloud<pcl::PointXYZ>& complete_cloud)
{
	pcl::PointCloud<pcl::PointXYZ>::iterator pcl_it;
  pcl::PointCloud<pcl::PointXYZ> front_cloud;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  final_cloud.points.clear();

	for (pcl_it = complete_cloud.begin(); pcl_it != complete_cloud.end(); ++pcl_it)
	{
      // Check if the point is invalid
    if (!std::isnan (pcl_it->x) && !std::isnan (pcl_it->y) && !std::isnan (pcl_it->z))
    {
   		if((pcl_it->x)<max_cache.x&&(pcl_it->x)>min_cache.x&&(pcl_it->y)<max_cache.y&&(pcl_it->y)>min_cache.y&&(pcl_it->z)<max_cache.z&&(pcl_it->z)>min_cache.z) 
      {
        octomapCloud.push_back(pcl_it->x, pcl_it->y, pcl_it->z);
      }
      
      if((pcl_it->x)<front.x&&(pcl_it->x)>back.x&&(pcl_it->y)<front.y&&(pcl_it->y)>back.y&&(pcl_it->z)<front.z&&(pcl_it->z)>back.z)
      {
        front_cloud.points.push_back(pcl::PointXYZ(pcl_it->x,pcl_it->y,pcl_it->z));
      }
      
   //   if((pcl_it->x)<max.x&&(pcl_it->x)>min.x&&(pcl_it->y)<max.y&&(pcl_it->y)>min.y&&(pcl_it->z)<max.z&&(pcl_it->z)>min.z)
   //   {
   //     cloud->points.push_back(pcl::PointXYZ(pcl_it->x,pcl_it->y,pcl_it->z));
    //  }    

    }
  }

  if(front_cloud.points.size()>1)
  {    
  //  octomapCloud.crop(octomap::point3d(min_cache.x,min_cache.y,min_cache.z),octomap::point3d(half_cache.x,half_cache.y,half_cache.z));

    obstacle = true;

    octomap::Pointcloud::iterator oc_it;
		for (oc_it = octomapCloud.begin(); oc_it != octomapCloud.end(); ++oc_it)
		{
      if((oc_it->x())<max.x&&(oc_it->x())>min.x&&(oc_it->y())<max.y&&(oc_it->y())>min.y&&(oc_it->z())<max.z&&(oc_it->z())>min.z)// if((data.x)<max_x&&(data.x)>-min_x&&(data.y)<max_y&&(data.y)>-min_y&&(data.z)<max_z&&(data.z)>-min_z)
      {
        cloud->points.push_back(pcl::PointXYZ(oc_it->x(),oc_it->y(),oc_it->z()));
      }
    }
 
  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
  sor.setInputCloud(cloud);
  sor.setMeanK (5);
  sor.setStddevMulThresh (0.5);
  sor.filter(final_cloud);

  }
  else
    obstacle = false;
   
    ROS_INFO(" Cloud transformed");

    final_cloud.header.stamp =  complete_cloud.header.stamp;
    final_cloud.header.frame_id = complete_cloud.header.frame_id;
    final_cloud.width = final_cloud.points.size();
    final_cloud.height = 1; 
}

static int vscanDetection(int closest_waypoint)
{

    if (_vscan.empty() == true)
        return -1;

    for (int i = closest_waypoint + 1; i < closest_waypoint + _search_distance; i++) {

        if(i > _path_dk.getPathSize() - 1 )
            return -1;

        tf::Vector3 tf_waypoint = _path_dk.transformWaypoint(i);
        //tf::Vector3 tf_waypoint = TransformWaypoint(_transform,_current_path.waypoints[i].pose.pose);
        tf_waypoint.setZ(0);

        //std::cout << "waypoint : "<< tf_waypoint.getX()  << " "<< tf_waypoint.getY() << std::endl;
        int point_count = 0;
        for (pcl::PointCloud<pcl::PointXYZ>::const_iterator item = _vscan.begin(); item != _vscan.end(); item++) {
            if ((item->x == 0 && item->y == 0) || item->z > _detection_height_top || item->z < _detection_height_bottom)
                continue;

            tf::Vector3 point((double) item->x, (double) item->y, 0);
            double dt = tf::tfDistance(point, tf_waypoint);
            if (dt < _detection_range) {
                point_count++;
                //std::cout << "distance :" << dt << std::endl;
                //std::cout << "point : "<< (double) item->x  << " " <<  (double)item->y  <<  " " <<(double) item->z << std::endl;
                //std::cout << "count : "<< point_count << std::endl;
            }

            if (point_count > _threshold_points)
                return i;

        }
    }
    return -1;

    }