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

static void Callback(const sensor_msgs::PointCloud2ConstPtr& msg)
{

    pcl::fromROSMsg(*msg, _vscan);
    //  int i = 0;
    for (pcl::PointCloud<pcl::PointXYZ>::const_iterator item = _vscan.begin(); item != _vscan.end(); item++) {
        if ((item->x == 0 && item->y == 0))
            continue;
        // if (i < _loop_limit) {
            // std::cout << "vscan_points : ( " << item->x << " , " << item->y << " , " << item->z << " )" << std::endl;

            geometry_msgs::Point point;
            point.x = item->x;
            point.y = item->y;
            point.z = item->z;
            _linelist.points.push_back(point);

            //}else{
            // break;
            // }
            //i++;

    }
   // std::cout << "i = " << i << std::endl;
    _pub.publish(_linelist);
    _linelist.points.clear();
}

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
}

void SuperFrameParser::convertImageToPointCloud (const sensor_msgs::ImagePtr& depth_msg, const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud)
{
    cloud->height = depth_msg->height;
    cloud->width = depth_msg->width;
    cloud->resize (cloud->height * cloud->width);
    // Use correct principal point from calibration
    float center_x = depth_info_->K[2]; // c_x
    float center_y = depth_info_->K[5]; // c_y

    // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
    double unit_scaling = 0.001f;
    float constant_x = unit_scaling / depth_info_->K[0]; // f_x
    float constant_y = unit_scaling / depth_info_->K[4]; // f_y
    float bad_point = std::numeric_limits<float>::quiet_NaN ();

    pcl::PointCloud<pcl::PointXYZ>::iterator pt_iter = cloud->begin ();
    const uint16_t* depth_row = reinterpret_cast<const uint16_t*> (&depth_msg->data[0]);
    int row_step = depth_msg->step / sizeof (uint16_t);
    for (int v = 0; v < (int)depth_msg->height; ++v, depth_row += row_step)
    {
        for (int u = 0; u < (int)depth_msg->width; ++u)
        {
            pcl::PointXYZ& pt = *pt_iter++;
            uint16_t depth = depth_row[u];

            // Missing points denoted by NaNs
            if (depth == 0.0f)
            {
                pt.x = pt.y = pt.z = bad_point;
                continue;
            }

            // Fill in XYZ
            pt.x = (u - center_x) * depth * constant_x;
            pt.y = (v - center_y) * depth * constant_y;
            pt.z = depth * 0.001f;
        }
    }
}

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;

    }

    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();

    }