C++ pcl::PointCloud类代码示例

template <typename PointT> void PCL2P3d(const pcl::PointCloud<PointT> &cloud,vector<cv::Point3f>& p3ds){

	int s = cloud.size();
	p3ds.resize(s);
	for(int i=0;i<s;++i){
		p3ds[i].x = cloud[i].x;
		p3ds[i].y = cloud[i].y;
		p3ds[i].z = cloud[i].z;
	}

}

int removeOutliers(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in){
	std::cout << "*** Removing outliers from cloud..***" << std::endl;
	int numberPoints = cloud_in->size();
	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
	sor.setInputCloud(cloud_in);
	sor.setMeanK (50);
	sor.setStddevMulThresh(1.0);
	sor.filter(*cloud_in);
	std::cout << "*** removeOutliers complete*** \nTotal outliers removed: " << numberPoints- cloud_in->size() << std::endl;

}

 CalibrateKinectCheckerboard()
   : nh_("~"), it_(nh_), calibrated(false)
 {
   // Load parameters from the server.
   nh_.param<std::string>("fixed_frame", fixed_frame, "/base_link");
   nh_.param<std::string>("camera_frame", camera_frame, "/camera_link");
   nh_.param<std::string>("target_frame", target_frame, "/calibration_pattern");
   nh_.param<std::string>("tip_frame", tip_frame, "/gripper_link");
   
   nh_.param<int>("checkerboard_width", checkerboard_width, 6);
   nh_.param<int>("checkerboard_height", checkerboard_height, 7);
   nh_.param<double>("checkerboard_grid", checkerboard_grid, 0.027);
   
   // Set pattern detector sizes
   pattern_detector_.setPattern(cv::Size(checkerboard_width, checkerboard_height), checkerboard_grid, CHESSBOARD);
   
   transform_.translation().setZero();
   transform_.matrix().topLeftCorner<3, 3>() = Quaternionf().setIdentity().toRotationMatrix();
   
   // Create subscriptions
   info_sub_ = nh_.subscribe("/camera/rgb/camera_info", 1, &CalibrateKinectCheckerboard::infoCallback, this);
   
   // Also publishers
   pub_ = it_.advertise("calibration_pattern_out", 1);
   detector_pub_ = nh_.advertise<pcl::PointCloud<pcl::PointXYZ> >("detector_cloud", 1);
   physical_pub_ = nh_.advertise<pcl::PointCloud<pcl::PointXYZ> >("physical_points_cloud", 1);
   
   // Create ideal points
   ideal_points_.push_back( pcl::PointXYZ(0, 0, 0) );
   ideal_points_.push_back( pcl::PointXYZ((checkerboard_width-1)*checkerboard_grid, 0, 0) );
   ideal_points_.push_back( pcl::PointXYZ(0, (checkerboard_height-1)*checkerboard_grid, 0) );
   ideal_points_.push_back( pcl::PointXYZ((checkerboard_width-1)*checkerboard_grid, (checkerboard_height-1)*checkerboard_grid, 0) );
   
   // Create proper gripper tip point
   nh_.param<double>("gripper_tip_x", gripper_tip.point.x, 0.0);
   nh_.param<double>("gripper_tip_y", gripper_tip.point.y, 0.0);
   nh_.param<double>("gripper_tip_z", gripper_tip.point.z, 0.0);
   gripper_tip.header.frame_id = tip_frame;
   
   ROS_INFO("[calibrate] Initialized.");
 }

/* To investigate on if the closest match to the target descriptor is also the previously mathced source */
void experiment_correspondences (pcl::PointCloud<pcl::PFHSignature125>::Ptr &source_descriptors,
                              pcl::PointCloud<pcl::PFHSignature125>::Ptr &target_descriptors,
                              vector<int> &correct){
  vector<int> corr1(source_descriptors->size ());
  vector<int> corr2(target_descriptors->size ());
  correct.resize(source_descriptors->size());
  // Use a KdTree to search for the nearest matches in feature space
  pcl::KdTreeFLANN<pcl::PFHSignature125> descriptor_kdtree;
  descriptor_kdtree.setInputCloud (target_descriptors);

  // Find the index of the best match for each keypoint, and store it in "correspondences_out"
  const int k = 1;
  std::vector<int> k_indices (k);
  std::vector<float> k_squared_distances (k);
  for (size_t i = 0; i < source_descriptors->size (); ++i)
  {
    descriptor_kdtree.nearestKSearch (*source_descriptors, i, k, k_indices, k_squared_distances);
    corr1[i] = k_indices[0];
  }

  // Use a KdTree to search for the nearest matches in feature space
  pcl::KdTreeFLANN<pcl::PFHSignature125> descriptor_kdtree2;
  descriptor_kdtree2.setInputCloud (source_descriptors);

  // Find the index of the best match for each keypoint, and store it in "correspondences_out"
  std::vector<int> k_indices2 (k);
  std::vector<float> k_squared_distances2 (k);
  for (size_t i = 0; i < target_descriptors->size (); ++i)
  {
    descriptor_kdtree2.nearestKSearch (*target_descriptors, i, k, k_indices2, k_squared_distances2);
    corr2[i] = k_indices2[0];
  }
  int count = 0;
  for(size_t i=0; i<source_descriptors->points.size(); i++){
    if(abs(corr1[corr2[i]])!=i){ count++; correct[i]=0;}
    else{
      correct[i]=1;
    }
  }
  cout<<"Not matched correspondences : "<<count<<endl;
}

void mpcl_compute_normals(pcl::PointCloud<pcl::PointXYZ> &cloud,
                          int ksearch,
                          pcl::PointCloud<pcl::Normal> &out)
{
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;

    ne.setSearchMethod (tree);
    ne.setInputCloud (cloud.makeShared());
    ne.setKSearch (ksearch);
    ne.compute (out);
}

bool StereoSensorProcessor::cleanPointCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud)
{
  pcl::PointCloud<pcl::PointXYZRGB> tempPointCloud;

  originalWidth_ = pointCloud->width;
  pcl::removeNaNFromPointCloud(*pointCloud, tempPointCloud, indices_);
  tempPointCloud.is_dense = true;
  pointCloud->swap(tempPointCloud);

  ROS_DEBUG("ElevationMap: cleanPointCloud() reduced point cloud to %i points.", static_cast<int>(pointCloud->size()));
  return true;
}

    pcl::PointCloud<PointT>::Ptr removeWallsCloud(pcl::PointCloud<PointT>::Ptr cloud_seg)
    {
        pcl::PointCloud<PointT>::Ptr cloud_plane (new pcl::PointCloud<PointT>);
        pcl::ModelCoefficients::Ptr coeff (new pcl::ModelCoefficients);
        pcl::PointIndices::Ptr inliers (new pcl::PointIndices);

        pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
        pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
        pcl::NormalEstimation<PointT, pcl::Normal> ne;

        pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
        pcl::ExtractIndices<PointT> extract;

        // Estimate point normals
        ne.setSearchMethod (tree);
        ne.setKSearch (50);

        seg.setOptimizeCoefficients (true);
        seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
        seg.setMethodType (pcl::SAC_RANSAC);
        seg.setDistanceThreshold (seg_distance);
        seg.setNormalDistanceWeight (normal_distance_weight);
        seg.setMaxIterations (1000);

        int i = 0, nr_points = (int) cloud_seg->points.size ();
        // While 20% of the original cloud is still there
        while (cloud_seg->points.size () > seg_percentage * nr_points && i < 10 && cloud_seg->points.size() > 0)
        {
            //seg.setInputCloud (cloud);
            ne.setInputCloud (cloud_seg);
            ne.compute (*cloud_normals);
            //seg.setInputCloud (cloud);
            seg.setInputCloud (cloud_seg);
            seg.setInputNormals (cloud_normals);
            seg.segment (*inliers, *coeff);
            if (inliers->indices.size () == 0)
            {
                break;
            }
            if(inliers->indices.size() < nr_points/20 || inliers->indices.size() < 10){
                i++;
                continue;
            }
            // Extract the planar inliers from the input cloud
            extract.setInputCloud (cloud_seg);
            extract.setIndices (inliers);
            extract.setNegative (true);
            extract.filter (*cloud_plane);
            cloud_seg.swap (cloud_plane);
            i++;
        }
        return cloud_seg;
    }

int icp_alignment(pcl::PointCloud<pcl::PointXYZ>::ConstPtr prev_cloud,
		pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud_in,
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out) {
	std::cout << "Performing ICP alignment..." << std::endl;
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	std::cout << "prev_cloud size = " << prev_cloud->size() << std::endl;
	icp.setInputSource(prev_cloud);
	std::cout << "cloud_in size = " << cloud_in->size() << std::endl;
	icp.setInputTarget(cloud_in);
	icp.align(*cloud_out);
	std::cout << "cloud_out size = " << cloud_out->size() << std::endl;
	if (icp.hasConverged()) {
		std::cout << "has converged:" << icp.hasConverged() << " score: "
				<< icp.getFitnessScore() << std::endl;
		std::cout << icp.getFinalTransformation() << std::endl;
		return 0;
	} else {
		PCL_ERROR("\nERROR: ICP has not converged. \n");
		return 1;
	}
}

int Conversion::convert(const Eigen::MatrixXf & matrix, pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud){
 //cloud->empty();
 
 for (int i=0; i<matrix.rows();i++){
            pcl::PointXYZ basic_point;
            basic_point.x = matrix(i,0);
            basic_point.y = matrix(i,1);
            basic_point.z = matrix(i,2);
            cloud->push_back(basic_point);
      }
 return 1;
}

void CloudNoizeFiltration(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &filtered_cloud){
	// Create the filtering object
	pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud_tmp(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
	sor.setInputCloud(cloud);

	sor.setMeanK(10);
	sor.setStddevMulThresh(1.0);
	sor.setNegative(false);
	sor.filter(*filtered_cloud_tmp);
	filtered_cloud.swap(filtered_cloud_tmp);
}

 void
 SegmenterLight::computeNormals (pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud_in,
                            pcl::PointCloud<pcl::Normal>::Ptr &normals_out)
 {
   normals_out.reset (new pcl::PointCloud<pcl::Normal>);
   surface::ZAdaptiveNormals::Parameter za_param;
   za_param.adaptive = true;
   surface::ZAdaptiveNormals nor (za_param);
   nor.setInputCloud (cloud_in);
   nor.compute ();
   nor.getNormals (normals_out);
 }

void RunVisualization(const vector<cv::Point3d>& pointcloud,
					  const std::vector<cv::Vec3b>& pointcloud_RGB,
					  const Mat& img_1_orig, 
					  const Mat& img_2_orig,
					  const vector<KeyPoint>& correspImg1Pt) {
	cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
	orig_cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
	//  pcl::io::loadPCDFile ("output.pcd", *cloud);
    
	PopulatePCLPointCloud(pointcloud,pointcloud_RGB,img_1_orig,img_2_orig,correspImg1Pt);
//	SORFilter();
	copyPointCloud(*cloud,*orig_cloud);
//	FindNormalsMLS();
//	FindFloorPlaneRANSAC();
	
	//	pcl::PointCloud<pcl::PointXYZRGB>::Ptr final (new pcl::PointCloud<pcl::PointXYZRGB>);
	//	pcl::copyPointCloud<pcl::PointXYZRGB>(*cloud, inliers, *final);
	
    pcl::visualization::CloudViewer viewer("Cloud Viewer");
    
    //blocks until the cloud is actually rendered
    viewer.showCloud(orig_cloud,"orig");
	//	viewer.showCloud(final);
    
    //use the following functions to get access to the underlying more advanced/powerful
    //PCLVisualizer
    
    //This will only get called once
    viewer.runOnVisualizationThreadOnce (viewerOneOff);
    
    //This will get called once per visualization iteration
	viewer.runOnVisualizationThread (viewerThread);
    while (!viewer.wasStopped ())
    {
		//you can also do cool processing here
		//FIXME: Note that this is running in a separate thread from viewerPsycho
		//and you should guard against race conditions yourself...
		//		user_data++;
    }
}	

  void
  FeatureFactory::extractSurfaceFeatures (pcl::PointCloud<pcl::Normal>::Ptr pc2_normals, pcl::PointCloud<
      feature_factory::FeaturePoint<SURFACE_FEATURES_HISTOGRAM> > &surface_zen_features, pcl::PointCloud<
      feature_factory::FeaturePoint<SURFACE_FEATURES_HISTOGRAM> > &surface_azi_features)
  {
    cv::Mat normals_zen = cv::Mat (RANGE_IMAGE_HEIGHT, RANGE_IMAGE_WIDTH, CV_32F);
    cv::Mat normals_azi = cv::Mat (RANGE_IMAGE_HEIGHT, RANGE_IMAGE_WIDTH, CV_32F);

    for (uint16_t ri = 0; ri < RANGE_IMAGE_HEIGHT; ri++)
      for (uint16_t ci = 0; ci < RANGE_IMAGE_WIDTH; ci++)
      {
        double n_x = pc2_normals->points[ri * RANGE_IMAGE_WIDTH + ci].normal_x;
        double n_y = pc2_normals->points[ri * RANGE_IMAGE_WIDTH + ci].normal_y;
        double n_z = pc2_normals->points[ri * RANGE_IMAGE_WIDTH + ci].normal_z;

        //always return [0, PI]
        normals_zen.at<float> (ri, ci) = atan2 (sqrt (n_x * n_x + n_y * n_y), n_z);

        //may return [-PI, PI]
        normals_azi.at<float> (ri, ci) = atan2 (n_y, n_x);
        if (normals_azi.at<float> (ri, ci) < 0)
          normals_azi.at<float> (ri, ci) += 2 * PI;
      }

    const uint16_t n_grids_per_row = RANGE_IMAGE_WIDTH / grid_edge_size;
    const uint16_t n_grids_per_col = RANGE_IMAGE_HEIGHT / grid_edge_size;
    const uint16_t n_grids = n_grids_per_col * n_grids_per_row;

    surface_azi_features.resize (n_grids);
    surface_zen_features.resize (n_grids);
    for (uint16_t gri = 0; gri < n_grids_per_col; gri++)
      for (uint16_t gci = 0; gci < n_grids_per_row; gci++)
      {
        cv::Rect roi = cv::Rect (gci * grid_edge_size, gri * grid_edge_size, grid_edge_size, grid_edge_size);
        surface_azi_features[gri * n_grids_per_row + gci]
            = feature_factory::FeaturePoint<SURFACE_FEATURES_HISTOGRAM>::calculate (normals_azi, roi, 0, 2 * PI);
        surface_zen_features[gri * n_grids_per_row + gci]
            = feature_factory::FeaturePoint<SURFACE_FEATURES_HISTOGRAM>::calculate (normals_zen, roi, 0, PI);
      }
  }

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 AdjustCloudNormal(pcl::PointCloud<PointT>::Ptr cloud, pcl::PointCloud<NormalT>::Ptr cloud_normals)
{
    pcl::PointCloud<myPointXYZ>::Ptr center(new pcl::PointCloud<myPointXYZ>());
    ComputeCentroid(cloud, center);
    
    myPointXYZ origin = center->at(0);
    std::cerr << origin.x << " " << origin.y << " " << origin.z << std::endl;
    pcl::transformPointCloud(*cloud, *cloud, Eigen::Vector3f(-origin.x, -origin.y, -origin.z), Eigen::Quaternion<float> (1,0,0,0));
    
    int num = cloud->points.size();
    float diffx, diffy, diffz, dist, theta;
    for( int j = 0; j < num ; j++ )
    {
        PointT temp = cloud->at(j);
        NormalT temp_normals = cloud_normals->at(j);
        diffx = temp.x;
        diffy = temp.y;
        diffz = temp.z;
        dist = sqrt( diffx*diffx + diffy*diffy + diffz*diffz );
		
        theta = acos( (diffx*temp_normals.normal_x + diffy*temp_normals.normal_y + diffz*temp_normals.normal_z)/dist );
        if( theta > PI/2)
        {
            cloud_normals->at(j).normal_x = -cloud_normals->at(j).normal_x;
            cloud_normals->at(j).normal_y = -cloud_normals->at(j).normal_y;
            cloud_normals->at(j).normal_z = -cloud_normals->at(j).normal_z;
        }
    }
}